A-LOAM学习笔记

LOAM源码解析1——scanRegistration - 知乎

LOAM笔记及A-LOAM源码阅读 - WellP.C - 博客园

参考以上大佬的博客，结合自己的理解，对A-LOAM框架做一些整理

整体框架

 A-LOAM框架非常简介，一共有4个ROS节点，共有5个c++文件，分别为

kittiHelper.cpp        读取kitti odometry的数据集，包括点云、左右相机的图像、以及pose的groundtruth（训练集才有），然后分成5个topic以10Hz的频率发布出去。点云用于定位，后面4个用于rviz可视化

scanRegistration.cpp        对输入点云进行滤波，提取4种feature，边缘点特征sharp、less_sharp，面特征flat、less_flat

laserOdometry.cpp        基于4种feature进行帧与帧的点云特征配准，最终得到里程计坐标系下的激光雷达的位姿

laserMapping.cpp        基于less_sharp特征点和less_flat特征面，对帧与submap的点云特征配准，对Odometry线程计算的位姿进行优化

lidarFactor.hpp        计算laserOdometry和laserMapping中位姿变化时的残差

Ros下坐标系

1、map坐标系⼀般是固定的，通过点云匹配或者视觉特征匹配可以获得在地图坐标系下的位姿

2、odom坐标系 通常是以机器⼈上电时刻的位置作为原点，然后通过机器⼈对⾃⾝运动的估计来获取odom坐标系下的 位姿，可以理解为⾥程记构建的坐标系，通常会遭遇累计漂移的问题

3、baselink坐标系 ⻋体坐标系，通常以⻋上某个位置作为⻋体坐标系的原点，⼀般来讲，如果多个传感器需要都标定到⻋ 体系才能进⾏多传感器融合

4、lidar坐标系 我们获得的lidar点云都是在lidar坐标系下的，都是相对lidar中⼼的坐标，这也是从驱动拿到的lidar数据的所在的坐标系

odom坐标系=map坐标系+里程计累计漂移

ROS 中的三种坐标系（map、odom与base_link）_虚境的博客-CSDN博客

 kittiHelper.cpp       

主要用于ros的消息发布。

注意：kitti的训练集真值pose的坐标系和点云的坐标系不相同，为了统一，坐标系均采用点云的坐标系，所以需要将真值pose转到点云的坐标系下。

scanRegistration.cpp     

  一旦接受到一帧点云就执行一次回调函数laserCloudHandler

1 点云滤波，去除NaN值的无效点以及距离激光雷达传感器过近的点

2 通过Lidar坐标系下点的仰角以及水平夹角计算点云的scanID和时间戳

3 将子点云合并成一帧点云laserCloud，单帧点云laserCloud已经是按照scanID和时间戳顺序存放有序点云

4 计算点的曲率，曲率最大的两个点是corner_sharp特征点，最大的前20个点是corner_less_sharp特征点，曲率最小的前4点是surf_flat，其他的非corner特征点组成了surf_less_flat特征点，对surf_less_flat特征点降采样

laserOdometry.cpp     

1 进行点到线以及点到面的ICP匹配，迭代2次，使用Ceres优化

2 用最新计算的位姿增量，得到当前帧的位姿（世界坐标系下）

3 发布当前帧在世界坐标系的位姿，点/面特征点，滤波后的点云

4  更新 laserCloudCornerLast和laserCloudSurfLast以及相应的kdtree，为下一次点云特征匹配提供target

laserMapping.cpp  

Mapping线程估计当前帧在世界坐标系下的位姿，但不同于前端，这些位姿是需要优化的变量

 黄色区域就是submap，将当前帧的角/面特征与局部地图的角/面特征进行匹配，对前端计算出的位姿进行优化。

1 由于Mapping线程耗时长，为了保证LOAM算法的实时性，线程只处理最新的那部分消息，其他的缓存将被清空

2 为建图设定初始位姿。当前帧在世界坐标系下的位姿=上一帧在世界坐标系下的位姿×当前帧到上一帧的位姿变换。

3 动态调整map的位置，尽量保证当前帧在submap的中心

4 以降采样后的submap特征点云为target（世界坐标系），以当前帧降采样后的特征点云（Lidar坐标系）为source进行ICP匹配。

5 在submap的corner特征点（target）中采集距离当前特征角点最近的5个点，建立协方差矩阵，如果这5个点的呈线性特征，则需要submap利用PCA原理再次构建线ceres问题，以确保边界线足够直；

6 同理，对最近邻的5个点进行基于最小二乘的平面拟合，check拟合出的平面是不是“足够平”，更新位姿增量、更新submap：

lidarFactor.hpp        

计算库，包含不同线程下点到线、点到面的运算。