利用SLAM技术从激光雷达数据中构建地图
本示例展示了如何处理安装在车辆上的传感器的三维激光雷达数据，以逐步构建地图，并使用同步定位和测绘(SLAM)估计车辆的轨迹。除了3d激光雷达数据，惯性导航传感器(INS)也被用来帮助建立地图。以这种方式构建的地图可以促进车辆导航的路径规划，也可以用于定位。
概述
的建立一个地图从激光雷达数据(自动驾驶工具箱)example使用三维激光雷达数据和IMU读数逐步构建车辆所经过的环境地图。虽然这种方法构建了本地一致的映射，但它只适用于映射小区域。在较长的序列中，漂移累积成一个显著的错误。为了克服这个限制，本示例识别以前访问过的地方，并尝试使用图SLAM方法纠正累积漂移。
加载和探索记录的数据
控件中使用的数据是Velodyne Slam DataSet.，并且表示接近6分钟的记录数据。将数据下载到临时目录。
笔记：这个下载可能需要几分钟。
BrienceOdloadURL =.'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip';datafolder = fullfile（tempdir，'kit_velodyneslam_data_scenario1', filesep);选择= weboptions (“超时”、正);zipFileName = dataFolder +“scenario1.zip”;folderExists =存在(dataFolder,'dir');如果~ folderExists％在临时目录中创建一个文件夹以保存下载的zip％ 文件。mkdir (dataFolder);disp (下载scenario .zip (153 MB)…) websave(zipFileName, baseDownloadURL, options);%解压下载的文件解压缩(zipFileName dataFolder);结束
使用helperReadDataset函数从创建的文件夹中读取数据时间表．LIDAR捕获的点云以PNG图像文件的形式存储。提取点云文件名列表pointCloudTable多变的。要从图像文件中读取点云数据，请使用HelperReadPointCloudFromFile.函数。此函数拍摄图像文件名并返回apointcloud.对象。INS读数直接从配置文件中读取，并存储在insDataTable多变的。
DataSettable = HelperReadDataset（DataFolder）;pointcloudtable = datasettable（：，1）;INSDATATABLE = DATASETTABLE（:,2：结束）;
读取第一个点云并在MATLAB®命令提示符中显示它
ptcloud = helperReadPointCloudFromFile（PointCloudTable.PointCloudFileName {1}）;DISP（PTCLOUD）
pointCloud with properties: Location: [64×870×3 single] Count: 55680 XLimits: [-78.4980 77.7062] YLimits: [-76.8795 74.7502] ZLimits:[-2.4839 2.6836]颜色:[]Normal:[]强度:[]
显示第一次读取。的时间表拥有标题，沥青，卷，X，Y,Z来自INS的信息。
DISP（INSDATATABLATE（1，:)）
时间戳向距辊X Y Z ____________________ _______ ________ _________ _______ _______ ______ 截止2010年6月13 06:27:31 1.9154 0.007438 -0.019888 -2889.1 -2183.9 116.47
使用可视化的点云PCPlayer.，一个流点云显示。车辆穿过一条由两个环路组成的道路。在第一个循环中，飞行器进行了一系列转弯并返回起点。在第二个循环中，车辆沿着另一条路线进行一系列转弯，然后再次返回起点。
%指定玩家的限制Xlimits = [-45 45];％仪表Ylimits = [-45 45];Zlimits = [-10 20];%创建一个流点云显示对象lidarPlayer = pcplayer(xlimits, ylimits, zlimits);％自定义播放器轴标签包含(lidarPlayer。轴,'x（m）') ylabel (lidarPlayer。轴,'y（m）') zlabel (lidarPlayer。轴,'z（m）'）标题（LidarPlayer.axes，'LIDAR传感器数据'）%跳过其他帧，因为这是一个很长的序列skipFrames = 2;numFrames =身高(pointCloudTable);为n = 1：skipframes：numframes%读取点云文件名= pointCloudTable.PointCloudFileName {n};ptCloud = helperReadPointCloudFromFile(文件名);%可视化点云查看（Lidarplayer，Ptcloud）;暂停（0.01）结束
使用里程表绘制地图
首先，使用该方法解释建立一个地图从激光雷达数据(自动驾驶工具箱)构建映射的示例。该方法包括以下步骤:
结合激光雷达扫描:使用点云配准技术对齐连续的激光雷达扫描。这个示例使用
Pcregisterndt.注册扫描。通过依次组合这些变换，每个点云被转换回第一个点云的参考坐标系。结合对齐扫描:生成一个地图结合所有的变换点云。
递增地构建地图和估计车辆轨迹的这种方法被称为车辆的轨迹测程法．
使用一个pcviewset对象存储和管理多个视图的数据。视图集包括一组连接的视图。
每个视图存储与单个视图关联的信息。这些信息包括视图的绝对姿态、在该视图上捕获的点云传感器数据以及视图的唯一标识符。使用。向视图集添加视图
addView．建立视图之间的连接使用
addConnection．连接存储的信息包括连接视图之间的相对转换、计算此度量所涉及的不确定性(以信息矩阵表示)以及相关的视图标识符。使用
情节用于可视化视图集建立的连接的方法。这些连接可用于可视化车辆遍历的路径。
隐藏(lidarPlayer)%设置随机种子以确保再现性RNG（0）;％创建一个空视图集vset = pcviewset;％为View Set Display创建一个图形hFigBefore =图(“名字”，'查看Set Display');HaxBefore =轴（HFIGBEFORE）;%初始化点云处理参数DownSamplePercent = 0.1;reggridsize = 3;%初始化转换absTform = rigid3d;％绝对变换为参考帧Reltform = Rigid3D;连续扫描之间的相对变换viewId = 1;skipFrames = 5;numFrames =身高(pointCloudTable);displayRate = 100;％更新每100帧显示每100帧为n = 1：skipframes：numframes％读点云文件名= pointCloudTable.PointCloudFileName {n};ptcloudorig = helperReadPointCloudFromFile（文件名）;%工艺点云％ - 段和去除地面平面% -分割和移除自我车辆ptcloud = helperprocesspointpointcloud（ptcloudorig）;％拆下加工点云ptcloud = pcdownsample（ptcloud，“随机的”, downsamplePercent);firstframe =（n == 1）;如果firstFrame%将第一个点云扫描作为视图添加到视图集vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续;结束％使用INS估计注册的初始转换initTform = helperComputeInitialEstimateFromINS (relTform,......INSDATATABLE（n-skipframes：n，:)）;%计算注册当前点云的刚性转换％前一点云relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，......“InitialTransform”, initTform);％更新到参考帧的绝对变换（第一点云）abstform = rigid3d（Reltform.t * abstform.t）;%将当前点云扫描作为视图添加到视图集vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);添加一个从前一个视图到当前视图的连接，表示%它们之间的相对变换vSet = addConnection(vSet, viewid1, viewId, relTform);viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n>1 && mod(n, displayRate) == 1 plot(vSet，“父母”, hAxBefore);drawnow更新结束结束
视图集对象vSet，现在持有视图和连接。在vSet的Views表中，有AbsolutePose变量指定每个视图相对于第一个视图的绝对姿态。在连接表格vSet,RelativePose变量指定连接视图之间的相对约束InformationMatrix变量为每条边指定与连接相关的不确定性。
％显示前几种视图和连接头（vset.views）head（vset.Connections）
ANS = 8×3表查看Absolutepose PointCloud ______ _____________ ___________________ [1×1 ridid3d] [1×1 pifid3d] 2 [1×1 rigid3d] [1×1 pifid3d] 3 [1×1 rigid3d] [1×1 pixcloud] 4 [1×1 rigid3d] [1×1 pointcloud] 5 [1×1 rigid3d] [1×1 pointcloud] 6 [1×1 rigid3d] [1×1 pointcloud] 7 [1×1 rigid3d] [1×1 pointcloud] 8 [1×1 rigid3d] [1×1 pointcloud] ans = 8×4表viewid1ippid2 actionsidematrix _______ _________________________ 1 2 [1×1 ridid3d] {6×6 double} 2 3 [1×1ridid3d] {6×6 double} 3 4 [1×1 ridid3d] {6×6双} 4 5 [1×1 ridid3d] {6×6 double} 5 6 [1×1 ridid3d] {6×6双}6 7 [1×1 rigid3d] {6×6双} 7 8 [1×1 rigid3d] {6×6双} 8 9 [1×1 rigid3d] {6×6双}
现在，使用创建的视图集构建点云图。对齐视图绝对姿态与视图集中的点云使用pcalign.．指定网格尺寸以控制地图的分辨率。地图作为一个返回pointcloud.对象。
ptClouds = vSet.Views.PointCloud;absPoses = vSet.Views.AbsolutePose;mapGridSize = 0.2;ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);
请注意，使用此方法所遍历的路径会随时间而漂移。虽然沿着第一个循环回到起点的路径似乎是合理的，但第二个循环却明显偏离了起点。累积的漂移导致第二个回路在离起点几米远的地方终止。
单独使用Odometry建造的地图是不准确的。使用遍历路径显示构建的点云映射。请注意，第二循环的地图和遍历路径与第一个循环不一致。
举行（哈克斯特，“上”);pcshow (ptCloudMap);举行（哈克斯特，“关闭”);关上(hAxBefore.Parent)
使用姿势图优化正确漂移
图大满贯是一种广泛使用的技术，用于解决内径型漂移。图形SLAM方法递增地创建图形，其中节点对应于车辆姿势，边缘表示限制连接的姿势的传感器测量。这样的图表被称为a姿态图．由于噪声或测量不准确，姿态图包含编码矛盾信息的边。然后优化构造图中的节点，以找到最优解释测量的车辆姿态集。这种技术叫做构成图优化．
要从视图集中创建姿态图，可以使用createPoseGraph函数。这个函数为每个视图创建一个节点，为视图集中的每个连接创建一条边。要优化姿态图，您可以使用optimizePoseGraph(导航工具箱)函数。
有助于图形漂移在校正漂移中的有效性的关键方面是准确地检测环，即先前访问的位置。这就是所谓的环路闭合检测或者位置识别．向对应于环路闭合的姿势图添加边缘为连接的节点姿势提供相互矛盾的测量，这可以在姿势图优化期间解决。
环路闭包可以使用描述符来检测局部环境对激光雷达传感器可见。的扫描环境描述符[1]就是这样一种描述符，可以使用scanContextDescriptor函数。此示例使用helperLoopClosureDetector类来检测循环闭包。该类使用扫描上下文描述符描述单个点云，并使用两阶段描述符搜索算法。
描述符计算：方法从每个点云中提取扫描上下文描述符和环键
scanContextDescriptor函数。描述符匹配:在第一阶段，环键用于找到潜在的环路候选。在第二阶段中，使用潜在循环候选中的扫描上下文特征描述符来执行最近的邻居搜索。的
scanContextDistance函数用于计算两个扫描上下文描述符之间的距离。看到helperFeatureSearcher类获取更多细节。
%设置随机种子以确保再现性RNG（0）;％创建一个空视图集vset = pcviewset;%创建一个循环闭包检测器matchtresh = 0.08;LoopDetector = HelperloopClosuredetector（'matchthreshold', matchThresh);％为View Set Display创建一个图形hFigBefore =图(“名字”，'查看Set Display');HaxBefore =轴（HFIGBEFORE）;%初始化转换absTform = rigid3d;％绝对变换为参考帧Reltform = Rigid3D;连续扫描之间的相对变换maxTolerableRMSE = 3;允许一个循环闭包候选的最大RMSE值viewId = 1;为n = 1：skipframes：numframes％读点云文件名= pointCloudTable.PointCloudFileName {n};ptcloudorig = helperReadPointCloudFromFile（文件名）;%工艺点云％ - 段和去除地面平面% -分割和移除自我车辆ptcloud = helperprocesspointpointcloud（ptcloudorig）;％拆下加工点云ptcloud = pcdownsample（ptcloud，“随机的”, downsamplePercent);firstframe =（n == 1）;如果firstFrame%将第一个点云扫描作为视图添加到视图集vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续;结束％使用INS估计注册的初始转换initTform = helperComputeInitialEstimateFromINS (relTform,......INSDATATABLE（n-skipframes：n，:)）;%计算注册当前点云的刚性转换％前一点云relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize，......“InitialTransform”, initTform);％更新到参考帧的绝对变换（第一点云）abstform = rigid3d（Reltform.t * abstform.t）;%将当前点云扫描作为视图添加到视图集vSet = addView(vSet, viewId, absTform，“PointCloud”, ptCloudOrig);％从上一个视图中添加连接到当前视图%它们之间的相对变换vSet = addConnection(vSet, viewid1, viewId, relTform);％检测循环闭合候选[loopFound, loopViewId] = detectLoop(loopDetector, ptCloudOrig);%找到一个循环候选项如果loopFound loopViewId = loopViewId(1);从查看集中检索点云ptCloudOrig = vSet.Views。PointCloud(找到(vSet.Views。ViewId == loopViewId, 1));%工艺点云ptcloudold = helperprocesspointcloud（ptcloudorig）;％倒下点云ptcloudold = pcdownsample（ptcloudold，“随机的”, downsamplePercent);%使用配准估计相对姿态[RELTFORM，〜，RMSE] = PCREGISTERNDT（PTCLOUD，PTCLOUDOLD，......regGridSize,“MaxIterations”, 50);acceptLoopClosure = rmse <= maxTolerableRMSE;如果acceptLoopClosure％为简单起见，使用常数，小信息矩阵%循环闭合边Infomat = 0.01 *眼睛（6）;%添加一个与循环闭包对应的连接vSet = addConnection(vSet, loopViewId, viewId, relTform, infoMat);结束结束viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n> 1 && mod（n，displayrate）== 1 hg = plot（vset，“父母”, hAxBefore);drawnow更新结束结束
属性从视图集中创建姿态图createPoseGraph方法。姿态图是有向图对象:
包含每个视图的绝对姿态的节点
包含每个连接的相对姿态约束的边
G = createPoseGraph (vSet);disp (G)
带属性的有向图:Edges: [507×3 table] Nodes: [503×2 table]
除了连续视图之间的里程表连接之外，视图集现在还包括循环闭包连接。例如，请注意第二个循环遍历和第一个循环遍历之间的新连接。这些是循环闭包连接。这些边可以被识别为图中结束节点不是连续的边。
%更新轴限制聚焦于循环闭包连接xlim (hAxBefore 50 [-50]);ylim (hAxBefore 50 [-100]);％查找和突出显示循环闭合连接LoopEdgeDIDS =查找（ABS（Diff（G.Edges.Endnodes，1,2））> 1）;亮点（Hg，“边缘”，loopededids，“EdgeColor”，“红色”，“线宽”3)
优化姿态图形使用optimizePoseGraph．
optimG = optimizePoseGraph (G,'G2O-Levenberg-Marquardt');vSetOptim = updateView(vSet, optimG.Nodes);
使用优化的姿势显示视图集。请注意，现在已被检测到的循环合并，导致更准确的轨迹。
情节(vSetOptim“父”hAxBefore)
Optimized View集中的绝对姿势现在可以用于构建更准确的地图。使用pcalign.使用优化的视图集绝对位于单点云映射，将视图设定点云对齐。指定网格尺寸以控制创建的点云映射的分辨率。
mapGridSize = 0.2;ptClouds = vSetOptim.Views.PointCloud;absPoses = vSetOptim.Views.AbsolutePose;ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);hFigAfter =图(“名字”，'查看设置显示（优化后）');haxafter =轴（hfigafter）;pcshow（ptcloudmap，“父”，haxafter）;%覆盖视图集显示持有上情节(vSetOptim“父”，haxafter）;HelpermakeFigurepublishfriendly（HFigafter）;
虽然精度仍然可以提高，这一点云图是明显更准确。
参考
G. Kim和A. Kim，“扫描情境:三维点云图中位置识别的自中心空间描述符”，2018 IEEE / RSJ智能机器人和系统国际会议（IROS），马德里，2018年，第4802-4809页。
金宝app支持功能和课程
helperReadDataset将指定文件夹中的数据读入时间表。
函数DataSettable = HelperReadDataset（DataFolder）%helperReadDataset读取Velodyne SLAM数据到时间表% datasetTable = helperReadDataset(dataFolder)从将dataFolder中指定的%文件夹指定为时间表。这个函数％期望来自Velodyne Slam数据集的数据。％%参见fileDatastore, helperReadINSConfigFile。%点云存储为PNG文件在scenario1文件夹pointcloudfilepattern = fullfile（datafolder，“scenario1”，“扫描* . png”);%创建一个文件数据存储，以正确的顺序读取文件提起= fileDatastore (pointCloudFilePattern,“ReadFcn”，......@HelperReadPointCloudFromFile）;从数据存储中提取文件列表pointcloudfiles = fileds.files;imuconfigfile = fullfile（datafolder，“scenario1”，“imu.cfg”);insDataTable = helperReadINSConfigFile (imuConfigFile);％从ins config文件中删除坏行InsdataTable（1447，:) = [];％删除不会使用的列datasetTable = removevars (insDataTable,......{'num_satellites'，“纬度”，“经”，“高度”，“Omega_Heading”，......“Omega_Pitch”，“Omega_Roll”，“V_X”，'v_y'，'v_zdown'}）;DataSettable = addVars（DataSettable，PointCloudFiles，“之前”, 1......“NewVariableNames”，“PointCloudFileName”);结束
helperProcessPointCloud通过移除属于地平面和自我车辆的点来处理点云。
函数ptCloud = helperProcessPointCloud(ptCloudIn，方法)％HelperProcessPointPointCloud处理PointCloud以拆下地面和自助式车辆% ptCloud = helperProcessPointCloud(ptCloudIn, method) processes% ptCloudIn通过移除地平面和自我载体。%方法可以是“planefit”或“rangeflodfill”。％%参见pcfitplane, pointCloud/findNeighborsInRadius。争论ptCloudIn(1,1) pointCloud方法字符串{mustBeMember(方法,[“planefit”、“rangefloodfill”])}＝“rangefloodfill”结束isOrganized =〜ismatrix（ptcloudin.location）;如果(方法= =“rangefloodfill”&& ISONORGANIZED）%在距离图像上使用洪水填充分割地面groundFixedIdx = segmentGroundFromLidarData (ptCloudIn,......“Elevationangledelta”11）;其他的%以参考法线分割地面为主导平面％矢量指向正z方向maxdistance = 0.4;maxangulardistance = 5;参考矢量= [0 0 1];[〜，proundfixedidx] = pcfitplane（ptcloudin，maxdistance，......referenceVector maxAngularDistance);结束如果isOrganized groundFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的proundfixed = false（ptcloudin.count，1）;结束groundFixed (groundFixedIdx) = true;将自我车辆分割为传感器给定半径内的点传感器位置= [0 0 0];半径= 3.5;egoFixedIdx = findNeighborsInRadius(ptCloudIn, sensorLocation, radius);如果IsOrganized Egofixed = false（大小（ptcloudin.location，1），size（ptcloudin.location，2））;其他的(ptCloudIn egoFixed = false。统计,1);结束egoFixed (egoFixedIdx) = true;%保留点云子集，没有地面和自我车辆如果isOrganized indices = ~groundFixed & ~egoFixed;其他的索引=查找（〜〜ProundFixed＆〜Egofixed）;结束ptCloud = select(ptCloudIn, indices);结束
HelpercomputeInitialestimateFromins.根据INS的读数估计注册的初始转换。
函数initTform = helperComputeInitialEstimateFromINS(initTform, insData)%如果没有可用的INS读数，返回如果isempty（insdata）返回;结束在INS读数中，X指向前面，Y指向左边%和Z向上。下面的翻译说明了转换为激光雷达%框架。insToLidarOffset = [0 -0.79 -1.73];%见DATAFORMAT.txtTnow = [-insData.Y(end)， insData.X(end)， insData.Z(end)]。' + insToLidarOffset ';Tbef = [-insData.Y (1) insData.X (1) insData.Z(1)]。' + insToLidarOffset ';由于车辆要在地面上移动，所以滚动发生了变化％和间距最小。忽略滚动和音调的变化，仅使用标题。Rnow = rotmat([insData.Heading(end) 0 0]，“欧拉”，“ZYX股票”，“点”)，“点”);Rbef = rotmat([insData.Heading(1) 0 0]，“欧拉”，“ZYX股票”，“点”)，“点”);t = [rbef tbef; 0 0 0 1] \ [rnow tnow; 0 0 0 0 1];inittform = rigid3d（t。'）;结束
helperMakeFigurePublishFriendly调整数字，以便发布的截图是正确的。
函数HelpermakeFigurepublishFriendly（HFIG）如果~isempty(hFig) &&是有效的(hFig) hFig。HandleVisibility =“回调”;结束结束
helperFeatureSearcher创建一个对象，可用于搜索最接近的特性匹配。
helperLoopClosureDetector创建一个对象，该对象可使用扫描上下文特性描述符检测循环闭包。
另请参阅
功能
对象
相关话题
- 建立一个地图从激光雷达数据
- 激光雷达探测地面和障碍物(自动驾驶工具箱)
外部网站
你也可以从以下列表中选择一个网站:
