使用虚幻引擎开发视觉SLAM算法仿真
这个例子展示了如何开发一个视觉同时定位和地图(大满贯)算法利用图像数据从虚幻引擎®获得仿真环境。
视觉大满贯是一个过程,计算相机的位置和姿态对其周围环境,同时环境的映射。开发一个视觉SLAM算法和评估其在不同条件下的性能是一项非常具有挑战性的任务。最大的挑战之一是产生地面真理的相机传感器,特别是在户外环境中。使用模拟下可以测试各种场景和相机的配置,同时提供精确的地面实况。
这个例子演示了使用虚幻引擎模拟开发视觉SLAM算法单眼或立体相机在停车的情况下。有关视觉的实现大满贯的更多信息管道,看到单眼视觉同步定位和映射的例子,立体视觉同步定位和映射的例子。
在仿真环境中建立场景
使用仿真3 d场景配置块建立仿真环境。选择内置的大型停车场的场景,它包含几个停放车辆。在连续图像视觉SLAM算法匹配特性。增加潜在的数量特性匹配,您可以使用停车辆子系统增加更多停放车辆到现场。指定停车造成的车辆使用helperAddParkedVehicle函数。如果你选择了一个更自然的场景,其他车辆的存在是没有必要的。自然场景通常有足够的结构和特性适合各种特性匹配。
你可以按照选择路径点虚幻引擎模拟(自动驾驶工具箱)交互式地选择停车位置序列的例子。您可以使用相同的方法来选择一个锚点序列,为自我车辆生成参考轨迹。这个示例使用记录参考轨迹和停放车辆的位置。
%负荷参考路径data =负载(“parkingLotReferenceData.mat”);%设置参考轨迹的自我refPosesX = data.refPosesX;refPosesY = data.refPosesY;refPosesT = data.refPosesT;%设置停放车辆的姿势parkedPoses = data.parkedPoses;%显示参考路径和停放车辆的位置sceneName =“LargeParkingLot”;hScene =图;helperShowSceneImage (sceneName);持有在情节(refPosesX (:, 2), refPosesY (:, 2),“线宽”2,“DisplayName的”,参考路径的);散射(parkedPoses (: 1) parkedPoses (:, 2), [],“填充”,“DisplayName的”,停放车辆的);xlim (40 [-60]) ylim hScene (60 [10])。位置= (100、100、1000、500);%调整图传说举行从
打开模型并添加停放车辆
modelName =“GenerateImageDataOfParkingLot”;open_system (modelName);snapnow;helperAddParkedVehicles (modelName parkedPoses);
建立自我车辆和相机传感器
建立自我车辆沿着指定的参考路径通过使用模拟3 d车辆与地面块。相机变体子系统包含两个摄像头传感器配置:单眼和立体声。在这两种配置,相机安装在车辆屋顶中心。您可以使用相机校准器或立体相机校准器应用估计intrinsic实际的相机,你想模拟。这个例子显示了单眼相机工作流首先其次是立体相机工作流。
%选择单眼相机useMonoCamera = 1;%检查单眼相机open_system ([modelName,“/相机/单眼”]);snapnow;%相机intrinsicfocalLength = (700、700);%以像素为单位指定principalPoint = (600、180);%的像素(x, y)图象尺寸= (370、1230);%在像素(mrows, ncols)intrinsic = cameraIntrinsics (focalLength principalPoint图象尺寸);
传感器数据可视化并记录
运行仿真可视化和传感器数据记录。使用视频查看器块来可视化图像相机传感器的输出。使用工作空间块记录地面真理摄像机的位置和方向传感器。
关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”金宝app+ char (174) +“Windows”+ char (174) +“。”);结束%运行仿真simOut = sim (modelName);snapnow;作为一个imageDatastore %提取相机图像imd = helperGetCameraImages (simOut);%提取地面实况rigid3d对象数组gTruth = helperGetSensorGroundTruth (simOut);
开发单眼视觉SLAM算法使用记录数据
使用图像来评估单眼视觉算法。这个函数helperVisualSLAM实现了单眼ORB-SLAM管道:
地图初始化:ORB-SLAM首先初始化地图的三维点两个图像。使用
relativeCameraPose由三角形组成的imageviewsetworldpointset对象。跟踪:一旦初始化地图,对于每个新形象,功能
helperTrackLastKeyFrame估计摄像机构成当前帧的匹配特性特性在最后关键帧。这个函数helperTrackLocalMap改进估计相机姿势通过跟踪当地地图。当地的地图:当前帧用来创建新的3 d地图点是否被确定为一个关键帧。在这个阶段,
bundleAdjustment循环关闭:为每个关键帧检测到循环通过比较它与以前的所有关键帧使用bag-of-features方法。一旦检测到循环闭合,构成图进行了优化完善相机所有关键帧的使用
optimizePoseGraph(导航工具箱)
算法的实现细节,请参阅单眼视觉同步定位和映射的例子。
[mapPlot, optimizedPoses addedFramesIdx] = helperVisualSLAM (imd, intrinsic);
地图初始化第一帧与帧3环之间的边缘添加关键帧:7 - 156
评估对地面实况
你可以评估优化相机轨迹对地面真理从模拟获得。由于图像来自一个单眼相机,相机的轨迹只能恢复到一个未知的比例因子。你可以大概计算比例因子从地面真理,因此模拟你通常会获得从外部传感器。
%画出相机地面实况轨迹scaledTrajectory = plotActualTrajectory (mapPlot gTruth (addedFramesIdx) optimizedPoses);%显示传奇showLegend (mapPlot);
你也可以计算均方根误差(RMSE)轨迹估计。
helperEstimateTrajectoryError (gTruth (addedFramesIdx) scaledTrajectory);
绝对RMSE关键帧轨迹(m): 11.8389
立体视觉SLAM算法
单眼视觉SLAM算法,深度不能准确地确定使用一个相机。这张地图的比例尺和估计是未知的和漂移轨迹。此外,由于地图点往往不能从第一帧三角,引导系统需要多个视图产生一个初始地图。使用立体相机解决了这些问题,并提供一个更可靠的视觉解决方案。这个函数helperVisualSLAMStereo实现了立体视觉大满贯管道。单眼管道的关键区别在于,在地图初始化阶段,立体管道从一对立体影像创建3 d地图点相同的框架,而不是创建它们从两个不同的图像帧。算法的实现细节,请参阅立体视觉同步定位和映射的例子。
%选择立体相机useMonoCamera = 0;%检查立体相机open_system ([modelName,/相机/立体声的]);snapnow;%设置立体相机的基线基线= 0.5;%在米%运行仿真simOut = sim (modelName);snapnow;
提取立体影像
[imdsLeft, imdsRight] = helperGetCameraImagesStereo (simOut);%提取地面实况rigid3d对象数组gTruth = helperGetSensorGroundTruth (simOut);
运行立体视觉算法
[mapPlot, optimizedPoses addedFramesIdx] = helperVisualSLAMStereo (intrinsic imdsLeft, imdsRight,基线);%画出相机地面实况轨迹optimizedTrajectory = plotActualTrajectory (mapPlot gTruth (addedFramesIdx));%显示传奇showLegend (mapPlot);%计算均方根误差(RMSE)轨迹估计helperEstimateTrajectoryError (gTruth (addedFramesIdx) optimizedTrajectory);
循环之间的边缘添加关键帧:5和145年绝对RMSE关键帧轨迹(m): 0.25164
与单眼视觉SLAM算法相比,立体视觉SLAM算法产生一个更精确的估计摄像机轨迹。
模型和数据。
close_system (modelName 0);关闭所有
金宝app支持功能
helperGetCameraImages让相机输出
函数imd = helperGetCameraImages (simOut)%将图像数据保存到一个临时文件夹中dataFolder = fullfile (tempdir,“parkingLotImages”,filesep);folderExists =存在(dataFolder,“dir”);如果~ folderExists mkdir (dataFolder);结束文件= dir (dataFolder);如果元素个数(文件)< 3 numFrames =元素个数(simOut.images.Time);为我= 3:numFrames%忽略前两帧img =挤压(simOut.images.Data(::,:,我));imwrite (img, [dataFolder sprintf (' % 04 d '我2),“使用”])结束结束%创建一个imageDatastore对象来存储所有的图像imd = imageDatastore (dataFolder);结束
helperGetCameraImagesStereo让相机输出
函数[imdsLeft, imdsRight] = helperGetCameraImagesStereo (simOut)%将图像数据保存到一个临时文件夹中dataFolderLeft = fullfile (tempdir,“parkingLotStereoImages”filesep,“左”,filesep);dataFolderRight = fullfile (tempdir,“parkingLotStereoImages”filesep,“对”,filesep);folderExists =存在(dataFolderLeft,“dir”);如果~ folderExists mkdir (dataFolderLeft);mkdir (dataFolderRight);结束文件= dir (dataFolderLeft);如果元素个数(文件)< 3 numFrames =元素个数(simOut.imagesLeft.Time);为我= 3:numFrames%忽略前两帧imgLeft =挤压(simOut.imagesLeft.Data(::,:,我));imwrite (imgLeft [dataFolderLeft sprintf (' % 04 d '我2),“使用”])imgRight =挤压(simOut.imagesRight.Data(::,:,我));imwrite (imgRight [dataFolderRight sprintf (' % 04 d '我2),“使用”])结束结束%使用imageDatastore对象存储立体图像imdsLeft = imageDatastore (dataFolderLeft);imdsRight = imageDatastore (dataFolderRight);结束
helperGetSensorGroundTruth保存传感器地面实况
函数gTruth = helperGetSensorGroundTruth (simOut) numFrames =元素个数(simOut.location.Time);gTruth = repmat (rigid3d numFrames-2 1);为我= 1:numFrames-2%忽略前两帧gTruth(我)。翻译= (simOut.location挤。数据(:,:,i + 2));%忽略横滚和俯仰旋转自地面是平的偏航= (simOut.orientation两倍。数据(:3 + 2));gTruth(我)。旋转= [cos(偏航),罪(偏航),0;…sin(偏航),因为(偏航),0;…0,0,1];结束结束
helperEstimateTrajectoryError计算跟踪误差
函数rmse = helperEstimateTrajectoryError (gTruth scaledLocations) gLocations = vertcat (gTruth.Translation);rmse =√意味着总和((scaledLocations - gLocations)。^ 2,2)));disp ([“绝对RMSE关键帧轨迹(m):“num2str (rmse)]);结束
