主要内容

使用虚幻引擎开发视觉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);

图视频查看器包含一个坐标轴对象和其他对象类型的uiflowcontainer, uimenu uitoolbar。坐标轴对象包含一个类型的对象的形象。

helperAddParkedVehicles (modelName parkedPoses);

GenerateImageDataOfParkingLotModel.PNG

建立自我车辆和相机传感器

建立自我车辆沿着指定的参考路径通过使用模拟3 d车辆与地面块。相机变体子系统包含两个摄像头传感器配置:单眼和立体声。在这两种配置,相机安装在车辆屋顶中心。您可以使用相机校准器或立体相机校准器应用估计intrinsic实际的相机,你想模拟。这个例子显示了单眼相机工作流首先其次是立体相机工作流。

%选择单眼相机useMonoCamera = 1;%检查单眼相机open_system ([modelName,“/相机/单眼”]);%相机intrinsicfocalLength = (700、700);%以像素为单位指定principalPoint = (600、180);%的像素(x, y)图象尺寸= (370、1230);%在像素(mrows, ncols)intrinsic = cameraIntrinsics (focalLength principalPoint图象尺寸);

传感器数据可视化并记录

运行仿真可视化和传感器数据记录。使用视频查看器块来可视化图像相机传感器的输出。使用工作空间块记录地面真理摄像机的位置和方向传感器。

关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”万博1manbetx+ char (174) +“Windows”+ char (174) +“。”);结束%运行仿真simOut = sim (modelName);

图视频查看器包含一个坐标轴对象和其他对象类型的uiflowcontainer, uimenu uitoolbar。坐标轴对象包含一个类型的对象的形象。

作为一个imageDatastore %提取相机图像imd = helperGetCameraImages (simOut);%提取地面实况rigidtform3d对象数组gTruth = helperGetSensorGroundTruth (simOut);

开发单眼视觉SLAM算法使用记录数据

使用图像来评估单眼视觉算法。这个函数helperVisualSLAM实现了单眼ORB-SLAM管道:

  • 地图初始化:ORB-SLAM首先初始化地图的三维点两个图像。使用estrelpose计算基于2 d ORB功能通讯和相对姿态由三角形组成的计算三维地图点。这两个框架都存储在一个imageviewset对象作为关键帧。三维地图点及其对应的关键帧存储在一个worldpointset对象。

  • 跟踪:一旦初始化地图,对于每个新形象,功能helperTrackLastKeyFrame估计摄像机构成当前帧的匹配特性特性在最后关键帧。这个函数helperTrackLocalMap改进估计相机姿势通过跟踪当地地图。

  • 当地的地图:当前帧用来创建新的3 d地图点是否被确定为一个关键帧。在这个阶段,bundleAdjustment用于最小化reprojection错误通过调整相机的姿势和3 d点。

  • 循环关闭:为每个关键帧检测到循环通过比较它与以前的所有关键帧使用bag-of-features方法。一旦检测到循环闭合,构成图进行了优化完善相机所有关键帧的使用optimizePoseGraph(导航工具箱)函数。

算法的实现细节,请参阅单眼视觉同步定位和映射的例子。

[mapPlot, optimizedPoses addedFramesIdx] = helperVisualSLAM (imd, intrinsic);
地图初始化第一帧与帧4

图包含一个坐标轴对象。坐标轴对象与标题匹配特性在当前帧包含2图像类型的对象,线。

循环之间的边缘添加关键帧:4和97年

图点云播放器包含一个坐标轴对象。坐标轴对象包含3散射类型的对象。这些对象代表映射点,估计轨迹,轨迹进行了优化。

评估对地面实况

你可以评估优化相机轨迹对地面真理从模拟获得。由于图像来自一个单眼相机,相机的轨迹只能恢复到一个未知的比例因子。你可以大概计算比例因子从地面真理,因此模拟你通常会获得从外部传感器。

%画出相机地面实况轨迹scaledTrajectory = plotActualTrajectory (mapPlot gTruth (addedFramesIdx) optimizedPoses);%显示传奇showLegend (mapPlot);

图点云播放器包含一个坐标轴对象。坐标轴对象包含4散射类型的对象。这些对象代表映射点,估计轨迹,实际优化轨迹,轨迹。

你也可以计算均方根误差(RMSE)轨迹估计。

helperEstimateTrajectoryError (gTruth (addedFramesIdx) scaledTrajectory);
绝对RMSE关键帧轨迹(m): 1.9101

立体视觉SLAM算法

单眼视觉SLAM算法,深度不能准确地确定使用一个相机。这张地图的比例尺和估计是未知的和漂移轨迹。此外,由于地图点往往不能从第一帧三角,引导系统需要多个视图产生一个初始地图。使用立体相机解决了这些问题,并提供一个更可靠的视觉解决方案。这个函数helperVisualSLAMStereo实现了立体视觉大满贯管道。单眼管道的关键区别在于,在地图初始化阶段,立体管道从一对立体影像创建3 d地图点相同的框架,而不是创建它们从两个不同的图像帧。算法的实现细节,请参阅立体视觉同步定位和映射的例子。

%选择立体相机useMonoCamera = 0;%检查立体相机open_system ([modelName,/相机/立体声的]);snapnow;%设置立体相机的基线基线= 0.5;%在米% reprojection矩阵构造3 d重建reprojectionMatrix = (1,0,0, -principalPoint (1);0 1 0,-principalPoint (2);0,0,0,focalLength (1);0,0,1 /基线,0);%最大差距在立体图像maxDisparity = 48;%运行仿真simOut = sim (modelName);

图包含一个坐标轴对象。坐标轴对象包含4图像类型的对象,线。

图视频查看器包含一个坐标轴对象和其他对象类型的uiflowcontainer, uimenu uitoolbar。坐标轴对象包含一个类型的对象的形象。

snapnow;

提取立体影像

[imdsLeft, imdsRight] = helperGetCameraImagesStereo (simOut);%提取地面实况rigidtform3d对象数组gTruth = helperGetSensorGroundTruth (simOut);

运行立体视觉算法

[mapPlot, optimizedPoses addedFramesIdx] = helperVisualSLAMStereo (imdsLeft、imdsRight intrinsic、maxDisparity reprojectionMatrix);

图包含一个坐标轴对象。坐标轴对象与标题匹配特性在当前帧包含4中类型的对象形象,线。

循环之间的边缘添加关键帧:2和92
%画出相机地面实况轨迹optimizedTrajectory = plotActualTrajectory (mapPlot gTruth (addedFramesIdx));%显示传奇showLegend (mapPlot);

图点云播放器包含一个坐标轴对象。坐标轴对象包含4散射类型的对象。这些对象代表映射点,估计轨迹,实际优化轨迹,轨迹。

%计算均方根误差(RMSE)轨迹估计helperEstimateTrajectoryError (gTruth (addedFramesIdx) optimizedTrajectory);
绝对RMSE关键帧轨迹(m): 0.27679

与单眼视觉SLAM算法相比,立体视觉SLAM算法产生一个更精确的估计摄像机轨迹。

从立体影像密度重建

鉴于精制相机的姿势,可以执行密集的重建与关键帧相对应的立体影像。

pointCloudsAll = helperDenseReconstructFromStereo (imdsLeft imdsRight,…图象尺寸、addedFramesIdx optimizedPoses、maxDisparity reprojectionMatrix);%可视化场景图(位置= (1100 600 1000 500));ax = pcshow (pointCloudsAll VerticalAxis =“y”VerticalAxisDir =“向下”);包含(“X”)ylabel (“Y”)zlabel (“Z”)%显示停车场的鸟瞰视图(ax, [0 1 0]);camroll (ax, 90);

模型和数据。

close_system (modelName 0);关闭所有

万博1manbetx支持功能

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 (rigidtform3d numFrames-2 1);为我= 1:numFrames-2%忽略前两帧gTruth(我)。翻译= (simOut.location挤。数据(:,:,i + 2));%忽略横滚和俯仰旋转自地面是平的偏航= (simOut.orientation两倍。数据(:3 + 2));gTruth(我)。R = [cos(偏航),sin(偏航),0;…罪(偏航),因为(偏航),0;…0,0,1];结束结束

helperEstimateTrajectoryError计算跟踪误差

函数rmse = helperEstimateTrajectoryError (gTruth scaledLocations) gLocations = vertcat (gTruth.Translation);rmse =√意味着总和((scaledLocations - gLocations)。^ 2,2)));disp ([“绝对RMSE关键帧轨迹(m):“num2str (rmse)]);结束

helperDenseReconstructFromStereo执行密集的重建立体影像与已知相机姿势

函数pointCloudsAll = helperDenseReconstructFromStereo (imdsLeft imdsRight,…图象尺寸、addedFramesIdx optimizedPoses、maxDisparity reprojectionMatrix) ptClouds = repmat (pointCloud(0(1、3)),元素个数(addedFramesIdx), 1);为i = 1:元素个数(addedFramesIdx) I1 = readimage (imdsLeft, addedFramesIdx(我));I2 = readimage (imdsRight addedFramesIdx(我));disparityMap = disparitySGM (im2gray (I1) im2gray (I2) DisparityRange = [0, maxDisparity], UniquenessThreshold = 20);xyzPoints = reconstructScene (disparityMap reprojectionMatrix);%忽略图像的上半部分,主要显示了天空xyzPoints(1:10 0,:,) =南;xyzPoints =重塑(xyzPoints,图象尺寸(1)*图象尺寸(2),3));validIndex = xyzPoints (: 3) > 0 & xyzPoints (:, 3) < 40 / reprojectionMatrix (4,3);xyzPoints = xyzPoints (validIndex:);颜色=重塑(I1,图象尺寸(1)*图象尺寸(2),3));颜色=颜色(validIndex:);currPose = optimizedPoses.AbsolutePose(我);xyzPoints = transformPointsForward (currPose xyzPoints);ptCloud = pointCloud (xyzPoints颜色=颜色);ptClouds (i) = pcdownsample (ptCloud,随机= 0.2);结束%连接的点云pointCloudsAll = pccat (ptClouds);结束