A-Loam框架学习

激光入门之A-Loam

Aloam将Loam中很多公式优化了，直接调运库进行实现，代码简洁易读，但是效率有所降低，代码风格较好，适合新手入门。在

unbuntu20.04进行了复现，过程较为坎坷，首先是ros安装，安装的博客很多，其中台式电脑初始化容易出问题，网上都建议换手机热点，但是不一定管事，有个好途径，去淘宝找人安装，十元二十元左右，有专门干这个的，可以节省你很多时间，主要原因就是网站被墙了，可能能够解决。再就是pcl库的安装，不要安装太高版本，1.9就行，1.12容易出问题。ceres库安装还好，版本最好也别太高。3.3网上说问题也有，总之20.04的解决经验不多，最好用Ubuntu16.04或者18.04。

• LOAM是一个激光里程计算法，没有闭环检测，也就没有加入图优化框架，该算法把SLAM问题分为两个算法并行运行：一个odometry算法，10Hz；另一个mapping算法，1Hz，最终将两者输出的pose做整合，实现10Hz的位姿实时输出。两个算法都是使用点云中提取出尖锐的边点和平整的面点作为特征点，然后进行特征点匹配，来估计lidar的位姿以及对位姿进行fine tune。匹配特征点时，不是简单的点到点，而是edge point到edge line（点到线）以及planar point到planar patch（点到面），在后续运动估计中建立的残差距离也就是点到线的距离以及点到面的距离，所以感觉LOAM匹配的过程就像是一个点到线以及点到面相结合的ICP算法；需要注意的是odometry和mapping算法的edge line和planar patch（即匹配时的target）的确定略有不同。

• 复现的A-LOAM
  

• 关于其代码，节点关系图如下：
  

• kittiHelper
  kittiHelper.cpp 的作用是将kitti数据集转为rosbag
  代码的详细解释
  讲的很细，了解数据集构成，以及转化过程。

https://blog.csdn.net/qq_32761549/article/details/120322726

• scanRegistration.cpp
  前端lidar点预处理及特征提取

https://www.guyuehome.com/35528

• LOAM提出了一种简单而高效的特征点提取方式：根据点云点的曲率提取特征点。即把特别尖锐的边线点与特别平坦的平面点作为特征点。
  公式看起来比较复杂，实际上就是同一条扫描线上的取目标点左右两侧各5个点，分别与目标点的坐标作差，得到的结果就是目标点的曲率。当目标点处在棱或角的位置时，自然与周围点的差值较大，得到的曲率较大；反之当目标点在平面上时，周围点与目标点的坐标相近，得到的曲率自然较小。

• laserOdometry

https://zhuanlan.zhihu.com/p/396331443

• laserOdometry的订阅了5个话题：有序点云、极大平面点、次极小平面点、极小平面点、次极小平面点。发布了4个话题：有序点云、上一帧的平面点、上一帧的边线点、当前帧位姿粗估计。

while (ros::ok())
    {
        // 帧间位姿估计过程
        ...
        rate.sleep();
    }

• 帧间匹配与位姿估计
  目标：希望找到位姿变换T，使得第k帧点云左乘T得到第k+1帧点云，或者与第k+1帧点云的误差最小。
• 边线点匹配方法

方法：现在在第k+1帧中发现了点边线点i，查询在第k帧中的最近邻点j（by KD-tree），查询j的附近扫描线上的最近邻点l，j与l相连形成一条直线l-j，让点i与这条直线的距离最短。

非线性优化：以点i与直线lj的距离为代价函数，以位姿变换T(四元数+t)为优化变量，构建非线性优化问题。

• 平面点匹配方法
  

方法：现在在第k+1帧中发现了平面点i，查询在第k帧（上一帧）中的最近邻点（by KD-tree）j，查询j的附近扫描线上的最近邻点l和同一条扫描线的最近邻点m，这三点确定一个平面，让点i与这个平面的距离最短

非线性优化：以点i与平面lmj的距离为代价函数，以位姿变换T(四元数+t)为优化变量，构建非线性优化问题。

• 由于LOAM原版应用的设备是机械式激光雷达，因此势必存在一定的运动畸变（即同一个点云中，各个点在采集时，LiDAR的位姿是不同的，就如同高速移动相机时拍摄的照片一样），为此LOAM提出假设：激光雷达的运动都是匀速的，由此对点云中的点进行去畸变。

• laserMapping

https://zhuanlan.zhihu.com/p/396661160

• 这里涉及到几个坐标系的相互转换，需要理清关系：雷达坐标系（每帧扫描到的点云点的坐标point_curr都在雷达坐标系中），里程计坐标系（由laseOdometry节点粗估计得到的LiDAR位姿wodom_curr对应的坐标系），地图坐标系（真实的世界坐标系）
• laserOdometry中只能做到粗估计，而laserMapping能够做到精估计

laserMapping中使用scan to map的匹配方法，即最新的关键帧scan（绿色线）与其他所有帧组成的全部地图（黑色线）进行匹配，因此laserMapping中的位姿估计方法联系了所有帧的信息，而不是像laserOdometry中仅仅只利用了两个关键帧的信息，所以位姿估计更准确。

显然如果完全使用所有区域的点云这样做的效率很低，因此只截取scan附近的全部地图中的一个10m³的cube，用这两个cube的匹配的结果代替全局匹配结果。

• 显然，在submap的cube与全部地图的cube进行匹配时，在laserOdomerty中帧与帧之间的匹配方法就不太适用了（实际上cube中已经没有了帧的概念）。cube的匹配方法如下：

1. 取当前帧的特征点（边线点/平面点）
2. 找到全部地图特征点中，当前特征点的5个最近邻点
3. 如果是边线点，则以这五个点的均值点为中心，以5个点的主方向向量（类似于PCA方法）为方向，作直线，令该边线点与直线距离最短，构建给非线性优化问题
4. 如果是平面点，则寻找五个点的法方向（反向的PCA），令这个平面点在法方向上与五个近邻点的距离最小，构建给非线性优化问题
5. 求解能够让非线性问题代价函数最小的LiDAR位姿

• 完成精位姿估计之后，还有几点工作要做：将当前帧的特征点加入到全部地图cube中，对全部地图cube中的点进行降采样，刷新附近点云地图，刷新全部点云地图，发布当前帧的精确位姿估计和平移估计。