使用虚幻引擎模拟建立一个地图与激光雷达里程计和测绘(LOAM)
这个例子展示了如何使用激光雷达里程计和绘图(LOAM)构建一个地图。[1]算法使用合成的激光雷达数据来自虚幻引擎®仿真环境。在本例中，您将学习如何:
使用虚幻引擎从3D模拟环境中记录和可视化合成激光雷达传感器数据。
使用LOAM算法对记录的点云进行配准，并建立地图。
在模拟环境中设置场景
加载预构建的大型停车场(自动驾驶工具箱)场景和预选的参考轨迹。有关如何通过选择路径点序列以交互方式生成参考轨迹的信息，请参阅选择路径点虚幻引擎模拟(自动驾驶工具箱)的例子。
%加载参考路径数据=负载(“parkingLotReferenceData.mat”）;设置自我载具的参考轨迹refPosesX = data.refPosesX;refPosesY = data.refPosesY;refPosesT = data.refPosesT;设置停放车辆的姿势。parkedpose = data. parkedpose;显示参考轨迹和停放车辆的位置。sceneName =“LargeParkingLot”；hScene = figure(Name= .“大停车场”NumberTitle =“关闭”）;helperShowSceneImage (sceneName);持有在情节(refPosesX (:, 2), refPosesY(:, 2),线宽= 2,DisplayName =“参考路径”）;散射(parkedPoses (: 1) parkedPoses (:, 2), [],“填充”DisplayName =“停车辆”）;xlim([-60 40]) ylim([10 60]) hScene。位置= [100 100 1000 500];%调整数字标题(“大停车场”传说)
打开Simulink®模型，并使用helperAddParkedVehicle函数。
modelName =“GenerateLidarDataOfParkingLot”；open_system(modelName)
记录和可视化数据
使用模拟三维车辆与地面跟踪(自动驾驶工具箱)块来模拟车辆沿着指定的参考轨迹移动。使用模拟三维激光雷达(自动驾驶工具箱)块安装一个激光雷达在车顶的中心，并记录传感器数据。
关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”char(174) +“Windows”char(174) +“。”）;结束%运行模拟simOut = sim(modelName);close_system (modelName, 0)
使用helperGetPointClouds功能和helperGetLidarGroundTruth函数提取激光雷达数据和地面真实pose。
ptCloudArr = helperGetPointClouds(simOut);groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);
检测边缘点和表面点
LOAM算法使用边缘点和曲面点进行配准和映射。的detectLOAMFeatures函数输出aLOAMPoints对象，该对象存储所选的边缘点和曲面点。它包括每个点的标签，可以是锋利边缘、不那么锋利边缘、平面表面或非平面表面。使用pcregisterloam函数注册两个有组织的点云。
ptCloud = ptCloudArr(1);nextPtCloud = ptCloudArr(2);gridStep = 1;tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);disp (tform)
R: [3×3 single]翻译:[-0.2297 0.0156 -0.0032]A: [4×4 single]
另外，为了更好地控制准确性和速度之间的权衡，可以先检测LOAM特征点，然后使用pcregisterloam．在注册LOAM之前，建议采取以下步骤:
方法检测LOAM特征点
detectLOAMFeatures函数。对较少的平面表面点进行下采样
downsampleLessPlanar对象的功能。此步骤有助于加快使用pcregisterloam函数。
由于检测算法依赖于每个点的邻域对边缘点和曲面点进行分类，以及对闭塞区域边界上的不可靠点进行识别，因此不建议在特征点检测前进行下采样、去噪和去地等预处理步骤。为了从距离传感器较远的数据中去除噪声，并加快配准，可以根据距离过滤点云。的helperRangeFilter函数选择传感器周围的圆柱形邻域，给定一个指定的圆柱形半径，并排除太接近传感器的数据，可能包括车辆的一部分。
egoRadius = 2;汽缸半径= 30;ptCloud = helperRangeFilter(ptCloud,egoRadius，圆筒半径);nextPtCloud = helperRangeFilter(nextPtCloud,egoRadius,cylinderRadius);图保存在标题(“圆柱附近”pcshow(ptCloud)视图(2)
方法检测LOAM特征点detectLOAMFeatures函数。调整这一功能需要进行实证分析。的detectLOAMFeatures名称-值参数提供了注册精度和速度之间的权衡。为了提高配准的准确性，必须最小化对齐点之间欧氏距离的均方根误差。跟踪和最小化均方根误差输出rmse的pcregisterloam函数的值增加时NumRegionsPerLaser，MaxSharpEdgePoints，MaxLessSharpEdgePoints,MaxPlanarSurfacePoints参数的detectLOAMFeatures．
maxPlanarSurfacePoints = 8;points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints= MaxPlanarSurfacePoints);nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints= MaxPlanarSurfacePoints);图保存在标题(“壤土点”)显示(点,MarkerSize = 12)
[~，rmse] = pcregisterloam(points,nextPoints);disp (rmse)
1.1887
detectLOAMFeatures首先识别锋利的边缘点，不太锋利的边缘点和平面表面点。所有剩余的不被认为是不可靠的点，并且曲率值低于阈值的点被分类为小平面曲面点。在使用时，对较少的平面表面点进行下采样可以加快配准pcregisterloam．
points = downsampleLessPlanar(points,gridStep);图保存在标题(“下采样较少平面表面点后的LOAM点”)显示(点,“MarkerSize”, 12)
使用激光雷达里程表建立地图
LOAM算法由两个主要组件组成，它们被集成来计算精确的转换:激光雷达里程计和激光雷达映射。使用pcregisterloam函数与一对一匹配方法，利用激光雷达里程计算法得到估计变换。一对一的匹配方法将每个点与它最近的邻居进行匹配，将边缘点与边缘点进行匹配，将曲面点与曲面点进行匹配。使用这些匹配来计算转换的估计。使用pcregisterloam用一一对应的方法增量构建停车场地图。使用一个pcviewset对象来管理数据。
初始化姿态和点云视图集。
abposse = groundTruthPosesLidar(1);relPose = rigidtform3d;vSetOdometry = pcviewset
将第一个视图添加到视图集。
viewId = 1;vSetOdometry = addView(vSetOdometry,viewId, abposse);
逐步记录点云，可视化车辆在停车场场景中的位置。
以参考轨迹显示停车场场景。hScene = figure(Name= .“大停车场”NumberTitle =“关闭”）;helperShowSceneImage (sceneName);持有在plot(refPosesX(:，2)，refPosesY(:，2)，LineWidth=2) xlim([-60 40]) ylim([10 60])位置= [100 100 1000 500];numSkip = 5;为k = (numSkip+1)+1:numSkip: numl (ptCloudArr) prevPoints = points;viewId = viewId + 1;ptCloud = ptCloudArr(k);对点云应用范围过滤器ptCloud = helperRangeFilter(ptCloud,egoRadius，圆筒半径);检测LOAM点，并对较少的平面表面点进行下采样points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints= MaxPlanarSurfacePoints);points = downsampleLessPlanar(points,gridStep);注册点使用之前的相对姿势作为初始。%转换relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);更新绝对姿势并将其存储在视图集中abposse = rigidtform3d(abposse。A * relposea);vSetOdometry = addView(vSetOdometry,viewId, abposse);在停车场的场景中想象绝对姿势。情节(absPose.Translation (1) absPose.Translation(2),颜色=“r”标志=“。”, MarkerSize = 8);Xlim ([-60 40]) ylim([10 60]) title(“使用激光雷达里程计绘制地图”)传说([“地面实况”，“估计位置”暂停(0.001)视图(2)结束
利用激光雷达测绘提高地图精度
激光雷达映射通过在激光扫描点和包含多次激光扫描的局部地图点之间进行配准，从激光雷达里程计中优化了位姿估计。它将每个点与本地地图中的多个最近邻居进行匹配，然后使用这些匹配来改进激光雷达里程计的转换估计。使用pcmaploam对象来管理映射中的点。从激光雷达里程计改进姿态估计使用findPose，并添加点到地图使用addPoints．
初始化姿势。
abposse = groundTruthPosesLidar(1);relPose = rigidtform3d;vSetMapping = pcviewset;
在第一个点云中检测LOAM点。
ptCloud = ptCloudArr(1);ptCloud = helperRangeFilter(ptCloud,egoRadius，圆筒半径);点= detectLOAMFeatures(ptCloud，“MaxPlanarSurfacePoints”, maxPlanarSurfacePoints);points = downsampleLessPlanar(points,gridStep);
将第一个视图添加到视图集。
viewId = 1;vSetMapping = addView(vSetMapping,viewId,absPose);
方法创建映射pcmaploam类，并向映射中添加点addPoints的目标函数pcmaploam．
voxelSize = 0.5;loamMap = pcmaploam(voxelSize);addPoints (loamMap点,absPose);
使用pcregisterloam用一对一匹配的方法，利用激光雷达里程计得到估计的位姿。然后，使用findPose的目标函数pcmaploam找出使这些点与地图上的点对齐的绝对姿势。
以参考轨迹显示停车场场景。hScene = figure(Name= .“大停车场”NumberTitle =“关闭”）;helperShowSceneImage (sceneName);持有在plot(refPosesX(:，2)，refPosesY(:，2)，LineWidth=2) xlim([-60 40]) ylim([10 60])位置= [100 100 1000 500];numSkip = 5;为k = (numSkip+1)+1:numSkip:numel(ptCloudArr) prevPtCloud = ptCloud;prevPoints =点数;viewId = viewId + 1;ptCloud = ptCloudArr(k);对点云应用范围过滤器ptCloud = helperRangeFilter(ptCloud,egoRadius，圆筒半径);检测LOAM点，并对较少的平面表面点进行下采样points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints= MaxPlanarSurfacePoints);points = downsampleLessPlanar(points,gridStep);注册点使用之前的相对姿势作为初始。%转换relPose = pcregisterloam(points,prevPoints,MatchingMethod= .“一对一”InitialTransform = relPose);找到精确的绝对姿势，使点对齐到地图abposse = findPose(loamMap,points,relPose);在视图集中存储经过改进的绝对姿势vSetMapping = addView(vSetMapping,viewId,absPose);在地图上添加新点。addPoints (loamMap点,absPose);在停车场的场景中想象绝对姿势。情节(absPose.Translation (1) absPose.Translation(2),颜色=“r”标志=“。”，MarkerSize=8) xlim([-60 40]) ylim([10 60]) title(“使用激光雷达绘制地图”)传说([“地面实况”，“估计位置”暂停(0.001)视图(2)结束
比较结果
将估计的轨迹形象化，并将其与地面真实情况进行比较。注意，结合激光雷达里程计和激光雷达测绘结果更精确的地图。
图绘制(refPosesX (:, 2), refPosesY(:, 2),线宽= 2,DisplayName =“地面实况”)举行在用激光雷达里程计得到估计的位置odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);情节(odometryPositions (: 1) odometryPositions(:, 2),线宽= 2,DisplayName =“测程法”）用激光雷达里程计和测绘得到估计的位置mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);情节(mappingPositions (: 1) mappingPositions(:, 2),线宽= 2,DisplayName =“里程计及绘图”)传说标题(“比较轨迹”）
参考文献
[1]张，吉和桑吉夫·辛格。“LOAM:实时激光雷达里程计和测绘。”在机器人:科学与系统X．机器人:科学与系统基础，2014。https://doi.org/10.15607/RSS.2014.X.007．
支持功能
helperGetPointClouds的数组。pointCloud包含激光雷达传感器数据的对象。
函数ptCloudArr = helperGetPointClouds(simOut)%提取信号ptCloudData = simOut.ptCloudData.signals.values;创建pointCloud数组ptclouddarr = pointCloud(ptCloudData(:，:，:，2));%忽略第一帧为n = 3:size(ptCloudData,4) ptclouddarr (end+1) = pointCloud(ptCloudData(:，:，:，n));% #好< AGROW >结束结束
helperGetLidarGroundTruth的数组。rigidtform3d对象，包含基本真相的位置和方向。
函数gTruth = helperGetLidarGroundTruth(simOut) numFrames = size(simOut. lidarlocation .time,1);gTruth = repmat(rigidtform3d, numframes -1,1);为i = 2:numFrames gTruth(i-1)。翻译=挤压(simOut.lidarLocation.signals.values(:，:，i));忽略滚动和俯仰旋转，因为地面是平的。偏航= double(simOut.lidarOrientation.signals.values(:，3,i));gTruth(张)。R = [cos(偏航)-sin(偏航)0;Sin(偏航)cos(偏航)0;0 0 1];结束结束
helperRangeFilter按范围过滤点云。
函数ptCloud = helperRangeFilter(ptCloud,egoRadius,cylinderRadius)计算每个点到原点的距离。dist =函数(ptCloud.Location (:,: 1), ptCloud.Location (:,: 2));选择圆柱体半径内和自我半径外的点>= egoRadius;ptCloud = select(ptCloud,cylinderIdx,OutputSize=“全部”）;结束
您也可以从以下列表中选择网站: