loam算法_LOAM源码解析2——laserOdometry

这是LOAM第二部分Lidar laserOdometry雷达里程计。 在第一章提取完特征点后，需要对特征点云进行关联匹配，之后估计姿态。 主要分为两部分：

特征点关联使用scan-to-scan方式t和t+1时刻相邻两帧的点云数据进行配准，分为边缘点匹配和平面点匹配两部分。计算点到直线的距离和点到平面的距离。 姿态解算根据匹配的特征点云使用LM算法估计接收端位姿。

这部分代码完全是放在main函数。 首先介绍前面的参数。后面出现的会重点标注下。

//一个点云周期

0、预处理

1. 订阅节点，发布消息
2. 创建里程计对象、坐标转换对象
3. 同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入

6个sub订阅器分别订阅scanRegistration节点发布的6个消息：/laser_cloud_sharp、/laser_cloud_less_sharp、pubSurfPointFlat、pubSurfPointLessFlat、/velodyne_cloud_2、/imu_trans消息。 调用6个Handler回调函数处理这些边特征、面特征、全部点云和IMU数据，把他们从ROS::Msg类型转换成程序可以处理的pcl点云类型； 4个pub发布器发布/laser_cloud_corner_last、/laser_cloud_surf_last、/velodyne_cloud_3、/laser_odom_to_init消息。

int 

接下来分为：初始化、点云处理、构建Jacobian矩阵估计位姿、坐标转换。

1、初始化

将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准。

1. 将第一个点云订阅的数据保存为上一时刻数据
2. 从ROS转化为pcl，再发布出去
3. 记住原点的翻滚角和俯仰角为imu
4. T平移量的初值赋值为imu加减速的位移量，为其梯度下降的方向

//********************1、初始化：将第一个点云数据集发送给laserMapping,从下一个点云数据开始配准****************

2、点云配准

点云配准前言：首先保证上一时刻特征点（边缘+平面）数量足够再开始进行匹配，其次，后面需要25次迭代LM算法求解位姿。 重要参数：

laserCloudOri：点云过滤：保留距离小，权重大的点；舍弃距离为零的点。 coeffSel：权重

if 

针对边缘点和平面点两部分分别进行匹配关系处理。 以边缘点为例：

1. kd-tree查找一个最近距离点j，Ind为索引，SqDis为距离平方
2. 寻找最邻近点j附近的次临近点l：分为scanID增加和减小两个方向分别进行寻找。
3. 计算点到线的距离，根据距离分配权重

边缘点处理：

/********************边缘点处理************/
          

同理，平面点也是相同步骤

/********************平面点处理************/
          

3、构建Jacobian矩阵估计位姿

LM 的计算流程为： 1. 计算matA,matB矩阵，可以Ax=B 2. 最小二乘法计算（QR分解），为了让左边满秩，同乘At-> AtAX = At*B 3. 出现退化用预测矩阵P

/*****************************3、构建Jacobian矩阵估计位姿 ********************/
          

4、坐标转换

求解的是点云间的相对运动，但他们是在这两帧点云的局部坐标系下的，我们需要把它转换到世界坐标系下，因此需要进行转换。

1. 局部坐标系转移到全局坐标系
2. 根据IMU修正旋转量，插入imu旋转，更新位姿
3. 欧拉角转换成四元数，再发布出去四元数和平移量
4. 对k+1时刻的less特征点（边缘+平面）转移至k+1时刻的sweep的结束位置处的雷达坐标系下
5. 更新，畸变校正之后的点作为last点保存等下个点云进来进行匹配
6. 按照跳帧数publich边沿点，平面点以及全部点给laserMapping(每隔一帧发一次)

1.局部坐标系转移到全局坐标系 分为两步骤，先把旋转累积量全局化，再把平移量全局化。

// 1.局部坐标系转移到全局坐标系

其中涉及到的旋转累积量函数。 AccumulateRotation() 相对于第一个点云即原点，积累旋转量.

//相对于第一个点云即原点，积累旋转量

2.根据IMU修正旋转量，插入imu旋转，更新位姿

PluginIMURotation

其中涉及到的IMU修正函数。 PluginIMURotation() 利用IMU修正旋转量，根据起始欧拉角，当前点云的欧拉角修正

void 

3.欧拉角转换成四元数，再发布出去四元数和平移量

geometry_msgs

4.对k+1时刻的less特征点（边缘+平面）转移至k+1时刻的sweep的结束位置处的雷达坐标系下

int 

其中，涉及到将当前时刻特征点投影到sweep结束位置的雷达坐标系TransformToEnd（）。这里其实一起来讲，有当前帧点云P(k+1) TransformToStart（）和上一帧点云P(k) TransformToEnd（）两个函数，为了将两帧点云去除畸变，统一到同一坐标系下计算。

TransformToStart() 当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云

void 

TransformToEnd() 当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描结束位置静止扫描得到的点云

void 

5.更新，畸变校正之后的点作为last点保存等下个点云进来进行匹配

pcl

6.按照跳帧数publich边沿点，平面点以及全部点给laserMapping(每隔一帧发一次)

if