当当前位姿即将到达地图边界的时候,当前珊格地图就必须动态调整,如下图,我们以⼀维case为例
当前帧即将抵达地图的左边界时,我们需要把当前帧和地图整体右移⼀部分,保证当前帧处于⼀个安全
的位置
这样相对移动之前,当前帧就处在⼀个“安全的位置”,然后左边会空出来⼀个珊格。
按照处理步骤来一点点描述
在 "laserMapping.cpp“的void process() 线程函数中运行,这是一个while循环
void transformAssociateToMap()
{
// T_w_curr = T_w_last * T_last_curr(from lidar odom)
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
第一次进入这里时的map_odom是初始化为单位矩阵的,即默认开始odom与map坐标系重合,前端只给到 odom_curr的变换,map-odom-curr三者坐标系关系不懂的同学可以看一下我总结的这篇文章关于ROS中map、odom、base_link三个坐标系的理解
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
centerCubeI、centerCubeJ、centerCubeK的作用:判断当前位姿在全局栅格地图中的哪个栅格里面,分别对应x、y、z
首先看代码
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
假设 t_w_curr.x = 1 米 即开始计算走了1米,(1+25)/ 50 = 0.52 由于 int 去尾的操作,所以这块 = 0,0+laserCloudCenWidth =0 + 10 ; x方向在第10个栅格的位置,刚好在栅格地图中间的栅格
再假设走了 26 米时 (26+25)/ 50 = 1.02 ,int 去尾变成 1 ,1+10 = 11,这是因为25是一个栅格的中心,一个栅格的长度为50米,所以中心就为 25米,从中间开始走超过 25米 就很明显越过当前栅格了
这部分是当 robot 向前走的情况
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
这是因为当走了-26米时,(-26+25) / 50 = -0.02 ,根据 int 的去尾是 = 0,所以要这样额外判断去使栅格id往后移
关于 int 去尾操作:1.01~1.99 都会变成1,-0.99 ~ -0.01 都会变成0
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
A-LOAM系列讲解
A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
A-LOAM(前端-2)异常点的剔除-算法流程+代码注释
A-LOAM(前端-3)的雷达畸变及运动补偿-算法流程+代码注释
A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
A-LOAM总结-(前端+后端)算法流程分析
关于ROS中map、odom、base_link三个坐标系的理解