selectSubmap
选择子映射在地图
描述
例子
激光雷达定位使用无损检测
加载一个正态分布变换(无损检测)地图从垫文件。
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)结束
