A-LOAM代码结构分析

A-LOAM代码分析

A-LOAM github网址:https://github.com/HKUST-Aerial-Robotics/A-LOAM
最近看了一下a-loam的代码，在这里分享一下自己的理解。A-LOAM是基于文章LOAM写的，但A-LOAM本身并没有文章。

A-LOAM中程序主体在src文件夹下，有4个cpp文件，分别是kittiHelper.cpp、scanRegistration.cpp、laserOdometry.cpp和laserMapping.cpp。其中kittiHelper.cpp是使用kitti数据集时使用，当使用打包好的bag文件时，可以省略这一节点；scanRegistration.cpp对初始的点云数据进行筛选并根据每个点的曲率对点进行分类；laserOdometry.cpp基于ICP的原理对里程计进行了粗计算；laserMapping.cpp基于PCA的原理对点进行了精计算，并与粗计算结果结合，得到最终的结果。当四个节点都被使用时，ROS节点图如下：

1.kittiHelper
kittihelper节点是当需要使用kitti数据集时，转换格式使用的，设置可参考：https://blog.csdn.net/weixin_43863870/article/details/120044546?spm=1001.2014.3001.5502
2.scanRegistration
该节点主要对接收的点云数据进行筛选和分类，然后把分类后的点云发送到下一节点，首先是对点云进行筛选

pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

其中removeNaNFromPointCloud为pcl库中的函数，去除无效点，removeClosedPointCloud为作者自定义的函数，作用是去除过近点。然后根据每个点的坐标算出每个点所在的线数，随后求出每个点的曲率，为后面的分类做准备。
在进行分类时，把一帧中每条线上的点云六等分，在每一份中按照曲率从大到小的顺序依次取不超过两个点放入cornerPointsSharp和不超过20个点放入cornerPointsLessSharp，然后按照曲率从小到大的顺序排列，取不超过4个点放入surfPointsFlat，然后将余下的点全部放入surfPointsLessFlatScan。

3.laserOdometry
laserOdometry节点中与里程计位姿计算相关的有四个比较重要的变量，分别是q_w_curr、t_w_curr、q_last_curr和t_last_curr。其中q_w_curr和t_w_curr表示从当前帧到世界坐标系的变换矩阵，q_last_curr和t_last_curr表示的是两帧在世界坐标系下的变换矩阵。
该节点通过ICP的方法，依靠ceres优化库，对位姿（q_last_curr和t_last_curr）进行求解，主要是用到了从scanRegistration节点发来的cornerPointsSharp和surfPointsFlat。对于当前帧中cornerPointsSharp的角点，基于最邻近匹配的原理利用kdtree算法寻找上一帧中cornerPointsSharp中对应的两个最接近的角点并判断是否合格，如果合格则被记录下来。对于当前帧中surfPointsFlat的面点，也是同样的方法，基于最邻近匹配的原理利用kdtree算法寻找上一帧中surfPointsFlat中对应的两个最接近的面点并判断是否合格，如果合格则被记录下来。该节点向下一节点发送的信息为计算出的轨迹laserOdometry，节点中有一个值为skipFrameNum，即每隔skipFrameNum帧就把cornerPointsLessSharp和surfPointsLessFlatScan发送到下一节点，用laserMapping线程计算。

4.laserMapping
laserMapping线程中与位姿计算有关的变量主要有六个，分别是q_w_curr、t_w_curr、q_wodom_curr、t_wodom_curr、q_wmap_wodom和t_wmap_wodom。其中q_w_curr和t_w_curr表示当前帧到世界坐标系下的变换矩阵，q_wodom_curr和t_wodom_curr表示laserOdometry节点中计算的当前帧在世界坐标系下的坐标（即laserOdometry节点中计算的q_w_curr和t_w_curr），q_wmap_wodom和t_wmap_wodom表示的是世界坐标系下laserOdometry节点计算的位姿和laserMapping节点计算的位姿之间的转换矩阵。
该节点主要用到的数据来自cornerPointsLessSharp和surfPointsLessFlatScan，对这两个容器中的点云进行了降采样，基于PCA原理，使用ceres求解器计算出两帧之间的位姿。在函数laserOdometryHandler中，将laserOdometry节点和laserMapping节点计算的位姿结合，即可得到最终的轨迹odomAftMapped。