建立一个地图使用大满贯从激光雷达数据
这个例子展示了如何处理三维激光雷达数据从一个传感器安装在车辆逐步建立一个地图和估计的轨道车辆使用同步定位和映射(大满贯)。除了三维激光雷达数据,一个惯性导航传感器(INS)也用于帮助构建地图。地图构建的这种方式可以促进车辆路径规划导航或可用于本地化。
概述
的从激光雷达数据建立一个映射示例使用3 d激光雷达数据和IMU数据逐步构建环境地图车辆穿越。虽然这种方法构建一个本地一致的地图,它是只适合小范围的映射。在时间序列,漂移积累到一个重大错误。为了克服这个限制,本例中承认之前访问过的地方,试图纠正使用图大满贯的累积漂移的方法。
加载和探索记录数据
在本例中使用的数据的一部分调速发电机大满贯数据集,代表了近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”;%得到完整的文件路径scenario1 PNG文件的文件夹。pointCloudFilePattern = fullfile (dataFolder,“scenario1”,“扫描* . png”);numExpectedFiles = 2513;folderExists =存在(dataFolder,“dir”);如果~ folderExists%在一个临时目录中创建一个文件夹来保存下载zip%的文件。mkdir (dataFolder);disp (“下载scenario1。邮政编码(153 MB)……”)websave (zipFileName baseDownloadURL选项);%解压下载文件解压缩(zipFileName dataFolder);elseiffolderExists & &元素个数(dir (pointCloudFilePattern)) < numExpectedFiles% Redownload数据如果它减少了临时目录。disp (“下载scenario1。邮政编码(153 MB)……”)websave (zipFileName baseDownloadURL选项);%解压下载文件。解压缩(zipFileName dataFolder)结束
下载scenario1。邮政编码(153 MB)……
使用helperReadDataset函数来读取数据从创建文件夹的形式时间表。被激光雷达点云存储在PNG图像文件的形式。提取点云文件名称的列表pointCloudTable变量。读取图像文件的点云数据,使用helperReadPointCloudFromFile函数。此函数接受一个图像文件名称和返回pointCloud对象。INS的读数是直接从配置文件读取和存储的insDataTable变量。
datasetTable = helperReadDataset (dataFolder pointCloudFilePattern);pointCloudTable = datasetTable (: 1);insDataTable = datasetTable(:, 2:结束);
阅读第一个点云在MATLAB®命令提示符并显示它
ptCloud = helperReadPointCloudFromFile (pointCloudTable.PointCloudFileName {1});disp (ptCloud)
pointCloud属性:位置:(64×870×3个)数:55680 XLimits: [-78.4980 - 77.7062] YLimits: [-76.8795 - 74.7502] ZLimits:[-2.4839 - 2.6836]颜色:正常:[][]强度:[]
显示第一个INS阅读。的时间表持有标题,球场,卷,X,Y,Z来自INS的信息。
:disp (insDataTable (1))
时间戳向距辊X Y Z ____________________累积________ _____ _________ ______ 13 - 2010年6月——06:27:31 1.9154 0.007438 -0.019888 -2889.1 -2183.9 116.47
使用可视化点云pcplayer,流点云显示。两个循环组成的遍历路径。在第一个循环,汽车使一系列转身回到了起点。在第二个循环,汽车使一系列沿着另一条路线,再次回到了起点。
%指定玩家的限制xlimits = 45 [-45];%米ylimits = 45 [-45];zlimits = 20 [-10];%创建一个流点云显示对象lidarPlayer = pcplayer (xlimits ylimits zlimits);%定制球员轴标签包含(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 = rigidtform3d;%绝对参考系变换relTform = rigidtform3d;%相对变换之间的连续扫描viewId = 1;skipFrames = 5;numFrames =身高(pointCloudTable);displayRate = 100;%更新显示每100帧为n = 1: skipFrames: numFrames%读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%处理点云% -段和删除地平面% -段和消除自我ptCloud = helperProcessPointCloud (ptCloudOrig);% Downsample点云处理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 = rigidtform3d (absTform。* relTform。一个);%添加当前点云扫描视图的视图集vSet = addView (vSet viewId absTform,“PointCloud”,ptCloudOrig);%从先前的视图添加一个连接到当前视图中,代表%相对他们之间转换vSet = addConnection (vSet viewId-1、viewId relTform);viewId = viewId + 1;ptCloudPrev = ptCloud;initTform = relTform;如果n > 1 & &国防部(n, displayRate) = = 1情节(vSet,“父”,hAxBefore);drawnow更新结束结束
视图设置对象vSet现在持有的观点和连接。在视图vSet表,AbsolutePose变量指定的绝对姿态每个视图的第一个观点。在连接表vSet,RelativePose变量指定相对约束之间的连接视图InformationMatrix变量指定,每条边的不确定性与连接相关联。
%显示最初几个观点和连接头(vSet.Views)头(vSet.Connections)
1 1×1 ViewId AbsolutePose PointCloud ______ ___________ * * * rigidtform3d 1×1 PointCloud 2 1×1 rigidtform3d 1×1 PointCloud 3 1×1 rigidtform3d 1×1 PointCloud 4 1×1 rigidtform3d 1×1 PointCloud 5 1×1 rigidtform3d 1×1 PointCloud 6 1×1 rigidtform3d 1×1 PointCloud 7 1×1 rigidtform3d 1×1 PointCloud 8 1×1 rigidtform3d 1×1 PointCloud ViewId1 ViewId2 RelativePose InformationMatrix ____ ____ ___________ _________________ 1 2 1×1 rigidtform3d{6×6双}2 3 1×1 rigidtform3d{6×6双}3 4 1×1 rigidtform3d{6×6双}4 5 1×1 rigidtform3d{6×6双}5 6 1×1 rigidtform3d{6×6双}6 7 1×1 rigidtform3d{6×6双}7 8 1×1 rigidtform3d{6×6双}8 9 1×1 rigidtform3d{6×6双}
现在,建立点云地图使用创建的视图组。看来绝对与点云对齐在视图中设置使用pcalign。指定网格大小来控制地图的分辨率。作为一个返回的地图pointCloud对象。
ptClouds = vSet.Views.PointCloud;absPoses = vSet.Views.AbsolutePose;mapGridSize = 0.2;ptCloudMap = pcalign (ptClouds absPoses mapGridSize);
注意,路径遍历使用这种方法随着时间的推移。虽然路径沿着第一循环回到起点似乎是合理的,第二个循环漂移显著的起点。积累的漂移导致第二个循环终止几米远的地方,起点。
使用测程法构建地图本身是不准确的。显示点云构建地图的遍历路径。注意,地图和第二循环遍历路径不符合第一个循环。
持有(hAxBefore“上”);pcshow (ptCloudMap);持有(hAxBefore“关闭”);关上(hAxBefore.Parent)
正确的使用姿势图优化漂移
图大满贯是一种广泛使用的技术在测程法解决漂移。图大满贯的方法逐步创建一个图,节点对应于车辆的姿态和边代表传感器测量约束连接构成。这样的图称为构成图。构成图包含边缘编码相互矛盾的信息,由于噪声或不准确的测量。图中的节点优化找到车辆构成的集合,最佳解释测量。这种技术被称为构成图优化。
创建一个姿势图从一组视图,您可以使用createPoseGraph函数。这个函数创建一个节点为每个视图和视图中为每个连接一条边集。优化构成图,您可以使用optimizePoseGraph(导航工具箱)函数。
一个关键方面导致校正图像的有效性大满贯漂移的准确检测循环,也就是说,之前访问过的地方。这就是所谓的环路闭合检测或位置识别。添加边构成图对应于循环闭包提供了一个连接节点构成矛盾的测量,可以解决在构成图优化。
循环闭包可以使用描述符描述当地的环境检测可见激光雷达传感器。的扫描环境描述符[1]就是这样一个描述符,可以从一个点云计算使用scanContextDescriptor函数。这个示例使用scanContextLoopDetector对象管理扫描上下文描述符对应于每个视图。它使用detectLoop目标函数来检测循环闭包与一个两阶段描述符搜索算法。在第一阶段,它计算环键subdescriptors循环找到潜在的候选人。在第二阶段,分类视图距离阈值扫描循环闭包的上下文。
%设置随机种子,以确保再现性rng (0);%设置创建一个空视图vSet = pcviewset;%创建一个循环关闭探测器loopDetector = scanContextLoopDetector;%为视图将显示创建一个图hFigBefore =图(“名字”,“视图设置显示”);hAxBefore =轴(hFigBefore);%初始化转换absTform = rigidtform3d;%绝对参考系变换relTform = rigidtform3d;%相对变换之间的连续扫描maxTolerableRMSE = 3;%最大允许RMSE循环关闭候选人被接受viewId = 1;为n = 1: skipFrames: numFrames%读点云文件名= pointCloudTable.PointCloudFileName {n};ptCloudOrig = helperReadPointCloudFromFile(文件名);%处理点云% -段和删除地平面% -段和消除自我ptCloud = helperProcessPointCloud (ptCloudOrig);% Downsample点云处理ptCloud = pcdownsample (ptCloud,“随机”,downsamplePercent);firstFrame = (n = = 1);如果firstFrame%添加第一个点云扫描作为一个视图的视图组vSet = addView (vSet viewId absTform,“PointCloud”,ptCloudOrig);%提取扫描上下文描述符从第一个点云描述符= scanContextDescriptor (ptCloudOrig);%将第一个描述符添加到循环关闭探测器addDescriptor (loopDetector viewId描述符)viewId = viewId + 1;ptCloudPrev = ptCloud;继续;结束%利用INS估计初始转换登记initTform = helperComputeInitialEstimateFromINS (relTform,…insDataTable (n-skipFrames: n));%计算刚性变换,寄存器目前点云%之前的点云relTform = pcregisterndt (ptCloud ptCloudPrev regGridSize,…“InitialTransform”,initTform);%更新绝对转换参考系(点云)absTform = rigidtform3d (absTform。* relTform。一个);%添加当前点云扫描视图的视图集vSet = addView (vSet viewId absTform,“PointCloud”,ptCloudOrig);%从先前的视图添加一个连接到当前视图代表%相对他们之间转换vSet = addConnection (vSet viewId-1、viewId relTform);%从点云提取扫描上下文描述符描述符= scanContextDescriptor (ptCloudOrig);%描述符添加到循环关闭探测器addDescriptor (loopDetector viewId描述符)%检测回路关闭候选人loopViewId = detectLoop (loopDetector);%一个循环候选人被发现如果~ isempty (loopViewId) loopViewId = loopViewId (1);%从视图设置检索点云loopView = findView (vSet loopViewId);ptCloudOrig = loopView.PointCloud;%处理点云ptCloudOld = helperProcessPointCloud (ptCloudOrig);% Downsample点云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 & &国防部(n, displayRate) = = 1 hG =情节(vSet,“父”,hAxBefore);drawnow更新结束结束
创建一个姿势图的视图设置使用createPoseGraph方法。构成图有向图对象:
节点包含每个视图的绝对姿态
包含每个连接的相对姿态约束边缘
G = createPoseGraph (vSet);disp (G)
有向图的属性:边缘:[539×3表]节点:(503×2表)
除了连续之间的测程法连接视图,视图设置现在包括循环关闭连接。例如,注意到新的联系第二个循环遍历和第一个循环遍历。这些都是循环关闭连接。这些可以确定图中边的结束节点并不连续。
%更新轴限制关注循环关闭连接xlim (hAxBefore 50 [-50]);ylim (hAxBefore 50 [-100]);%找到并突出循环关闭连接loopEdgeIds =找到(abs (diff (G.Edges。EndNodes 1 2)) > 1);突出(hG,“边缘”loopEdgeIds,“EdgeColor”,“红色”,“线宽”3)
使用优化构成图optimizePoseGraph。
optimG = optimizePoseGraph (G,“g2o-levenberg-marquardt”);vSetOptim = updateView (vSet optimG.Nodes);
显示视图集优化的姿势。现在注意,检测到循环合并,导致一个更精确的轨迹。
情节(vSetOptim“父”hAxBefore)
绝对的姿势在优化视图设置现在可以用来建立一个更精确的地图。使用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·金和金,“扫描背景:自我中心的空间描述符的地方在三维点云识别地图,”2018年IEEE / RSJ智能机器人和系统国际会议(——)马德里,2018年,页4802 - 4809。
万博1manbetx支持函数和类
helperReadDataset从指定文件夹中读取数据到一个时间表。
函数datasetTable = helperReadDataset (dataFolder pointCloudFilePattern)% helperReadDataset调速发电机大满贯数据集数据读入一个时间表% datasetTable = helperReadDataset (dataFolder)读取的数据%文件夹中指定dataFolder时间表。这个函数%预计调速发电机大满贯数据集的数据。%%参见fileDatastore helperReadINSConfigFile。%创建一个数据存储的文件以正确的顺序读入文件提起= fileDatastore (pointCloudFilePattern,“ReadFcn”,…@helperReadPointCloudFromFile);%从数据存储中提取文件列表pointCloudFiles = fileDS.Files;imuConfigFile = fullfile (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过程pointCloud去除地面和自我% ptCloud = helperProcessPointCloud (ptCloudIn方法)的过程% ptCloudIn通过消除地面飞机和自我。%方法可以“planefit”或“rangefloodfill”。%%也看到pcfitplane pointCloud / findPointsInCylinder。参数ptCloudIn(1,1)pointCloud方法字符串{mustBeMember(方法,[“planefit”、“rangefloodfill”])}=“rangefloodfill”结束isOrganized = ~ ismatrix (ptCloudIn.Location);如果(方法= =“rangefloodfill”& & isOrganized)%段地面上使用floodfill范围的形象groundFixedIdx = segmentGroundFromLidarData (ptCloudIn,…“ElevationAngleDelta”11);其他的%段飞机地面作为主要参照正常%向量指向积极的z方向maxDistance = 0.4;maxAngularDistance = 5;referenceVector = (0 0 1);[~,groundFixedIdx] = pcfitplane (ptCloudIn maxDistance,…referenceVector maxAngularDistance);结束如果isOrganized groundFixed = false(大小(ptCloudIn.Location, 1),大小(ptCloudIn.Location 2));其他的(ptCloudIn groundFixed = false。统计,1);结束groundFixed (groundFixedIdx) = true;%在圆柱段自我车辆分区域的传感器sensorLocation = (0 0 0);egoRadius = 3.5;egoFixed = findPointsInCylinder (ptCloudIn egoRadius,“中心”,sensorLocation);%的保留子集点云没有地面和自我如果isOrganized指数= ~ groundFixed & ~ egoFixed;其他的指数=找到(~ groundFixed & ~ egoFixed);结束ptCloud =选择(ptCloudIn、指标);结束
helperComputeInitialEstimateFromINS估计的初始转换登记INS读数。
函数initTform = helperComputeInitialEstimateFromINS (initTform insData)%如果没有INS读数,回报如果isempty (insData)返回;结束% INS读数提供X指向前方,Y的离开了%和Z。翻译下面占转换成激光雷达%框架。insToLidarOffset = -0.79 - -1.73 [0];%见DATAFORMAT.txtTnow = [-insData.Y(结束),insData.X(结束),insData.Z(结束)]。' + insToLidarOffset ';Tbef = [-insData.Y (1) insData.X (1) insData.Z (1)]。' + insToLidarOffset ';%自车预计将沿着地面,卷的变化%和间距很小。忽略横滚和俯仰的变化,使用标题。Rnow = rotmat(四元数([insData.Heading(结束)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 = rigidtform3d (T);结束
helperMakeFigurePublishFriendly调整数据,这样截图被公布是正确的。
函数helperMakeFigurePublishFriendly (hFig)如果~ isempty (hFig) & & (hFig) hFig是否是可用的。HandleVisibility =“回调”;结束结束
