构建一个映射与激光雷达测程法和映射(壤土)使用虚幻引擎模拟
这个例子展示了如何构建一个映射与激光雷达测程法和映射(壤土)[1]算法通过使用虚幻引擎®合成激光雷达数据模拟环境。在这个例子中,您将了解如何:
记录和可视化合成激光雷达传感器数据从一个使用虚幻引擎3 d仿真环境。
使用壤土算法注册记录点云和构建一个映射。
在仿真环境中建立场景
加载预先构建的大型停车场(自动驾驶工具箱)场景和预选的参考轨迹。有关如何生成参考轨迹的信息交互通过选择一个锚点序列,看到选择路径点虚幻引擎模拟(自动驾驶工具箱)的例子。
%负荷参考路径data =负载(“parkingLotReferenceData.mat”);%设置参考轨迹的自我refPosesX = data.refPosesX;refPosesY = data.refPosesY;refPosesT = data.refPosesT;%设置停放车辆的姿势parkedPoses = data.parkedPoses;%显示参考轨迹和停放车辆的位置sceneName =“LargeParkingLot”;hScene图(Name = =“大型停车场”NumberTitle =“关闭”);helperShowSceneImage (sceneName);持有在情节(refPosesX (:, 2), refPosesY(:, 2),线宽= 2,DisplayName =“参考路径”);散射(parkedPoses (: 1) parkedPoses (:, 2), [],“填充”DisplayName =“停车辆”);xlim (40 [-60]) ylim hScene (60 [10])。位置= (100 100 1000 500);%调整图标题(“大型停车场”传说)
打开模型®模型,添金宝app加额外的车辆到现场使用helperAddParkedVehicle函数。
modelName =“GenerateLidarDataOfParkingLot”;open_system (modelName) snapnow helperAddParkedVehicles (modelName parkedPoses)
记录和可视化数据
使用模拟3 d车辆与地面(自动驾驶工具箱)块来模拟车辆沿着指定的参考轨迹。使用模拟3 d激光雷达(自动驾驶工具箱)块中心的屋顶上挂载激光雷达的车辆,并记录传感器数据。
关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”金宝app+ char (174) +“Windows”+ char (174) +“。”);结束%运行仿真simOut = sim (modelName);close_system (modelName, 0)
使用helperGetPointClouds函数和helperGetLidarGroundTruth函数来提取激光雷达数据和地面真理构成。
ptCloudArr = helperGetPointClouds (simOut);groundTruthPosesLidar = helperGetLidarGroundTruth (simOut);
检测边缘点和表面点
壤土算法使用边缘点和表面点注册和映射。的detectLOAMFeatures函数输出一个LOAMPoints对象,该对象存储选中的边缘点和表面点。它包括每个点的标签,可以锐,less-sharp-edge,平面或less-planar-surface。使用pcregisterloam函数注册两个组织点云。
ptCloud = ptCloudArr (1);nextPtCloud = ptCloudArr (2);gridStep = 0.5;tform = pcregisterloam (ptCloud nextPtCloud gridStep);disp (tform)
rigidtform3d属性:维数:3翻译:[-0.2765 2.5534 -0.6325]R:[3×3单]A:(4×4个)
另外,更多的控制精度和速度之间的权衡,你可以首先检测壤土特征点,然后执行壤土登记使用pcregisterloam。这些步骤之前推荐的壤土注册:
检测壤土特征点使用
detectLOAMFeatures函数。Downsample平面表面越少点使用
downsampleLessPlanar对象的功能。这个步骤有助于加快注册使用pcregisterloam函数。
因为每个点的检测算法依赖于邻居对边缘点进行分类和表面点,以及识别可靠点的边界闭塞的地区,如将采样预处理步骤,去噪和地面特征点检测之前删除不推荐。从远离传感器的数据,删除噪音和加快注册、过滤范围的点云。的findPointsInCylinder目标函数选择一个圆柱周围的邻居传感器,给定一个指定的圆柱半径,也不包括数据接近传感器和可能包括车辆的一部分。
egoRadius = 3.5;cylinderRadius = 50;selectedIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);ptCloud =选择(ptCloud selectedIdx OutputSize =“全部”);selectedIdx = findPointsInCylinder (nextPtCloud [egoRadius cylinderRadius]);nextPtCloud =选择(nextPtCloud selectedIdx OutputSize =“全部”);图保存在标题(“圆柱附近”)pcshow (ptCloud)视图(2)
接下来,壤土特征点检测使用detectLOAMFeatures函数。调优这个函数需要实证分析。的detectLOAMFeatures名称-值参数提供一个注册的准确性和速度之间的权衡。提高注册的准确性,必须最小化均方误差之间的欧几里得距离对齐点。跟踪和最小化均方误差输出rmse的pcregisterloam当你增加的价值功能NumRegionsPerLaser,MaxSharpEdgePoints,MaxLessSharpEdgePoints,MaxPlanarSurfacePoints参数的detectLOAMFeatures。
maxPlanarSurfacePoints = 8;点= detectLOAMFeatures (ptCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);nextPoints = detectLOAMFeatures (nextPtCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);图保存在标题(“壤土点”)显示(点,MarkerSize = 12)
[~,rmseValue] = pcregisterloam(点,nextPoints);disp ([“RMSE:”num2str (rmseValue)))
RMSE: 0.59803
detectLOAMFeatures首先确定了锋利的边缘点,少锋利的边缘点,平面表面点。所有剩余的点不被认为是不可靠的点,和曲率值低于阈值被归类为平面表面少点。将采样平面表面的点可以加快注册时使用pcregisterloam。
点= downsampleLessPlanar(点,gridStep);图保存在标题(将采样后的壤土点少平面表面点”)显示(点,“MarkerSize”,12)
使用激光雷达测程法构建地图
壤土算法由两个主要组件集成计算准确的转换:激光雷达测距和激光雷达映射。使用pcregisterloam函数与一对一的匹配方法估计转换使用激光雷达测距算法。一对一的匹配方法匹配每个点的最近邻,匹配边缘指向边缘点和表面点表面点。使用这些匹配计算转换的估计。使用pcregisterloam一对一的匹配方法,逐步建立一个停车场。使用的地图pcviewset对象来管理数据。
初始化的姿态和点云视图集。
absPose = groundTruthPosesLidar (1);relPose = rigidtform3d;vSetOdometry = pcviewset;
第一个视图添加到视图集。
viewId = 1;vSetOdometry = addView (vSetOdometry viewId absPose);
注册点云增量和可视化在停车场车辆位置的场景。
%显示停车场场景与参考轨迹hScene图(Name = =“大型停车场”NumberTitle =“关闭”);helperShowSceneImage (sceneName);持有在情节(refPosesX (:, 2), refPosesY(:, 2),线宽= 2)xlim (40 [-60]) ylim hScene (60 [10])。位置= (100 100 1000 500);numSkip = 5;为k = (numSkip + 1): numSkip:元素个数(ptCloudArr) prevPoints =点;viewId = viewId + 1;ptCloud = ptCloudArr (k);%选择一个圆柱感兴趣的社区。selectedIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);ptCloud =选择(ptCloud selectedIdx OutputSize =“全部”);%检测壤土分和downsample平面表面点越少点= detectLOAMFeatures (ptCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);点= downsampleLessPlanar(点,gridStep);%注册使用前面的相对冒充一个初始点%转换relPose = pcregisterloam(点、prevPoints InitialTransform = relPose);%更新绝对构成并将其存储在视图集absPose = rigidtform3d (absPose。* relPose.A);vSetOdometry = addView (vSetOdometry viewId absPose);%可视化绝对姿势在停车场场景情节(absPose.Translation (1) absPose.Translation(2),颜色=“r”标志=“。”,MarkerSize = 8);xlim (40 [-60]) ylim(60[10])标题(“建立一个地图使用激光雷达测程法”)传说([“地面实况”,“估计位置”])暂停(0.001)视图(2)结束
改善与激光雷达测绘的地图的准确性
从激光雷达测距激光映射改进姿势估计做登记点激光扫描和点之间在当地的地图,包括多个激光扫描。它匹配每个点到多个最近的邻居在当地的地图,然后使用这些匹配细化变换估计从激光雷达测距。使用pcmaploam对象管理分地图。从激光雷达测程法使用改进姿势估计findPose,并添加指向地图使用addPoints。
初始化动作。
absPose = groundTruthPosesLidar (1);relPose = rigidtform3d;vSetMapping = pcviewset;
检测壤土点第一个点云。
ptCloud = ptCloudArr (1);selectedIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);ptCloud =选择(ptCloud selectedIdx OutputSize =“全部”);点= detectLOAMFeatures (ptCloud,“MaxPlanarSurfacePoints”,maxPlanarSurfacePoints);点= downsampleLessPlanar(点,gridStep);
第一个视图添加到视图集。
viewId = 1;vSetMapping = addView (vSetMapping viewId absPose);
创建一个地图使用pcmaploam类,并添加指向地图使用addPoints对象的函数pcmaploam。
voxelSize = 0.1;loamMap = pcmaploam (voxelSize);addPoints (loamMap点,absPose);
使用pcregisterloam与一对一的匹配方法估计姿势使用激光雷达测距。然后,使用findPose对象的函数pcmaploam找到绝对的姿势相吻合的点对点的映射。
%显示停车场场景与参考轨迹hScene图(Name = =“大型停车场”NumberTitle =“关闭”);helperShowSceneImage (sceneName);持有在情节(refPosesX (:, 2), refPosesY(:, 2),线宽= 2)xlim (40 [-60]) ylim hScene (60 [10])。位置= (100 100 1000 500);为k = (numSkip + 1): numSkip:元素个数(ptCloudArr) prevPtCloud = ptCloud;prevPoints =点;viewId = viewId + 1;ptCloud = ptCloudArr (k);%选择一个圆柱感兴趣的社区。selectedIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);ptCloud =选择(ptCloud selectedIdx OutputSize =“全部”);%检测壤土分和downsample平面表面点越少点= detectLOAMFeatures (ptCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);点= downsampleLessPlanar(点,gridStep);%注册使用前面的相对冒充一个初始点%转换relPose = pcregisterloam(点、prevPoints MatchingMethod =“一对一”InitialTransform = relPose);%找到同盟的精制绝对姿态指向地图absPose = findPose (loamMap,点,relPose);%将精制绝对构成存储在视图集vSetMapping = addView (vSetMapping viewId absPose);%添加新的指向地图addPoints (loamMap点,absPose);%可视化绝对姿势在停车场场景情节(absPose.Translation (1) absPose.Translation(2),颜色=“r”标志=“。”MarkerSize = 8) xlim (40 [-60]) ylim(60[10])标题(“构建地图使用激光雷达映射”)传说([“地面实况”,“估计位置”])暂停(0.001)视图(2)结束
比较结果
可视化估计轨迹和地面真值进行比较。注意结合激光雷达测距和激光雷达映射的结果在一个更精确的地图。
%得到地面真理轨迹groundTruthTrajectory = vertcat (groundTruthPosesLidar.Translation);%得到估计的轨迹odometryPositions = vertcat (vSetOdometry.Views.AbsolutePose.Translation);mappingPositions = vertcat (vSetMapping.Views.AbsolutePose.Translation);%画出轨迹图plot3 (groundTruthTrajectory (: 1), groundTruthTrajectory (:, 2), groundTruthTrajectory(:, 3),线宽= 2,DisplayName =“地面实况”)举行在plot3 (odometryPositions (: 1) odometryPositions (:, 2), odometryPositions(:, 3),线宽= 2,DisplayName =“测程法”)plot3 (mappingPositions (: 1) mappingPositions (:, 2), mappingPositions(:, 3),线宽= 2,DisplayName =“测程法和映射”)视图(2)传说标题(“比较轨迹”)
数值比较估计轨迹与地面真理,计算地面真理之间的均方根误差(RMSE)轨迹和估计的轨迹。
%选择要比较的姿势selectedGroundTruth = groundTruthTrajectory (1: numSkip:最终,);%计算RMSErmseOdometry = rmse (selectedGroundTruth odometryPositions,“所有”);rmseWithMapping = rmse (selectedGroundTruth mappingPositions,“所有”);disp ([的RMSE轨迹估计与测程法:num2str (rmseOdometry)))
RMSE的轨迹估计量距:1.1396
disp ([的RMSE轨迹估计测程法和映射:“num2str (rmseWithMapping)))
的RMSE轨迹估计测程法和映射:0.2185
引用
[1],霁,桑吉夫•辛格。“壤土:激光雷达实时测程法和映射。“在机器人:科学和系统X。机器人:科学和系统的基础上,2014年。https://doi.org/10.15607/RSS.2014.X.007。
金宝app支持功能
helperGetPointClouds提取的数组pointCloud对象包含激光雷达传感器数据。
函数ptCloudArr = helperGetPointClouds (simOut)%提取信号ptCloudData = simOut.ptCloudData.signals.values;%创建一个pointCloud数组ptCloudArr = pointCloud (ptCloudData (::,:, 2));%忽略第一帧为n = 1:尺寸(ptCloudData, 4) ptCloudArr结束(+ 1)= pointCloud (ptCloudData (:,:,:, n));% #好< AGROW >结束结束
helperGetLidarGroundTruth提取的数组rigidtform3d对象包含地面实况的位置和方向。
函数gTruth = helperGetLidarGroundTruth (simOut) numFrames =大小(simOut.lidarLocation.time, 1);gTruth = repmat (rigidtform3d numFrames-1 1);为i = 1: numFrames gTruth(我)。翻译=挤压(simOut.lidarLocation.signals.values(:,:我));%忽略横滚和俯仰旋转自地面是平的偏航=双(simOut.lidarOrientation.signals.values(:, 3,我));gTruth(我)。R = [cos(偏航)sin(偏航)0;sin(偏航)因为(偏航)0;0 0 1);结束结束
