本节目标
搭建一套700行代码的激光SLAM。通过对ALOAM进行修改实验，确定对激光SLAM最核心的技巧，并接上节里程计，完成后端，构建较大场景(轨迹约2km)地图。
预期效果：
rosbag数据:
https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A
提取码: mewi
程序：https://gitee.com/eminbogen/one_liom
实际地图与ALOAM效果
因为先试了一下LOAM跟丢了，所以用完整走完的ALOAM来进行实验。蓝色为里程计结果，绿色为后端优化后的效果，差距非常大。第三张图是跟丢的LOAM。
ALOAM修改实验
棱匹配与曲率排序
棱匹配，是希望图像中曲率较大的点匹配到对应的棱上，与点面匹配对应。在下图中c可以当做我们的当前帧的点，ab为前一帧棱上点，作公式如下，即CA×CB/AB,由于叉乘为|CA|*|CB|*sin,所以在AB点固定时角度越接近0度或180度，两向量越小，分子就越小，可以将d作为损失量进行优化。实验后来发现这个没啥用。
程序：
代码语言：javascript复制Eigen::Matrix
Eigen::Matrix
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();曲率排序是在选取特征点对于16线，每线分为6区域，每区域内按曲率高低录取面点和棱点。实验后来发现这个很有用。
程序：
代码语言：javascript复制//利用cloudScanEndInd将每条线分成六块，每块使用sort排序，并从最平缓的4个点录入laserCloudplane，录入后对周围点作不选取的标志
for(int i=0;i<16;i++)
{
//每条线起止
int start_num = cloudScanEndInd[i];
int end_num = cloudScanEndInd[i+1];
//分六块
for(int j=0;j<6;j++)
{
int start_num_temp = start_num+ ((end_num-start_num)/6)*j;
int end_num_temp = start_num+ ((end_num-start_num)/6)*(j+1);
//块内排序cloudSortInd会从1,2,3,4这种顺序变成乱序，乱序后指代的点曲率从小到大排列
std::sort (cloudSortInd + start_num_temp, cloudSortInd + end_num_temp, comp);
//计数4个
int plane_num=0;
//k要在区间内，点要小于5个
for(int k=start_num_temp;k { //根据cloudSortInd取点序号 long ind = cloudSortInd[k]; //可选点+曲率小就要 if(laserCloudall->points[ind].r==1&&cloudcurv[ind]<0.1) { plane_num++; laserCloudall->points[ind].g=plane_num; laserCloudplane->push_back(laserCloudall->points[ind]); //临近点变成不可选 for(int m=1;ind+m { laserCloudall->points[ind+m].r=2; } for(int m=1;ind-m>0&&m<=5;m++) { laserCloudall->points[ind-m].r=2; } } } } }这里第一张图为不进行点排序筛选的效果，从左到右，从上到下依次是上次数据集采用棱+面优化结果，上次数据集只采用面优化结果，本次数据集棱+面优化结果，本次数据集只面优化结果。 这里第二张图为进行点排序筛选的效果，从左到右，从上到下依次是上次数据集采用棱+面优化结果，上次数据集只采用面优化结果，本次数据集棱+面优化结果，本次数据集只面优化结果。对比来看，有无棱没有多少差距，但本次数据集来看，有点的排序筛选的形状畸变更小，在后续加入后端检测后，这一操作有无会产生重大影响。 帧对地图匹配 对于ALOAM，每帧点云是与地图中一定范围内的点匹配，这与一帧点云和前一定数量的帧形成的地图进行匹配是不同的，ALOAM因为这一设定拥有了一种类似闭环检测的能力。如下图中，绿色为我写的里程计结果，蓝色为后端优化后结果。第一张是新帧和前200帧形成的地图匹配，后一张为新帧和全局地图匹配，效果差距非常大。这里的匹配是寻找当前帧位姿变换后地图内的临近面点，之后进行点面优化，所以当里程计运算累积误差较大时，匹配也匹配不上实际平面，所以，这和icp求解位姿变换解决闭环检测问题不同，也就是还需要闭环检测。 棱面边界去除 这是指程序中如果检测到面点，那么会把周围点去除不进行点筛选，除非附近点曲率较大可能为棱点。 程序： 代码语言：javascript复制for (int l = 1; l <= 5; l++) { float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { break; } cloudNeighborPicked[ind + l] = 1; }因为ALOAM是先取棱后取面，实际面之间剩下的棱点比较少，其次边缘区域全去除不是坏事，因为曲率如果是0.06，那么这里会没有去除，在之后面点提取中可能把它提进来，这种面之间的棱点提进来我个人觉得不太好，相当于你收集了一个曲面，不利于后续结果，实验发现删了这个操作精度会有效提升。 棱点筛除 提取棱点有两个作用，一是你确实有了棱点，二是棱点周围的点你都会去除，不会算入面点中，实际效果没啥用。 地面点分离 这个是参考LEGO和HDL来的，将地面点删除后统计面点，后来发现地面如果比较平（如这个数据集），其实对结果有不错的影响，毕竟地面的面也是面。你的面从四周变成前后左右下五个方向，对结果是有益的。 后端构建 后端的坐标系有三个，里程计传递过来的相对里程计原点的坐标系q_wodom_curr,t_wodom_curr，后端坐标系q_w_curr，t_w_curr，后端相对里程计的坐标系q_wmap_wodom,t_wmap_wodom，整个系统接收里程计，使用后端相对里程计的坐标系计算后端坐标系。在odometry.cpp里将当前帧变成前一帧坐标系下（局部坐标系），然后在map.cpp里转换到后端坐标系与后端坐标系下的全部地图匹配，以点面匹配的方式，修改后端坐标系q_w_curr，t_w_curr。 损失函数： 代码语言：javascript复制//点面损失函数，输入的是当前帧的某点_point_o_，目标平面的中心点_point_a_，目标平面的法线_norn_，常规求ao向量在法向量上的投影 struct CURVE_PLANE_COST { CURVE_PLANE_COST(Eigen::Vector3d _point_o_, Eigen::Vector3d _point_a_,Eigen::Vector3d _norn_): point_o_(_point_o_),point_a_(_point_a_),norn_(_norn_){} template bool operator()(const T* q,const T* t,T* residual)const { Eigen::Matrix Eigen::Matrix Eigen::Matrix Eigen::Quaternion Eigen::Matrix Eigen::Matrix p_o_last=rot_q * p_o_curr + rot_t; residual[0]=((p_o_last - p_a_last).dot(p_norm)); return true; } const Eigen::Vector3d point_o_,point_a_,norn_; };对于点面匹配的选点，为遍历当前帧的所有点，使用KD树寻找全局地图下的最近的五个点，并求出五个点的法向量，并根据法向量norn与（五个点和五点中心的向量 ）的投影大小确定五点是否成面。成面则进行优化。 代码语言：javascript复制for(int i=0;i { //将当前帧的点转换到世界坐标系，与世界坐标系内的点找五个最近的点 PointType pointseed;std::vector TransformToMap(&laserCloudPlane->points[i],&pointseed); kdtreePlane.nearestKSearch(pointseed, 5, pointSearchInd, pointSearchSqDis); //如果五个点里最远的那个也不超过2m， if (pointSearchSqDis[4] < 2.0) { //找五个点的中心点center，并计算五点形成平面的法向量norm std::vector Eigen::Vector3d center(0, 0, 0); for (int j = 0; j < 5; j++) { Eigen::Vector3d tmp(laserCloudMap->points[pointSearchInd[j]].x, laserCloudMap->points[pointSearchInd[j]].y, laserCloudMap->points[pointSearchInd[j]].z); center = center + tmp; nearCorners.push_back(tmp); } center = center / 5.0; Eigen::Matrix Eigen::Matrix for (int j = 0; j < 5; j++) { matA0(j, 0) = laserCloudMap->points[pointSearchInd[j]].x; matA0(j, 1) = laserCloudMap->points[pointSearchInd[j]].y; matA0(j, 2) = laserCloudMap->points[pointSearchInd[j]].z; //printf(" pts %f %f %f \n", matA0(j, 0), matA0(j, 1), matA0(j, 2)); } // find the norm of plane //可以根据这个学习一下https://www.cnblogs.com/wangxiaoyong/p/8977343.html Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); norm.normalize(); //将五个点和中心点形成向量，向量与法向量求点乘，如果大于0.1那么后面就不把这组点放入优化了 bool planeValid = true; for (int j = 0; j < 5; j++) { Eigen::Vector3d vector_temp(laserCloudMap->points[pointSearchInd[j]].x-center.x(), laserCloudMap->points[pointSearchInd[j]].y-center.y(), laserCloudMap->points[pointSearchInd[j]].z-center.z()); if (fabs(norm(0) * vector_temp.x() +norm(1) * vector_temp.y() +norm(2) * vector_temp.z()) > 0.1) { planeValid = false; break; } } //当前点curr_point，放入优化 Eigen::Vector3d curr_point(pointseed.x, pointseed.y, pointseed.z); if (planeValid) { Eigen::Vector3d curr_point_o(laserCloudPlane->points[i].x,laserCloudPlane->points[i].y,laserCloudPlane->points[i].z); problem.AddResidualBlock(new ceres::AutoDiffCostFunction (new CURVE_PLANE_COST(curr_point_o,center,norm)),loss_function,parameters,parameters+4); res_num++; } } } 讨论 时耗 后端在后面点多时约70ms，此时前端点处理大概6ms，里程计大约13ms。这种帧对全图的匹配耗时巨大，应该使用当前帧匹配前一定数量的帧（匹配前200帧大概只要几毫秒，这和0.4的降采样有关），之后引入闭环检测，计划使用LIO_SAM的简单位姿欧拉距离求临近帧再icp的方式解决，第五节见。 平整度 可以看出后端有效地让地面平整了，约束了累积误差。 不足 虽然是帧对全地图匹配，但因为没有真正的闭环，在最后会因为累积误差匹配不上闭环（一图），中间地带虽然有效匹配上了，但任然存在点云分层问题（二图），实际轨迹也并不是在中间地区来回路径一致，需要闭环检测。 版权声明：本文为CSDN博主「Eminbogen」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。编辑：古月居 原文链接： https://blog.csdn.net/unlimitedai/article/details/108064632 本文仅做学术分享，如有侵权，请联系删文。