使用线段匹配构建地图和定位
这个例子展示了如何构建一个映射与激光雷达数据和本地化使用SegMatch车辆在地图上的位置[1]基于分段匹配,识别算法。
自主驾驶系统使用定位来确定车辆的位置在一个映射环境。自主导航需要准确定位,这依赖于一个精确的地图。建立一个精确的地图大规模环境是困难的,因为地图上积累漂移随着时间的推移,和检测回路关闭正确累积漂移挑战性是因为动态障碍。SegMatch算法健壮在大规模环境中动态障碍和可靠。该算法是一种基于航段的方法,利用描述性的形状和承认的地方通过匹配段。
概述
就像建立一个地图使用大满贯从激光雷达数据的例子,这个例子使用3 d激光雷达数据建立一个地图和纠正的累积使用图大满贯漂移。然而,这个例子中不需要全球姿势估计来自其他传感器,如惯性测量单元(IMU)。在构建地图之后,这个示例使用本地化车辆在一个已知的环境中。
在这个例子中,您将了解如何:
用SegMatch找到两个点云之间的相对变换对应于相同的地方
建立一个地图使用SegMatch环路闭合检测
本地化使用SegMatch预先构建的地图上
下载数据
在本例中使用的数据的一部分调速发电机大满贯数据集。它包括大约6分钟的数据记录与调速发电机®HDL64E-S2扫描仪。数据下载到一个临时目录中。这可能需要几分钟。
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)……
加载并选择数据
下载的数据集将点云数据存储在PNG文件。创建一个文件使用的数据存储helperReadVelodyneSLAMData函数加载PNG文件和点云数据距离值转换为三维坐标。辅助函数是一个定制的阅读功能,其目的是为调速发电机大满贯数据集。选择数据的一个子集,并将数据用于地图构建和本地化。
%创建一个文件数据存储读取文件以正确的顺序。提起= fileDatastore (pointCloudFilePattern,“ReadFcn”,…@helperReadVelodyneSLAMData);%读点云。ptCloudArr = readall(提交);%的选择一个子集点云扫描,并用于将数据%地图构建和本地化。ptCloudMap = vertcat (ptCloudArr {1:5:1550});ptCloudLoc = vertcat (ptCloudArr {2:5:1550});%想象第一个点云。图pcshow (ptCloudMap(1)标题(点云数据的)
SegMatch概述
SegMatch算法包含四个不同的组件:点云分割,特征提取,部分匹配和几何验证。为达到最佳效果,执行这些四个步骤之前,点云预处理。
预处理点云
选择最相关的点云数据,执行以下预处理步骤:
选择一个圆柱附近围绕车辆提取感兴趣的点云。
删除在准备点云分割成不同的对象。使用
segmentGroundSMRF段地面。
%从地图上选择一个点云预处理。ptCloud = ptCloudMap (25);%设置圆柱半径和自我半径。cylinderRadius = 40;egoRadius = 1;%选择圆柱半径内的点和外部自我半径。cylinderIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);cylinderPtCloud =选择(ptCloud cylinderIdx,“OutputSize”,“全部”);%去除地面。[~,ptCloudNoGround] = segmentGroundSMRF (cylinderPtCloud,“ElevationThreshold”,0.05);%可视化前后的点云预处理。图pcshowpair (ptCloud ptCloudNoGround)标题(点云预处理前后的)
分割和特征提取
接下来,部分点云从每个段和提取特征。
段使用的点云segmentLidarData功能和可视化部分。在这个例子中,每个部分都必须有一个最低150分。这会产生部分集群表示不同的对象和有足够的点来描述该地区的地图。
不同的数据集需要不同的参数分割。要求少点段可能导致假阳性循环闭包,并限制的部分更大的集群可以消除部分识别重要的地方。您还必须调整的距离和角度阈值,以确保每一部分对应于一个对象。小的距离阈值会导致许多段对应于相同的对象,和一个大的距离阈值和小角度阈值会导致部分结合许多对象。
minNumPoints = 150;distThreshold = 1;angleThreshold = 180;[标签,numClusters] = segmentLidarData (ptCloudNoGround distThreshold,…angleThreshold,“NumClusterPoints”,minNumPoints);%去除点包含可视化标签值0。idxValidPoints =找到(标签);labelColorIndex =标签(idxValidPoints);segmentedPtCloud =选择(ptCloudNoGround idxValidPoints);图pcshow (segmentedPtCloud.Location labelColorIndex)标题(“点云段”)
从每一段通过提取特征extractEigenFeatures函数。Eigenvalue-based特性几何特性。每个特性向量包括线性、平面化、散射omnivariance,各向异性,eigenentropy和曲率的变化。
(特性、段)= extractEigenFeatures (ptCloud、标签);disp(特性)
31日×1 eigenFeature数组属性:功能重心
disp(段)
31日×1 pointCloud数组属性:位置计数XLimits YLimits ZLimits颜色正常的强度
段匹配和几何验证
找到匹配的部分之间的转换部分对应的两个点云扫描循环关闭。
预处理和提取线段特征从两个点云。的helperPreProcessPointCloud函数包括预处理步骤预处理点云部分,简化预处理点云在整个工作流程。
ptCloud1 = ptCloudMap (27);ptCloud2 = ptCloudMap (309);ptCloud1 = helperPreProcessPointCloud (ptCloud1 egoRadius cylinderRadius);ptCloud2 = helperPreProcessPointCloud (ptCloud2 egoRadius cylinderRadius);labels1 = segmentLidarData (ptCloud1 distThreshold,…angleThreshold,“NumClusterPoints”,minNumPoints);labels2 = segmentLidarData (ptCloud2 distThreshold,…angleThreshold,“NumClusterPoints”,minNumPoints);[features1, segments1] = extractEigenFeatures (ptCloud1 labels1);[features2, segments2] = extractEigenFeatures (ptCloud2 labels2);
找到可能的部分匹配基于归一化特征向量通过使用之间的欧氏距离pcmatchfeatures函数。
featureMatrix1 = vertcat (features1.Feature);featureMatrix2 = vertcat (features2.Feature);indexPairs = pcmatchfeatures (featureMatrix1 featureMatrix2);
进行几何验证通过识别内围层和发现之间的三维刚性变换段匹配使用estgeotform3d函数。根据点云内围层的数量,可分为循环关闭。
centroids1 = vertcat (features1 (indexPairs (: 1)) .Centroid);centroids2 = vertcat (features2 (indexPairs (:, 2) .Centroid);[tform, inlierPairs] = estgeotform3d (centroids1 centroids2,“刚性”);
可视化部分匹配使用pcshowMatchedFeatures函数。
inlierIdx1 = indexPairs (inlierPairs, 1);inlierIdx2 = indexPairs (inlierPairs 2);图pcshowMatchedFeatures (segments1 (inlierIdx1) segments2 (inlierIdx2),…features1 (inlierIdx1) features2 (inlierIdx2))标题(“段匹配”)
对齐的转换从几何验证步骤使用pccat和pctransform。
ptCloudSegments1 = pccat (segments1);ptCloudSegments2 = pccat (segments2);tformedPtCloudSegments1 = pctransform (ptCloudSegments1 tform);
可视化对齐段使用pcshowpair。
图pcshowpair (tformedPtCloudSegments1 ptCloudSegments2)标题(“片段”)
构建地图
地图构建过程包括以下步骤:
点云预处理
注册点云
段和点云提取特征
检测循环闭包
预处理点云
预处理前和当前使用点云helperPreProcessPointCloud。Downsample点云pcdownsample提高注册速度和准确度。调整比例downsample输入,发现价值最低的维护所需的车辆转时登记的准确性。
currentViewId = 2;prevPtCloud = helperPreProcessPointCloud (ptCloudMap (currentViewId-1),…egoRadius cylinderRadius);ptCloud = helperPreProcessPointCloud (ptCloudMap (currentViewId),…egoRadius cylinderRadius);downsamplePercent = 0.5;prevPtCloudFiltered = pcdownsample (prevPtCloud,“随机”,downsamplePercent);ptCloudFiltered = pcdownsample (ptCloud,“随机”,downsamplePercent);
注册点云
注册当前点云与以前的点云找到相对的转换。
gridStep = 3;relPose = pcregisterndt (ptCloudFiltered prevPtCloudFiltered gridStep);
使用一个pcviewset对象跟踪绝对姿态和注册点云之间的联系。创建一个空pcviewset。
vSet = pcviewset;
初始化构成的点云身份刚性变换,并将其添加到视图设置使用addView。
initAbsPose = rigidtform3d;vSet = addView (vSet currentViewId-1 initAbsPose);
计算的绝对姿态第二点云在注册过程中使用的相对姿态估计,并将其添加到视图集。
absPose = rigidtform3d (initAbsPose。* relPose.A);vSet = addView (vSet currentViewId absPose);
连接两个视图使用addConnection。
vSet = addConnection (vSet currentViewId-1、currentViewId relPose);
改变当前的点云对齐全球地图。
ptCloud = pctransform (ptCloud absPose);
部分点云并提取特征
段之前和现在的点云segmentLidarData。
labels1 = segmentLidarData (prevPtCloud distThreshold angleThreshold,…“NumClusterPoints”,minNumPoints);labels2 = segmentLidarData (ptCloud distThreshold angleThreshold,…“NumClusterPoints”,minNumPoints);
从以前的和当前的点云提取特征段使用extractEigenFeatures。
[prevFeatures, prevSegments] = extractEigenFeatures (prevPtCloud labels1);(特性、段)= extractEigenFeatures (ptCloud labels2);
跟踪部分和功能使用pcmapsegmatch对象。创建一个空pcmapsegmatch。
sMap = pcmapsegmatch;
添加视图、功能和部分在过去和当前的点云pcmapsegmatch使用addView。
sMap = addView(大声叫嚷,currentViewId-1、prevFeatures prevSegments);sMap = addView (currentViewId sMap,特性,部分);
检测循环闭包
估计姿势积累漂移随着越来越多的点云被添加到地图。检测回路关闭帮助正确累积漂移和产生一个更精确的地图。
检测循环闭包使用findPose。
[absPoseMap, loopClosureViewId] = findPose (sMap, absPose);isLoopClosure = ~ isempty (absPoseMap);
如果findPose检测到一个循环闭包,找到当前视图之间的转换和循环关闭视图,并将它添加到pcviewset对象。
使用当前视图的绝对姿态没有积累漂移,absPoseMap,循环的绝对姿态关闭视图,absPoseLoop,计算之间的相对构成循环关闭没有漂移的姿势。
如果isLoopClosure absPoseLoop =姿势(vSet loopClosureViewId) .AbsolutePose;relPoseLoopToCurrent = rigidtform3d(反转(absPoseLoop)。* absPoseMap.A);
添加循环关闭相对冒充一个连接使用addConnection。
vSet = addConnection (vSet loopClosureViewId currentViewId,…relPoseLoopToCurrent);
正确使用造成的累积漂移图优化。考虑寻找多个循环关闭连接优化姿势之前,因为优化构成图和更新pcmapsegmatch对象都是计算密集型的。
保存之前提出了优化。提出了需要更新部分和重心位置pcmapsegmatch对象。
prevPoses = vSet.Views.AbsolutePose;
创建一个姿势图的视图设置使用createPoseGraph,优化构成图使用optimizePoseGraph(导航工具箱)。
G = createPoseGraph (vSet);optimG = optimizePoseGraph (G,“g2o-levenberg-marquardt”);vSet = updateView (vSet optimG.Nodes);
找到转换提出了校正前后的漂移和使用它们来更新地图段和质心位置使用updateMap。
optimizedPoses = vSet.Views.AbsolutePose;relPoseOpt = rigidtform3d.empty;为k = 1:元素个数(prevPoses) relPoseOpt (k) = rigidtform3d (optimizedPoses (k)。*转化(prevPoses (k))。);结束sMap = updateMap (sMap, relPoseOpt);结束
构建地图和正确的累积漂移,这些步骤适用于其余的点云扫描。
%设置随机种子例如再现性。rng (0)%更新显示每5扫描。图updateRate = 5;%初始化变量注册。prevPtCloud = ptCloudFiltered;prevPose = rigidtform3d;%跟踪关闭循环的优化提出了一旦足够的循环%检测到闭包。totalLoopClosures = 0;为我= 3:元素个数(ptCloudMap) ptCloud = ptCloudMap(我);%进行预处理和注册点云。ptCloud = helperPreProcessPointCloud (ptCloud egoRadius cylinderRadius);ptCloudFiltered = pcdownsample (ptCloud,“随机”,downsamplePercent);relPose = pcregisterndt (ptCloudFiltered prevPtCloud gridStep,…“InitialTransform”,relPose);ptCloud = pctransform (ptCloud absPose);%将当前的点云存储注册下一个点云。prevPtCloud = ptCloudFiltered;%计算的绝对构成当前的点云。absPose = rigidtform3d (absPose。* relPose.A);%如果车辆已经至少2米自上次,继续%与分割、特征提取和环路闭合检测。如果规范(absPose.Translation-prevPose.Translation) > = 2%部分点云并提取特征。标签= segmentLidarData (ptCloud distThreshold angleThreshold,…“NumClusterPoints”,minNumPoints);(特性、段)= extractEigenFeatures (ptCloud、标签);%跟踪当前视图id。currentViewId = currentViewId + 1;%将视图添加到云角度设置和地图表示。vSet = addView (vSet currentViewId absPose);vSet = addConnection (vSet currentViewId-1 currentViewId,…rigidtform3d(反转(prevPose)。* absPose.A));sMap = addView (currentViewId sMap,特性,部分);%更新视图设置显示。如果国防部(currentViewId updateRate) = = 0 (vSet) drawnow阴谋结束%检查如果有一个循环关闭。[absPoseMap, loopClosureViewId] = findPose (absPose sMap,“MatchThreshold”,1…“MinNumInliers”5,“NumSelectedClusters”4“NumExcludedViews”,150);isLoopClosure = ~ isempty (absPoseMap);如果isLoopClosure totalLoopClosures = totalLoopClosures + 1;%之间找到相对构成回路关闭。.AbsolutePose absPoseLoop =姿势(vSet loopClosureViewId);relPoseLoopToCurrent = rigidtform3d(反转(absPoseLoop)。* absPoseMap.A);vSet = addConnection (vSet loopClosureViewId currentViewId,…relPoseLoopToCurrent);%优化构成的图表和地图更新每次3%检测到循环闭包。如果国防部(totalLoopClosures 3) = = 0 prevPoses = vSet.Views.AbsolutePose;%对积累的漂移。G = createPoseGraph (vSet);optimG = optimizePoseGraph (G,“g2o-levenberg-marquardt”);vSet = updateView (vSet optimG.Nodes);%更新地图。optimizedPoses = vSet.Views.AbsolutePose;relPoseOpt = rigidtform3d.empty;为k = 1:元素个数(prevPoses) relPoseOpt (k) = rigidtform3d (optimizedPoses (k)。*转化(prevPoses (k))。);结束sMap = updateMap (sMap, relPoseOpt);%更新绝对体式后图形优化。absPose = optimizedPoses(结束);结束结束prevPose = absPose;结束结束
%从顶部视图可视化部分的地图。图显示(sMap)视图(2)标题(段的地图)
本地化工具在已知的地图
本地化的预处理步骤使用SegMatch用于地图构建是相同的预处理步骤。由于算法依赖于一致的分割,使用相同的分割参数,等待最好的结果。
ptCloud = ptCloudLoc (1);%点云预处理。ptCloud = helperPreProcessPointCloud (ptCloud egoRadius cylinderRadius);%部分点云并提取特征。标签= segmentLidarData (ptCloud distThreshold angleThreshold,…“NumClusterPoints”,minNumPoints);特点= extractEigenFeatures (ptCloud、标签);
因为没有位置估计车辆,你必须使用初始车辆定位地图的程度。选择地图的程度为第一次使用本地化selectSubmap。
sMap = selectSubmap (sMap, [sMap。XLimits sMap。YLimits sMap.ZLimits]);
使用findPose对象的函数pcmapsegmatch本地化的车辆预构建地图。
absPoseMap = findPose(大声叫嚷,特性,“MatchThreshold”,1“MinNumInliers”5);
可视化地图,和使用showShape车辆在地图上可视化是一个长方体。
mapColor = 0.4 - 0.7 [0];mapSegments = pccat (sMap.Segments);hAxLoc = pcshow (mapSegments.Location mapColor);标题(“一个预先构建的地图定位”)视图(2)poseTranslation = absPoseMap.Translation;皮疹=四元数(absPoseMap.Rotation ',“rotmat”,“点”);θ= eulerd(皮疹,“ZYX股票”,“点”);pos = [poseTranslation 5 9 3.5θ(2)θ(3)θ(1)];showShape (“长方体”、pos、“颜色”,“绿色”,“父”hAxLoc,“不透明度”,0.8,“线宽”,0.5)
提高定位速度其余的扫描,选择一个submap使用selectSubmap。
submapSize = (65 65 200);sMap = selectSubmap(大声叫嚷,poseTranslation submapSize);
继续本地化工具使用剩下的点云扫描。使用isInsideSubmap和selectSubmapsubmap更新。如果没有足够的细分定位车辆使用部分匹配,使用登记估计姿势。
%可视化地图。图(“可见”,“上”)hAx = pcshow (mapSegments.Location mapColor);标题(“一个预先构建的地图定位”)%设置参数更新子映射。submapThreshold = 30;%初始化注册的姿态和以前的点云。prevPtCloud = ptCloud;relPose = rigidtform3d;prevAbsPose = rigidtform3d;%段每个点云和本地化找到段匹配。为n = 2:元素个数(ptCloudLoc) ptCloud = ptCloudLoc (n);%点云预处理。ptCloud = helperPreProcessPointCloud (ptCloud egoRadius cylinderRadius);%部分点云并提取特征。标签= segmentLidarData (ptCloud distThreshold angleThreshold,…“NumClusterPoints”,minNumPoints);特点= extractEigenFeatures (ptCloud、标签);%本地化的点云。absPoseMap = findPose(大声叫嚷,特性,“MatchThreshold”,1“MinNumInliers”5);%登记时的位置与段无法估计%匹配。如果isempty (absPoseMap) relPose = pcregisterndt (ptCloud、prevPtCloud gridStep,…“InitialTransform”,relPose);absPoseMap = rigidtform3d (prevAbsPose。* relPose.A);结束%在地图上显示位置估计。poseTranslation = absPoseMap.Translation;皮疹=四元数(absPoseMap.Rotation ',“rotmat”,“点”);θ= eulerd(皮疹,“ZYX股票”,“点”);pos = [poseTranslation 5 9 3.5θ(2)θ(3)θ(1)];showShape (“长方体”、pos、“颜色”,“绿色”,“父”hAx,“不透明度”,0.8,“线宽”,0.5)%确定选定的子映射需要更新。[isInside, distToEdge] = isInsideSubmap (sMap, poseTranslation);needSelectSubmap = ~ isInside…% submap外电流构成。| |任何(distToEdge (1:2) < submapThreshold);%当前姿势接近submap边缘。%选择一个新的子映射。如果needSelectSubmap sMap = selectSubmap (sMap, poseTranslation submapSize);结束prevAbsPose = absPoseMap;prevPtCloud = ptCloud;结束
引用
[1]r·杜布d杜佳斯大肠作声响,j .分担r . Siegwart, c . Cadena。“SegMatch:基于段的识别在三维点云。”IEEE International Conference on Robotics and Automation (ICRA), 2017.
金宝app支持功能
helperReadVelodyneSLAMData从从调速发电机PNG图像文件读取点云数据集。
helperPreProcessPointCloud选择一个圆柱附近和消除了地面点云。
函数ptCloud = helperPreProcessPointCloud (ptCloud egoRadius cylinderRadius)%选择圆柱半径内的点和外部自我半径。cylinderIdx = findPointsInCylinder (ptCloud [egoRadius cylinderRadius]);ptCloud =选择(ptCloud cylinderIdx,“OutputSize”,“全部”);%去除地面。[~,ptCloud] = segmentGroundSMRF (ptCloud,“ElevationThreshold”,0.05);结束
