findPose
描述
例子
激光雷达定位使用无损检测
加载一个正态分布变换(无损检测)地图从垫文件。
data =负载(“ndtMapParkingLot.mat”);ndtMap = data.ndtMapParkingLot;
从第二个加载点云扫描和姿势估计垫文件。
data =负载(“parkingLotData.mat”);ptCloudScans = data.parkingLotData.ptCloudScans;initPoseEsts = data.parkingLotData.initPoseEsts;
无损检测地图显示。
显示(ndtMap)
改变视角的顶视图。
视图(2)
选择submap围绕第一个估计。
中心= initPoseEsts (1) .Translation;深圳= (70 50 20);ndtMap = selectSubmap (ndtMap,中心、深圳);
为可视化的当前位置和半径的距离阈值更新子映射。
半径= 0.5;distThresh = 15;
循环的点云,在地图上定位,并根据需要更新选中的子映射。
numScans =元素个数(ptCloudScans);为n = 1: numScans ptCloud = ptCloudScans (n);initPose = initPoseEsts (n);poseTranslation = initPose.Translation;[isInside, distToEdge] = isInsideSubmap (ndtMap poseTranslation);submapNeedsUpdate = ~ isInside…% submap外电流构成| |任何(distToEdge (1:2) < distThresh);%当前姿势接近submap边缘如果submapNeedsUpdate ndtMap = selectSubmap (ndtMap poseTranslation, sz);结束%的点云扫描定位地图。currPose = findPose (ndtMap ptCloud initPose);%的位置估计显示为一个圈。pos = [currPose.Translation(1:2)半径);showShape (“圆”、pos、“颜色”,“r”);%停下来查看变化。暂停(0.05)结束
输入参数
输出参数
提示
为了提高定位的准确性和效率,考虑将采样点云
pcdownsample在使用这个函数。
引用
[1]Biber, P。,and W. Strasser. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” In诉讼2003 IEEE / RSJ智能机器人和系统国际会议(2003年——)(猫。No.03CH37453)3卷,2743 - 48。美国内华达州拉斯维加斯:IEEE 2003。https://doi.org/10.1109/IROS.2003.1249285。
[2]Magnusson,马丁。“三维正态分布变换:一种有效表示注册、表面分析,和循环检测。”PhD thesis, Örebro universitet, 2009. http://urn.kb.se/resolve?urn=urn:nbn:se:oru:diva-8458 urn:nbn:se:oru:diva-8458.
