本节目标

搭建一套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作为损失量进行优化。实验后来发现这个没啥用。

程序：

Eigen::Matrix<T, 3, 1> nu = (lpc - lpa).cross(lpc - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();

曲率排序是在选取特征点对于16线，每线分为6区域，每区域内按曲率高低录取面点和棱点。实验后来发现这个很有用。

程序：

//利用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<end_num_temp&&plane_num<5;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<laser_num&&m<=5;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求解位姿变换解决闭环检测问题不同，也就是还需要闭环检测。

棱面边界去除

这是指程序中如果检测到面点，那么会把周围点去除不进行点筛选，除非附近点曲率较大可能为棱点。

程序：

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。

损失函数：

//点面损失函数，输入的是当前帧的某点_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 <typename T>bool operator()(const T* q,const T* t,T* residual)const
{Eigen::Matrix<T, 3, 1> p_o_curr{T(point_o_.x()), T(point_o_.y()), T(point_o_.z())};Eigen::Matrix<T, 3, 1> p_a_last{T(point_a_.x()), T(point_a_.y()), T(point_a_.z())};Eigen::Matrix<T, 3, 1> p_norm{T(norn_.x()), T(norn_.y()), T(norn_.z())};Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};Eigen::Matrix<T, 3, 1> p_o_last;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与（五个点和五点中心的向量 ）的投影大小确定五点是否成面。成面则进行优化。

for(int i=0;i<plane_num;i++)
{//将当前帧的点转换到世界坐标系，与世界坐标系内的点找五个最近的点PointType pointseed;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;TransformToMap(&laserCloudPlane->points[i],&pointseed);kdtreePlane.nearestKSearch(pointseed, 5, pointSearchInd, pointSearchSqDis);//如果五个点里最远的那个也不超过2m，if (pointSearchSqDis[4] < 2.0){//找五个点的中心点center，并计算五点形成平面的法向量normstd::vector<Eigen::Vector3d> nearCorners;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<double, 5, 3> matA0;Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();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.htmlEigen::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<CURVE_PLANE_COST,1,4,3>(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

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~