使用虚幻引擎模拟建立一个地图与激光雷达里程计和测绘(LOAM)
这个例子展示了如何使用激光雷达里程计和绘图(LOAM)来构建一个地图[1]算法通过使用来自虚幻引擎®仿真环境的合成激光雷达数据。在本例中，您将学习如何:
使用虚幻引擎从3D模拟环境中记录和可视化合成激光雷达传感器数据。
使用LOAM算法对记录的点云进行注册，并构建地图。
在仿真环境中设置场景
加载预先构建的大型停车场(自动驾驶工具箱)场景和预选的参考轨迹。有关如何通过选择路径点序列交互式生成参考轨迹的信息，请参见为虚幻引擎模拟选择路径点(自动驾驶工具箱)的例子。
%加载参考路径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([-60 40]) ylim([10 60]) hScene.;Position = [100 100 1000 500];%调整图标题(“大型停车场”传说)
打开Simulink®模型，并使用helperAddParkedVehicle函数。
modelName =“GenerateLidarDataOfParkingLot”;open_system (modelName) snapnow helperAddParkedVehicles (modelName parkedPoses)
记录和可视化数据
使用带有地面跟踪的模拟3D车辆(自动驾驶工具箱)Block，模拟车辆沿指定参考轨迹行驶。使用模拟3 d激光雷达(自动驾驶工具箱)块在车辆车顶中心安装激光雷达，并记录传感器数据。
关上(hScene)如果~ ispc错误(“虚幻引擎模拟只支持微软”+ char (174) +“Windows”+ char (174) +“。”);结束%运行仿真simOut = sim (modelName);close_system (modelName, 0)
使用helperGetPointClouds函数和helperGetLidarGroundTruth提取激光雷达数据和地面真实pose的函数。
ptCloudArr = helperGetPointClouds (simOut);groundTruthPosesLidar = helperGetLidarGroundTruth (simOut);
检测边缘点(Edge Points)和表面点(Surface Points)
LOAM算法使用边缘点和曲面点进行配准和映射。的detectLOAMFeatures函数输出一个LOAMPoints对象，它存储所选的边缘点和曲面点。它包括每个点的标签，可以是锋利边缘、不那么锋利边缘、平面表面或少平面表面。使用pcregisterloam函数注册两个有组织的点云。
ptCloud = ptCloudArr (1);nextPtCloud = ptCloudArr (2);gridStep = 1;tform = pcregisterloam (ptCloud nextPtCloud gridStep);disp (tform)
rigidtform3d with properties: dimension: 3 R: [3×3 single] Translation: [-0.2297 0.0156 -0.0032] A: [4×4 single]
或者，为了更好地控制精度和速度之间的权衡，你可以先检测LOAM特征点，然后执行LOAM注册使用pcregisterloam。在注册LOAM之前，推荐以下步骤:
检测LOAM特征点使用
detectLOAMFeatures函数。下采样较少的平面表面点使用
downsampleLessPlanar对象的功能。此步骤有助于加快使用pcregisterloam函数。
因为检测算法依赖于每个点的邻居来分类边缘点和曲面点，以及在闭塞区域边界上识别不可靠的点，所以不建议在特征点检测之前进行下采样、去噪和去地等预处理步骤。为了从距离传感器较远的数据中去除噪声，并加快配准速度，可以按范围对点云进行过滤。的helperRangeFilter函数在给定指定的圆柱体半径的情况下，选择传感器周围的圆柱体邻域，并排除距离传感器太近、可能包括车辆部分的数据。
egoRadius = 2;cylinderRadius = 30;ptCloud = helperRangeFilter (ptCloud egoRadius cylinderRadius);nextPtCloud = helperRangeFilter (nextPtCloud egoRadius cylinderRadius);图保存在标题(“圆柱附近”) pcshow (ptCloud)视图(2)
方法检测LOAM特征点detectLOAMFeatures函数。调整这个函数需要实证分析。的detectLOAMFeatures名称-值参数提供了注册准确性和速度之间的权衡。为了提高配准的准确性，必须最小化对齐点之间欧氏距离的均方根误差。跟踪并最小化均方根误差输出rmse的pcregisterloam功能，当你增加的值NumRegionsPerLaser,MaxSharpEdgePoints,MaxLessSharpEdgePoints,MaxPlanarSurfacePoints参数的detectLOAMFeatures。
maxPlanarSurfacePoints = 8;点= detectLOAMFeatures (ptCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);nextPoints = detectLOAMFeatures (nextPtCloud MaxPlanarSurfacePoints = MaxPlanarSurfacePoints);图保存在标题(“壤土点”)显示(点,MarkerSize = 12)
[~, rmse] = pcregisterloam(点,nextPoints);disp (rmse)
1.1887
detectLOAMFeatures首先识别锋利的边缘点，不那么锋利的边缘点，以及平面表面点。其余所有不被认为是不可靠点，且曲率值低于阈值的点都被归类为较小平面面点。对较少的平面面点进行下采样，可以在使用时加快配准pcregisterloam。
点= downsampleLessPlanar(点,gridStep);图保存在标题(“减少平面表面点采样后的LOAM点”)显示(点,“MarkerSize”, 12)
使用Lidar Odometry构建地图
LOAM算法由两个主要组件组成，集成在一起计算一个精确的变换:Lidar Odometry和Lidar Mapping。使用pcregisterloam函数与一对一匹配方法，使用激光雷达里程计算法得到估计的变换。一对一匹配方法将每个点与其最近的邻居进行匹配，将边缘点与边缘点进行匹配，将曲面点与曲面点进行匹配。利用这些匹配来计算变换的估计值。使用pcregisterloam用一对一匹配的方法增量构建停车场地图。使用一个pcviewset对象来管理数据。
初始化姿态和点云视图集。
absPose = groundTruthPosesLidar (1);relPose = rigidtform3d;vSetOdometry = pcviewset;
将第一个视图添加到视图集。
viewId = 1;vSetOdometry = addView (vSetOdometry viewId absPose);
增量注册点云，可视化车辆在停车场场景中的位置。
%用参考轨迹显示停车场场景hScene图(Name = =“大型停车场”NumberTitle =“关闭”);helperShowSceneImage (sceneName);持有在plot(refPosesX(:，2)，refPosesY(:，2)，LineWidth=2) xlim([-60 40]) ylim([10 60]) hScene. .Position = [100 100 1000 500];numSkip = 5;为k = (numSkip+1)+1:numSkip:numel(ptCloudArr) prevPoints = points;viewId = viewId + 1;ptCloud = ptCloudArr (k);%对点云应用范围过滤器ptCloud = helperRangeFilter (ptCloud egoRadius cylinderRadius);%检测LOAM点，并下采样较少的平面表面点点= 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 ([-60 40]) ylim([10 60]) title(“使用激光雷达里程计构建地图”)传说([“地面实况”,“估计位置”])暂停(0.001)视图(2)结束
使用Lidar Mapping提高地图的准确性
激光雷达映射(Lidar Mapping)通过在激光扫描的点和包含多次激光扫描的局部地图的点之间进行配准，细化了来自激光雷达里程计的姿态估计。它将每个点与局部地图中的多个最近邻居进行匹配，然后使用这些匹配细化来自激光雷达里程计的转换估计。使用pcmaploam对象来管理地图中的点。改进从激光雷达里程计的姿态估计使用findPose，并在地图上添加点使用addPoints。
初始化动作。
absPose = groundTruthPosesLidar (1);relPose = rigidtform3d;vSetMapping = pcviewset;
在第一个点云中检测LOAM点。
ptCloud = ptCloudArr (1);ptCloud = helperRangeFilter (ptCloud egoRadius cylinderRadius);点= detectLOAMFeatures (ptCloud,“MaxPlanarSurfacePoints”, maxPlanarSurfacePoints);点= downsampleLessPlanar(点,gridStep);
将第一个视图添加到视图集。
viewId = 1;vSetMapping = addView (vSetMapping viewId absPose);
创建一个地图使用pcmaploam类，并向地图添加点使用addPoints对象的函数pcmaploam。
voxelSize = 0.5;loamMap = pcmaploam (voxelSize);addPoints (loamMap点,absPose);
使用pcregisterloam与一对一匹配的方法，使用激光雷达里程计(Lidar Odometry)获得估计的姿势。然后,使用findPose对象的函数pcmaploam找到使点与地图上的点对齐的绝对姿势。
%用参考轨迹显示停车场场景hScene图(Name = =“大型停车场”NumberTitle =“关闭”);helperShowSceneImage (sceneName);持有在plot(refPosesX(:，2)，refPosesY(:，2)，LineWidth=2) xlim([-60 40]) ylim([10 60]) hScene. .Position = [100 100 1000 500];numSkip = 5;为k = (numSkip+1)+1:numSkip: nummel (ptCloudArr) prevPtCloud = ptCloud;prevPoints =点;viewId = viewId + 1;ptCloud = ptCloudArr (k);%对点云应用范围过滤器ptCloud = helperRangeFilter (ptCloud egoRadius cylinderRadius);%检测LOAM点，并下采样较少的平面表面点点= 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([-60 40]) ylim([10 60]) title(“使用激光雷达绘制地图”)传说([“地面实况”,“估计位置”])暂停(0.001)视图(2)结束
比较结果
将估计的轨迹形象化，并将其与地面真实情况进行比较。注意，结合Lidar Odometry和Lidar Mapping可以得到更精确的地图。
图绘制(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] Zhang, Ji，和Sanjiv Singh。“LOAM:实时激光雷达里程计和测绘。”在机器人:科学与系统X。机器人:科学与系统基金会，2014。https://doi.org/10.15607/RSS.2014.X.007。
支持功能
helperGetPointClouds提取一个数组。pointCloud包含激光雷达传感器数据的对象。
函数ptCloudArr = helperGetPointClouds (simOut)%提取信号ptCloudData = simOut.ptCloudData.signals.values;%创建一个pointCloud数组ptCloudArr = pointCloud (ptCloudData (::,:, 2));%忽略第一帧为n = 3:size(ptCloudData,4) ptCloudArr(end+1) = pointCloud(ptCloudData(:，:，:，n));% #好< AGROW >结束结束
helperGetLidarGroundTruth提取一个数组。rigidtform3d包含ground truth位置和方向的对象。
函数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(:,:我));%忽略滚动和俯仰旋转，因为地面是平的偏航=双(simOut.lidarOrientation.signals.values(:, 3,我));gTruth(张)。R = [cos(偏航)-sin(偏航)0;sin(偏航)因为(偏航)0;0 0 1);结束结束
helperRangeFilter按范围过滤点云。
函数ptCloud = helperRangeFilter (ptCloud egoRadius cylinderRadius)%计算每个点到原点的距离dist =函数(ptCloud.Location (:,: 1), ptCloud.Location (:,: 2));%选择圆筒半径内和自我半径外的点汽缸idx = dist <=汽缸半径& dist >= egoRadius;ptCloud =选择(ptCloud cylinderIdx OutputSize =“全部”);结束
您也可以从以下列表中选择一个网站: