利用虚幻引擎仿真开发可视化SLAM算法
本示例展示如何使用从虚幻引擎®仿真环境中获得的图像数据开发可视化同步定位和映射(SLAM)算法。
视觉大满贯是计算相机相对于周围环境的位置和方向，同时映射环境的过程。开发可视化SLAM算法并评估其在不同条件下的性能是一项具有挑战性的任务。最大的挑战之一是生成相机传感器的地面真实值，特别是在室外环境中。使用模拟可以在各种场景和相机配置下进行测试，同时提供精确的地面真相。
本例演示了在停车场景中使用虚幻引擎模拟开发用于单目或立体摄像机的视觉SLAM算法。有关可视化SLAM管道实现的更多信息，请参见单目视觉同步定位与作图示例和立体视觉同步定位与映射的例子。
在仿真环境中设置场景
使用Simulation 3D Scene Configuration块设置仿真环境。选择内置的大型停车场场景，其中包含几辆停放的车辆。视觉SLAM算法匹配连续图像的特征。为了增加潜在特征匹配的数量，您可以使用停放车辆子系统向场景中添加更多停放车辆。要指定车辆的停车姿势，请使用helperAddParkedVehicle函数。如果你选择一个更自然的场景，额外车辆的存在是没有必要的。自然场景通常具有足够的纹理和适合特征匹配的特征多样性。
你可以参考为虚幻引擎模拟选择路点交互式选择停车位置序列的示例。你可以使用同样的方法来选择一系列路径点，并为自我载具生成一个参考轨迹。本例使用记录的参考轨迹和停放的车辆位置。
负载参考路径%数据=负载(“parkingLotReferenceData.mat”）;设定自我飞行器的参考轨迹refPosesX = data.refPosesX;refPosesY = data.refPosesY;refPosesT = data.refPosesT;设置停放车辆的姿势parkedpose = data. parkedpose;显示参考路径和停放车辆的位置sceneName =“LargeParkingLot”；hScene =图;helperShowSceneImage (sceneName);持有在plot(refPosesX(:，2)， refPosesY(:，2)， LineWidth=2, DisplayName=参考路径的）;scatter(parkedpose (:，1)， parkedpose (:，2)， []，“填充”DisplayName =停放车辆的）;xlim([-60 40]) ylim([10 60])位置= [100,100,1000,500];%调整大小传说举行从
打开模型并添加停放的车辆
modelName =“GenerateImageDataOfParkingLot”；open_system (modelName);
helperAddParkedVehicles (modelName parkedPoses);
设置自我车辆和摄像传感器
设置自我车辆沿指定的参考路径移动使用模拟三维车辆与地面跟踪块。相机变型子系统包含两种配置的相机传感器:单眼和立体声。在这两种配置中，摄像头都安装在车顶中心。您可以使用相机校准器或立体摄像机校准器应用程序来估计你想模拟的实际相机的intrinsic。这个例子首先展示了单眼相机工作流，然后是立体相机工作流。
%选择单目相机useMonoCamera = 1;检查单目相机open_system ([modelName,“/相机/单眼”]);%相机本征focalLength = [700, 700];%以像素为单位指定principalPoint = [600, 180];%(像素[x, y])imageSize = [370,1230];%(像素)[mrows, ncols]intrinsic = cameraIntrinsics(focalLength, principalPoint, imageSize);
可视化和记录传感器数据
运行模拟以可视化和记录传感器数据。使用视频查看器块来可视化来自摄像机传感器的图像输出。使用To Workspace块记录相机传感器的真实位置和方向。
关上(hScene)如果~ ispc错误(虚幻引擎模拟仅支持微软+ char(174“Windows”+ char(174“。”）;结束%运行模拟simOut = sim(modelName);
提取相机图像作为imagedatstoreimds = helpergetcamerimages (simOut);提取ground truth作为rigidtform3d对象数组gTruth = helperGetSensorGroundTruth(simOut);
利用记录数据开发单目视觉SLAM算法
利用图像对单目视觉SLAM算法进行评价。这个函数helperVisualSLAM实现单目ORB-SLAM管道:
地图初始化: ORB-SLAM首先从两幅图像初始化三维点的映射。使用
estrelpose来计算基于二维ORB特征对应和的相对位姿由三角形组成的来计算三维地图点。这两个帧存储在imageviewset对象作为关键帧。三维地图点及其与关键帧的对应关系存储在worldpointset对象。跟踪: map初始化后，对于每个新图像，函数
helperTrackLastKeyFrame通过匹配当前帧中的特征和最后一个关键帧中的特征来估计相机姿态。这个函数helperTrackLocalMap通过跟踪本地地图来改进估计的相机姿势。当地的地图:如果当前帧被识别为关键帧，则用于创建新的三维地图点。在这个阶段，
bundleAdjustment用于通过调整相机姿势和3-D点来最小化重投影误差。循环关闭:通过使用特征袋方法将每个关键帧与之前的所有关键帧进行比较来检测循环。一旦检测到循环闭合，姿态图将被优化，以优化所有关键帧的相机姿态
optimizePoseGraph(导航工具箱)函数。
算法的具体实现请参见单目视觉同步定位与作图的例子。
[mapPlot, optimizedpositions, addedFramesIdx] = helperVisualSLAM(imds, intrinsic);
用帧1和帧4初始化的映射
循环边缘添加在关键帧:4和97之间
根据事实进行评估
您可以根据从模拟中获得的地面真相来评估优化的摄像机轨迹。由于图像是由单目相机生成的，相机的轨迹只能恢复到一个未知的比例因子。您可以根据实际情况大致计算比例因子，从而模拟通常从外部传感器获得的结果。
绘制相机地面真实轨迹scaledTrajectory = plotActualTrajectory(mapPlot, gTruth(addedFramesIdx)， optimizedpositions);%显示图例showLegend (mapPlot);
您还可以计算轨迹估计的均方根误差(RMSE)。
helperEstimateTrajectoryError (gTruth (addedFramesIdx) scaledTrajectory);
关键帧轨迹的绝对RMSE (m): 1.9101
立体视觉SLAM算法
在单目视觉SLAM算法中，单台摄像机无法准确地确定深度。地图的比例和估计的轨迹是未知的，并随着时间的推移而漂移。此外，由于地图点通常不能从第一帧开始进行三角划分，引导系统需要多个视图来生成初始地图。使用立体摄像机解决了这些问题，并提供了更可靠的视觉SLAM解决方案。这个函数helperVisualSLAMStereo实现了立体视觉SLAM管道。与单目管道的关键区别在于，在地图初始化阶段，立体管道从同一帧的一对立体图像创建3-D地图点，而不是从不同帧的两张图像创建它们。算法的具体实现请参见立体视觉同步定位与映射的例子。
%选择立体相机usemoncamera = 0;检查立体相机open_system ([modelName,/相机/立体声的]);snapnow;设置立体相机基线基线= 0.5;%(米)构造用于三维重建的重投影矩阵reprojectionMatrix = [1,0,0， -principalPoint(1);0,1,0， -principalPoint(2);0, 0, 0, focalLength(1);0,0,1 /baseline, 0];立体声图像最大视差maxdifference = 48;%运行模拟simOut = sim(modelName);
snapnow;
提取立体图像
[imdsLeft, imdsRight] = helperGetCameraImagesStereo(simOut);提取ground truth作为rigidtform3d对象数组gTruth = helperGetSensorGroundTruth(simOut);
运行立体视觉SLAM算法
[mapPlot, optimizedpose, addedFramesIdx] = helperVisualSLAMStereo(imdsLeft, imdsRight, intrinsics, max悬殊，reprojectionMatrix);
循环边在关键帧:2和92之间添加
绘制相机地面真实轨迹optimizedTrajectory = plotActualTrajectory(mapPlot, gTruth(addedFramesIdx));%显示图例showLegend (mapPlot);
计算轨迹估计的均方根误差(RMSE)helperEstimateTrajectoryError (gTruth (addedFramesIdx) optimizedTrajectory);
关键帧轨迹的绝对RMSE (m): 0.27679
与单目视觉SLAM算法相比，立体视觉SLAM算法能更准确地估计出摄像机的轨迹。
立体图像的密集重建
给定精致的相机姿态，您可以从对应于关键帧的立体图像执行密集重建。
pointCloudsAll = helperDenseReconstructFromStereo(imdsLeft, imdsRight, imdsRight).．.imageSize, addedFramesIdx, optimizedpose, max悬殊，reprojectionMatrix);视觉化场景figure(Position=[1100 600 1000 500]);x = pcshow(pointCloudsAll,VerticalAxis=“y”VerticalAxisDir =“向下”）;包含(“X”) ylabel (“Y”) zlabel (“Z”）显示停车场的鸟瞰图View (ax， [0 -1 0]);camroll (ax, 90);
关闭模型和图形。
close_system (modelName 0);关闭所有
支持功能
helperGetCameraImages获取相机输出
函数imds = helpergetcamerimages (simOut)将图像数据保存到一个临时文件夹dataFolder = fullfile(tempdir，“parkingLotImages”, filesep);folderExists = exist(数据文件夹“dir”）;如果~ folderExists mkdir (dataFolder);结束files = dir(dataFolder);如果numFrames = nummel (simOut.images.Time);为i = 3:numFrames忽略前两帧img = squeeze(simOut.images.Data(:，:，:，i));img， [dataFolder, sprintf(' % 04 d '我2),“使用”])结束结束创建一个imageDatastore对象来存储所有的图像imds = imageDatastore(数据文件夹);结束
helperGetCameraImagesStereo获取相机输出
函数[imdsLeft, imdsRight] = helperGetCameraImagesStereo(simOut)将图像数据保存到一个临时文件夹dataFolderLeft = fullfile(tempdir，“parkingLotStereoImages”filesep,“左”, filesep);dataFolderRight = fullfile(tempdir，“parkingLotStereoImages”filesep,“对”, filesep);folderExists = exist(dataFolderLeft，“dir”）;如果~ folderExists mkdir (dataFolderLeft);mkdir (dataFolderRight);结束files = dir(dataFolderLeft);如果numFrames = nummel (simOut.imagesLeft.Time);为i = 3:numFrames忽略前两帧imgLeft = squeeze(simOut.imagesLeft.Data(:，:，:，i));imgLeft， [dataFolderLeft, sprintf(' % 04 d '我2),“使用”]) imgRight = squeeze(simOut.imagesRight.Data(:，:，:，i));imgRight， [dataFolderRight, sprintf(' % 04 d '我2),“使用”])结束结束使用imageDatastore对象存储立体图像imdsLeft = imageDatastore(dataFolderLeft);imdsRight = imageDatastore(dataFolderRight);结束
helperGetSensorGroundTruth保存传感器地面真相
函数gTruth = helperGetSensorGroundTruth(simOut) numFrames = nummel (simOut.location. time);gTruth = repmat(rigidtform3d, numframes - 2,1);为i = 1:numFrames-2忽略前两帧gTruth(我)。翻译=挤压(simOut.location。数据(:，:，i+2));忽略滚转和俯仰旋转，因为地面是平的偏航= double(simOut.orientation。数据(:，3,i+2));gTruth(我)。R = [cos(偏航)，-sin(偏航)，0;.．.Sin(偏航)cos(偏航)0;.．.0,0,1];结束结束
helperEstimateTrajectoryError计算跟踪误差
函数rmse = helperEstimateTrajectoryError(gTruth, scaledLocations) gLocations = vertcat(gTruth. translation);rmse = sqrt(mean(sum((scaledLocations - gLocations)。^2, 2)));disp ([关键帧轨迹的绝对RMSE (m):num2str (rmse)]);结束
helperDenseReconstructFromStereo从已知相机姿态的立体图像进行密集重建
函数pointCloudsAll = helperDenseReconstructFromStereo(imdsLeft, imdsRight, imdsRight).．.imageSize, addedFramesIdx, optimizedpositions, maxparallence, reprojectionMatrix) ptClouds = repmat(pointCloud(零(1,3))，numel(addedFramesIdx)， 1);为i = 1: numel(addedFramesIdx) I1 = readimage(imdsLeft, addedFramesIdx(i));I2 = readimage(imdsRight, addedFramesIdx(i));disparityMap = disparitySGM(im2gray(I1)， im2gray(I2)， DisparityRange=[0, maxdifference]，UniquenessThreshold=20);xyzPoints = reconstructScene(disparityMap, reprojectionMatrix);忽略图像的上半部分，它主要显示天空。xyzPoints(1:100，:，:) = NaN;xyzPoints =重塑(xyzPoints， [imageSize(1)*imageSize(2)， 3]);validIndex = xyzPoints(:， 3) > 0 & xyzPoints(:， 3) < 40/reprojectionMatrix(4,3);xyzPoints = xyzPoints(validIndex，:);colors =重塑(I1， [imageSize(1)*imageSize(2)， 3]);colors = colors(validIndex，:);currPose = optimizedposts . absolutepose (i);xyzPoints = transformPointsForward(currPose, xyzPoints);ptCloud = pointCloud(xyzPoints, Color=colors);ptCloud (i) = pcdownsample(ptCloud, random=0.2);结束连接点云pointCloudsAll = pccat(ptClouds);结束
另请参阅
imageviewset|由三角形组成的|optimizePoses|estrelpose|bundleAdjustment
相关的话题
您也可以从以下列表中选择一个网站: