本节目标
建立一套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，并计算五点形成平面的法向量norm
std::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.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<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
本文仅做学术分享，如有侵权，请联系删文。
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从入门到精通：理论与实战
扫码添加小助手微信，可
也可申请加入我们的细分方向交流群，目前主要有、、、、、等微信群。
一定要备注：，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。也请联系。
▲长按加微信群或投稿
▲长按关注公众号
：针对3D视觉领域的五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、答疑解惑、助你高效解决问题