A_loam框架的一些学习记录和细节
开源代码地址：https://github.com/HKUST-Aerial-Robotics/A-LOAM
1、a_loam节点关系和整体框架
a-loam主要分为五个文件，kittiHelper.cpp, laser，laserMapping.cpp，laserOdometry.cpp，lidarFactor.cpp和scanRegistration.cpp。这是一个很轻量化的雷达里程计，只使用了雷达点云数据构建出了雷达里程计，代码也很清晰短小。很适合入门激光slam的第一个开源框架。
2、KittiHelper.cpp
这个文件的主要作用是将kitti官方的kitti_odom数据转化成rosbag的形式发出来，其中需要注意一点的是kitti原始数据的odom真实坐标系和雷达坐标系并不是重合的。odom坐标系是z向前的，但是雷达坐标系是z向上的。所以这里作者直接读取了真实的odom数据，然后变换了一下旋转方向就发布出去了。但如果想要准确的odom的结果，还需要根据kitti里面提供的标定的真值做一个transform。
3、scanRegistration.cpp
这个是程序接受原始点云然后处理的第一个模块，主要的作用是将原始的雷达点云进行特征提取，根据点云的曲率分为四种特征点，laser_cloud_sharp，laser_cloud_less_sharp，laser_cloud_flat，laser_cloud_less_flat。 第一个大函数laserCloudHandler，这个函数里面首先将原始点云里面的无效点去除，然后利用removeClosedPointCloud函数去除部分距离过近的点，然后根据每个点的方位角计算他的scanPeriod。这里比较巧妙的是第一个点和最后一个点直接计算方向角看能会一样，但是作者直接通过一系列调整使得最后一个点比第一个点大2PI。另一个注意的地方时halfPassed，这是用来判断每一个激光束是否已经扫描过半，这样可以保证每束点云的方位角都是递增的。一般雷达的扫描方式都是顺时针扫描的。然后将点云的intensity标签用来保存其相对时间戳。整数部分是线束，小数部分是相对时间。 然后提取特征点的部分也比较常规，将对每一线的激光分别处理，对于第i线激光，首先剔除前5个和后5个激光点，然后将这束激光均匀分成6块，对于每一块最多提取两个最尖的角点和四个最平的平面点，然后发布出去，给下一个模块使用。
4、laserOdometry.cpp
这个模块就是slam的前端了，主要的作用是发布激光频率的激光里程计，其中主要是根据scan-to-scan的方式将当前帧的激光特征点和上一帧的特征点做匹配，然后利用残差构建ceres，优化帧间的位姿变换使得达到最小的误差。需要注意的是这里的点云去畸变默认使用的是旋转式的激光雷达，其中也就假设了机器人在0.1s内是匀速运动的，这个假设还算合理。另一个是ceres优化的初值是上一个帧间变换的结果，这也是可以接受的。 首先是一个大的while循环，然后里面首先判断几个特征点云的时间戳是否一样，防止出现丢包情况。之后第一帧的特征点只接受，不做优化。对于第k+1帧的特征点，首先寻找第k帧的对应的特征点，然后将他变换到第k帧坐标系下，计算特征点之间的残差。之后将角点残差块和平面点残差块构建ceres优化模型。之后发布/laser_odom_to_init和/laser_odom_pathz。优化结束后再将当前点云变换到k+1帧坐标系下，然后进行下一轮操作。这个前端的操作需要保证在100ms内完成。优化的就是7个变量，四元素和xyz，但是这四个变量并不是独立的。