建立从激光雷达数据使用SLAM一个地图
这个例子显示了激光雷达安装在车辆上从传感器数据以逐步建立一个地图，并估计使用同时定位和地图创建（SLAM）的车辆的轨迹如何处理3- d。除了3-d激光雷达数据，惯性导航传感器（INS）也被用来帮助建立地图。地图建这样可以方便路径规划车辆导航或可用于定位。
概观
该从激光雷达数据建立一个地图示例使用3-d激光雷达数据和IMU读数逐步建立地图上由车辆经过的环境的。虽然这种方法建立了一个一致的本地地图，它仅适用于映射小块区域。在较长序列，漂移积累到显著错误。为了克服这个限制，这个实例认识以前访问过的地方，并试图正确使用图形SLAM方法的积累漂移。
加载并查看记录的数据
在本实施例中使用的数据是部分调速发电机大满贯数据集和表示接近到6分钟记录的数据。将数据下载到一个临时目录。
注意:这种下载可能需要几分钟的时间。
baseDownloadURL ='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，“目录”);如果~ folderExists在临时目录中创建文件夹以保存下载的zip文件％文件。MKDIR（dataFolder）;DISP（'下载scenario1.zip（153 MB）......') websave(zipFileName, baseDownloadURL, options);%解压缩下载的文件解压缩(zipFileName dataFolder);结束
使用helperReadDataset函数从创建的文件夹中以a的形式读取数据时间表。通过激光雷达捕获的点云存储在PNG图像文件的形式。提取点云的文件名列表pointCloudTable变量。要读取的图像文件的点云数据，使用helperReadPointCloudFromFile函数。此函数接受图像文件名并返回点云对象。的INS读数直接从配置文件中读取，并存储在insDataTable变量。
datasetTable = helperReadDataset (dataFolder);pointCloudTable = datasetTable(:， 1);insDataTable = datasetTable(:， 2:end);
读取的第一点云和在MATLAB®命令提示显示它
ptCloud = helperReadPointCloudFromFile (pointCloudTable.PointCloudFileName {1});disp (ptCloud)
点云具有属性：位置：64×870×3单一]颜色：[]正常：[]强度：[]数：55680个XLimits：-78.4980 77.7062] YLimits：-76.8795 74.7502] ZLimits：[-2.4839 2.6836]
显示国际新闻社的第一次阅读。该时间表持有标题，沥青，滚，X，ÿ,ž来自国际新闻社的消息。
DISP（insDataTable（1，:)）
时间戳标题节距滚X Y Z ^ ____________________ _______ ________ _________ _______ _______ ______ 6月13日2010 6时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）;%自定义播放器轴标签xlabel（lidarPlayer.Axes，“X (m)”)ylabel (lidarPlayer.Axes'Y（M）')zlabel (lidarPlayer.Axes“Z (m)”)标题(lidarPlayer.Axes“激光雷达传感器数据”）因为这是一个很长的序列，所以每一帧都要跳过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;创建视图集显示的图形hFigBefore =系数（'名称'，“查看设置显示”);hAxBefore =轴（hFigBefore）;初始化点云处理参数downsamplePercent = 0.1;regGridSize = 3;％初始化转换absTform = rigid3d;％绝对变换到参考帧relTform = rigid3d;％连续的扫描之间的相对变viewId = 1;skipFrames = 5;numFrames =身高(pointCloudTable);displayRate = 100;％更新显示每100帧对于n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {N};ptCloudOrig = helperReadPointCloudFromFile（文件名）;%流程点云分段并移去接地面％ - 段和删除自车辆ptCloud = helperProcessPointCloud (ptCloudOrig);％下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”，downsamplePercent）;firstFrame =（N == 1）;如果firstFrame％添加第一点云扫描作为视图的视图集VSET = addView（VSET，viewId，absTform，“点云”,ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续;结束使用INS来估计注册的初始转换initTform = helperComputeInitialEstimateFromINS（relTform，...insDataTable（正skipFrames：N，:)）;％计算刚性变换，其登记与当前点云%先前点云relTform = pcregisterndt（ptCloud，ptCloudPrev，regGridSize，...“InitialTransform”,initTform);更新绝对转换为参考帧(第一点云)absTform = rigid3d(relTform)。T * absTform。T);将当前点云扫描作为视图添加到视图集VSET = addView（VSET，viewId，absTform，“点云”,ptCloudOrig);％添加从前一视图到当前视图的连接，表示它们之间的相对变换VSET = addConnection（VSET，viewId-1，viewId，relTform）;viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果N> 1个&& MOD（N，displayRate）== 1个图（VSET，“父”，hAxBefore）;的DrawNow更新结束结束
视图集对象VSET，现在持有的观点和连接。在VSET的的意见表AbsolutePose变量指定每个视图相对于第一个视图的绝对位姿。在连接表VSET中，RelativePose变量指定连接视图之间的相对约束InformationMatrix变量指定，对于每个边缘，所述不确定性与连接相关联。
％显示前几个视图和连接头(vSet.Views)头(vSet.Connections)
ans = 8×3表ViewId AbsolutePose PointCloud ______ _________________ ___________ 1 [1×1 rigid3d] [1×1 PointCloud] 2 [1×1 rigid3d] [1×1 PointCloud] 3 [1×1 rigid3d] [1×1 PointCloud] 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表ViewId1 ViewId2 RelativePose InformationMatrix ____ ____ _________________ _________________ 1 2 [1×1 rigid3d]{6×6双}2 3 [1×14[1×1 rigid3d]{6×6 double} 3 4[1×1 rigid3d]{6×6 double} 4 5[1×1 rigid3d]{6×6 double} 5 6[1×1 rigid3d]{6×6 double} 6 7[1×6 double} 7 8[1×1 rigid3d]{6×6 double} 9[1×1 rigid3d]{6×6 double}
现在，通过使用创建的视图集制作地图helperBuildMapFromViewset助手功能。指定网格尺寸控制地图的分辨率。地图返回为一个点云对象。
mapGridSize = 0.2;ptCloudMap = helperBuildMapFromViewset（VSET，mapGridSize）;
请注意，路径使用这种方法遍历随时间漂移。而沿着第一循环回到起点的路径似乎是合理的，第二个循环显著从起点漂移。累计漂移结果在第二循环中从起点终止数米之遥。
单独使用里程表绘制的地图是不准确的。显示经过遍历的路径的构建点云图。注意，第二个循环的映射和遍历路径与第一个循环不一致。
持有(hAxBefore'上');pcshow（ptCloudMap）;持有(hAxBefore“关”);关上(hAxBefore.Parent)
正确使用漂移姿态图形优化
图SLAM是一种广泛应用于测距中解决漂移问题的技术。graph SLAM方法增量地创建一个图形，其中节点对应于车辆姿态，边缘代表传感器测量，限制了连接的姿态。这样的图称为a姿态图形。姿态图包含编码矛盾的信息，由于测量噪声或不准确的边缘。然后在构建的图中的节点被优化以找到所述一组车辆姿势最优地解释测量的。这种技术被称为构成图优化。
若要从视图集的姿态图形，您可以使用createPoseGraph函数。该函数创建每个视图的节点，以及在该视图组中的每个连接的边缘。为了优化姿态图形，您可以使用optimizePoseGraph函数。
有助于图形SLAM的在校正漂移的有效性的关键方面是循环的精确的检测，即，先前访问的已去过的地方。这就是所谓的环路闭合检测或位置识别。在闭合环对应的位姿图上添加边，可以对连接节点的位姿进行矛盾度量，可以在位姿优化过程中解决。
本例使用helperLoopClosureDetector一流的检测环封闭。这个类的用途扫描上下文描述单个点云的特征描述符[1]，以及两阶段特征搜索算法。
特性计算:方法从每个点云中提取扫描上下文特征描述符和环密钥
helperExtractScanContextFeatures函数。特征匹配：在第一阶段，环键是用来寻找潜在循环的候选人。在第二阶段中，使用所述潜在循环候选中的扫描上下文特征描述符执行最近邻搜索。查看
helperFeatureSearcher类的更多细节。
％设定随机种子，以确保可重复性RNG（0）;％创建一个空的视图集vSet = pcviewset;％创建一个环路闭合检测器loopDetector = helperLoopClosureDetector;创建视图集显示的图形hFigBefore =系数（'名称'，“查看设置显示”);hAxBefore =轴（hFigBefore）;％初始化转换absTform = rigid3d;％绝对变换到参考帧relTform = rigid3d;％连续的扫描之间的相对变maxTolerableRMSE = 3;允许接受一个循环闭包候选的RMSE的最大百分比viewId = 1;对于n = 1: skipFrames: numFrames读点云文件名= pointCloudTable.PointCloudFileName {N};ptCloudOrig = helperReadPointCloudFromFile（文件名）;%流程点云分段并移去接地面％ - 段和删除自车辆ptCloud = helperProcessPointCloud (ptCloudOrig);％下采样处理的点云ptCloud = pcdownsample (ptCloud,“随机”，downsamplePercent）;firstFrame =（N == 1）;如果firstFrame％添加第一点云扫描作为视图的视图集VSET = addView（VSET，viewId，absTform，“点云”,ptCloudOrig);viewId = viewId + 1;ptCloudPrev = ptCloud;继续;结束使用INS来估计注册的初始转换initTform = helperComputeInitialEstimateFromINS（relTform，...insDataTable（正skipFrames：N，:)）;％计算刚性变换，其登记与当前点云%先前点云relTform = pcregisterndt（ptCloud，ptCloudPrev，regGridSize，...“InitialTransform”,initTform);更新绝对转换为参考帧(第一点云)absTform = rigid3d(relTform)。T * absTform。T);将当前点云扫描作为视图添加到视图集VSET = addView（VSET，viewId，absTform，“点云”,ptCloudOrig);％添加从前一视图到当前视图表示连接它们之间的相对变换VSET = addConnection（VSET，viewId-1，viewId，relTform）;检测环路闭合候选[loopFound，loopViewId] = detectLoop（loopDetector，ptCloudOrig）;％A循环候选被发现如果loopFound loopViewId = loopViewId（1）;从视图集检索点云ptCloudOrig = vSet.Views.PointCloud（FIND（vSet.Views.ViewId == loopViewId，1））;%流程点云ptCloudOld = helperProcessPointCloud（ptCloudOrig）;%下采样点云ptCloudOld = pcdownsample（ptCloudOld，“随机”，downsamplePercent）;％用途登记估计相对姿态[relTform，〜，RMSE = pcregisterndt（ptCloudOld，ptCloud，...regGridSize,'MaxIterations',50);acceptLoopClosure = rmse <= maxtolerance ablermse;如果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 =情节（VSET，“父”，hAxBefore）;的DrawNow更新结束结束
通过创建视图集的姿态图形createPoseGraph方法。该姿态图形是有向图对象有：
包含每个视图的绝对位姿的节点
包含每个连接的相对姿势约束边缘
G = createPoseGraph (vSet);disp (G)
属性有向图:边:[514×3表]节点:[503×2表]
除了连续视图之间的里程计连接，该视图组现在包括环路闭合连接。例如，请注意第二环路遍历和第一环路遍历之间的新的连接。这些环路闭合连接。这些可以被识别为在其端部的节点不连续的曲线边缘。
％更新轴线上环路闭合连接限制焦点XLIM（hAxBefore，[-50 50]）;ylim（hAxBefore，[-100 50]）;％找到并突出显示的环闭合连接loopEdgeIds =找到(abs (diff (G.Edges。EndNodes, 1, 2)) > 1);突出(hG,“边缘”，loopEdgeIds，'EdgeColor'，'红色'，'行宽'，3）
使用。优化视图中设置的姿态optimizePoseGraph函数。
%优化位姿图Goptim = optimizePoseGraph（G，'g2o-文伯格 - 马夸特');%更新视图设置与优化的姿态vSetOptim = updateView(vSet, goptimt . nodes);
显示设置了优化姿态的视图。请注意，检测到的循环现在被合并，从而得到更准确的轨迹。
图（vSetOptim，“父”，hAxBefore）
在优化的视图集的绝对姿态现在可以用来建立一个更准确的地图。
mapGridSize = 0.2;ptCloudMap = helperBuildMapFromViewset(vSetOptim, mapGridSize);hFigAfter =图('名称'，“视图集显示(优化后)”);hAxAfter =轴（hFigAfter）;pcshow（ptCloudMap，“父”,hAxAfter);％重叠视图组显示保持在图（vSetOptim，“父”,hAxAfter);helperMakeFigurePublishFriendly (hFigAfter);
虽然精确度还有待提高，但这张点云图的精确度要高得多。
参考
G. Kim和A.金，“扫描上下文：自我中心的空间描述为将识别在三维点云地图”2018年IEEE/RSJ智能机器人与系统国际会议(IROS)马德里，2018年，第4802-4809。
金宝app支持函数和类
helperReadDataset将指定文件夹中的数据读入时间表。
函数datasetTable = helperReadDataset (dataFolder)％helperReadDataset读Velodyne SLAM数据集的数据到一个时间表％datasetTable = helperReadDataset（dataFolder）读取从数据在数据文件中指定的文件夹。这个函数%期望得到来自Velodyne SLAM数据集的数据。％％参见fileDatastore，helperReadINSConfigFile。％点云存储为PNG在scenario1文件的文件夹pointCloudFilePattern = fullfile (dataFolder,'scenario1'，“扫描* . png”);创建一个文件数据存储，以便按正确的顺序读入文件的Fileds = fileDatastore（pointCloudFilePattern，'ReadFcn'，...@helperReadPointCloudFromFile);％提取从数据存储中的文件列表pointCloudFiles = fileDS.Files;imuConfigFile =完整文件（dataFolder，'scenario1'，'imu.cfg');insDataTable = helperReadINSConfigFile (imuConfigFile);从INS配置文件中删除坏行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，方法）％helperProcessPointCloud过程点云以除去地和自主车辆％ptCloud = helperProcessPointCloud（ptCloudIn，方法）工艺% ptCloudIn通过删除地面和自我车辆。％的方法可以是“planefit”或“rangefloodfill”。％%参见pcfitplane, pointCloud/findNeighborsInRadius。参数ptCloudIn（1,1）点云方法串{mustBeMember（方法，[ “planefit”， “rangefloodfill”]）}=“rangefloodfill”结束isOrganized =〜ismatrix（ptCloudIn.Location）;如果（方法==“rangefloodfill”&& isOrganized）在距离图象上使用洪水填塞的区段地面groundFixedIdx = segmentGroundFromLidarData（ptCloudIn，...“ElevationAngleDelta”11);其他的以线段地面为主导面，参考法线在正z方向％矢量指向maxDistance = 0.4;maxAngularDistance = 5;referenceVector = [0 0 1];[〜，groundFixedIdx] = pcfitplane（ptCloudIn，maxDistance，...referenceVector，maxAngularDistance）;结束如果isOrganized groundFixed = false(size(ptCloudIn.Location,1)，size(ptCloudIn.Location,2));其他的groundFixed =假（ptCloudIn.Count，1）;结束groundFixed（groundFixedIdx）= TRUE;％的区段自身车辆作为传感器的给定半径内的点sensorLocation = [0 0 0];半径= 3.5;findNeighborsInRadius(ptCloudIn, sensorLocation, radius);如果isOrganized egoFixed = FALSE（大小（ptCloudIn.Location，1），大小（ptCloudIn.Location，2））;其他的(ptCloudIn egoFixed = false。统计,1);结束egoFixed (egoFixedIdx) = true;保留点云的子集，没有地面和自我车辆如果isOrganized索引= ~groundFixed & ~egoFixed;其他的索引=查找(~groundFixed & ~egoFixed);结束ptCloud =选择（ptCloudIn，索引）;结束
helperComputeInitialEstimateFromINS从INS读数中估计注册的初始转换。
函数initTform = helperComputeInitialEstimateFromINS（initTform，insData）％如果没有INS读数可用，回报如果isempty (insData)返回;结束％的INS读数设置有X指向前方，Y向左%和Z向上。下面的翻译说明转换成激光雷达％帧。insToLidarOffset = [0 -0.79 -1.73];％的人认为DATAFORMAT.txtTNOW = [-insData.Y（结束），insData.X（结束），insData.Z（结束）]“。+ 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 1];initTform = rigid3d（T'）;结束
helperBuildMapFromViewset构建使用绝对姿态在视图集的意见点云地图。结果地图在由指定的分辨率建gridSize。
函数ptCloudMap = helperBuildMapFromViewset（VSET，gridSize）numViews = vSet.NumViews;从视图集中提取点云视图和绝对转换。ptClouds = vSet.Views.PointCloud;absTforms = vSet.Views.AbsolutePose;％创建点云散乱对于n = 1: numViews ptClouds(n) = removeInvalidPoints(ptClouds(n));结束％预分配图上的点totalNumPoints =总和([ptClouds.Count]);mapPoints = 0 (totalNumPoints, 3，'喜欢'，ptClouds（1）.Location）;pointIndex = 1;对于n = 1时：numViews％变换指向第一点云的参考帧ptCloud = pctransform（ptClouds（n）时，absTforms（N））;％厚积薄发地图点计数= ptCloud.Count;mapPoints（pointIndex：pointIndex +计数-1，:) = ptCloud.Location;pointIndex = pointIndex +计数;结束%下降采样点使用体素网格过滤器的要求的分辨率ptCloudMap = pcdownsample（点云（mapPoints），“gridAverage”，gridSize）;结束
helperMakeFigurePublishFriendly调整数字所以由发布捕获的屏幕截图是正确的。
函数helperMakeFigurePublishFriendly（hFig）如果〜的isEmpty（hFig）&&的isValid（hFig）hFig.HandleVisibility ='打回来';结束结束
helperExtractScanContextFeatures从点云中提取扫描上下文特征。Scan context是一个全局的、以自我为中心的特征描述符，用于位置识别。
helperCompareScanContextFeatures比较从点云扫描上下文特性。
helperFeatureSearcher创建一个对象，该对象可用于搜索最接近的特性匹配。
helperLoopClosureDetector创建一个对象，该对象可用于使用扫描上下文特征描述符检测循环闭包。
也可以看看
职能
对象
相关的话题
外部网站
你也可以从以下列表中选择一个网站:
