主要内容

findPose

利用正态分布变换(NDT)算法对地图中的点云进行定位

自从R2021a

折叠所有页面

语法

currPose = findPose(ndtMap,ptCloud,initPose)
currPose = findPose(___、名称、值)

描述

例子

currPose= findPose (ndtMap，ptCloud，initPose）定位点云的姿态ptCloud在无损检测地图内ndtMap使用NDT算法。函数将搜索空间限制为子映射，该子映射由SelectedSubmap的属性ndtMap对象。

currPose= findPose (___，名称,值）除前面语法中的输入参数外，使用一个或多个名称-值参数指定选项。例如,“MaxIterations”,30岁设置函数停止NDT算法之前的最大迭代次数。

例子

全部折叠

打开实时脚本

从MAT文件加载正态分布变换(NDT)映射。

数据=负载(“ndtMapParkingLot.mat”）;ndtMap = data.ndtMapParkingLot;

从第二个MAT文件加载点云扫描和姿态估计。

数据=负载(“parkingLotData.mat”）;ptCloudScans = data.parkingLotData.ptCloudScans;initPoseEsts = data.parkingLotData.initPoseEsts;

显示无损检测图。

显示(ndtMap)

将视角更改为俯视图。

视图(2)

选择以第一个估计值为中心的子地图。

center = initPoseEsts(1).Translation;Sz = [70 50 20];ndtMap = selectSubmap(ndtMap,center,sz);

设置当前位置的可视化半径和更新子地图的距离阈值。

半径= 0.5;distThresh = 15;

循环遍历点云，在地图中本地化它们，并根据需要更新所选的子地图。

numScans = nummel (ptCloudScans);为n = 1:numScans ptCloud = ptCloudScans(n);initPose = initPoseEsts(n);poseTranslation = initPose.Translation;[isInside,distToEdge] = isInsideSubmap(ndtMap,poseTranslation);submapNeedsUpdate = ~isInside.．.当前姿态在子映射之外|| any(distToEdge(1:2) < distThresh);当前位姿接近子地图边缘如果submapNeedsUpdate ndtMap = selectSubmap(ndtMap,poseTranslation,sz);结束在地图中定位点云扫描。currPose = findPose(ndtMap,ptCloud,initPose);将估算值的位置显示为圆形。pos = [currPose.Translation(1:2) radius];showShape (“圆”、pos、“颜色”，“r”）;%暂停查看更改。暂停(0.05)结束

图中包含一个轴对象。坐标轴对象包含一个散点类型的对象。

输入参数

全部折叠

NDT映射，指定为pcmapndt对象。

传感器坐标系中的点云，指定为pointCloud对象。

传感器在地图中的姿态的初始估计，指定为rigidtform3d对象。

名称-值参数

指定可选参数对为Name1 = Value1,…,以=家,在那里名字参数名称和价值对应的值。名称-值参数必须出现在其他参数之后，但对的顺序无关紧要。

在R2021a之前，使用逗号分隔每个名称和值，并将其括起来名字在报价。

例子:“MaxIterations”,30岁在30次迭代后停止NDT算法。

异常值相对于正态分布的期望百分比，指定为范围[0,1]中的标量。NDT算法假设一个点是由内值的正态分布和异常值的均匀分布的混合生成的。更大的值OutlierRatio减少异常值的影响。

数据类型:单|双

NDT算法停止前的最大迭代次数，指定为非负整数。

数据类型:单|双

连续NDT迭代之间的公差，指定为具有非负值的2元素向量。向量，[TdiffRdiff]，分别表示连续NDT迭代中估计的平移和旋转绝对差值的容差。Tdiff测量两个平移向量之间的欧氏距离。Rdiff以度为单位测量角差。当最近连续迭代中估计的刚性变换之间的差值低于指定的公差值时，算法停止。

数据类型:单|双

显示指定为逻辑的进度信息0（假)或1（真正的)．集详细的来真正的显示进度信息。

数据类型:逻辑

输出参数

全部折叠

传感器在映射中的姿态，返回为rigidtform3d对象。函数将搜索空间限制为子映射，该子映射由SelectedSubmap的属性ndtMap．

提示

  • 为了提高定位的准确性和效率，可以考虑对点云进行下采样pcdownsample在使用此函数之前。

参考文献

[1]比伯，P.和W.斯特拉瑟。正态分布变换:激光扫描匹配的一种新方法在2003年IEEE/RSJ智能机器人与系统国际会议论文集(IROS 2003) (Cat。No.03CH37453)卷3,2743-48。美国内华达州拉斯维加斯:IEEE, 2003。https://doi.org/10.1109/IROS.2003.1249285。

马格努松，马丁。三维正态分布变换:配准、表面分析和环路检测的有效表示博士论文，Örebro大学，2009。oru: http://urn.kb.se/resolve?urn=urn nbn公司禁止:se:女主角- 8458 urn: nbn公司禁止:se: oru:女主角- 8458。

扩展功能

C/ c++代码生成
使用MATLAB®Coder™生成C和c++代码。

版本历史

R2021a中引入

全部展开

另请参阅

对象

功能