LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)

LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。

本文原地址：wykxwyc的博客

github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：

1.地图优化代码理解

2.图像重投影代码理解

3.特征关联代码理解

4.LeGO-LOAM中的数学公式推导

以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。

featureAssociation.cpp概述

featureAssociation.cpp顾名思义，进行特征关联的过程。

FeatureAssociation

FeatureAssociation()构造函数的内容如下：

1. 订阅话题:

   • "/segmented_cloud"(sensor_msgs::PointCloud2)，数据处理函数laserCloudHandler
   • "/segmented_cloud_info"(cloud_msgs::cloud_info)，数据处理函数laserCloudInfoHandler
   • "/outlier_cloud"(sensor_msgs::PointCloud2)，数据处理函数outlierCloudHandler
   • imuTopic = "/imu/data"(sensor_msgs::Imu)，数据处理函数imuHandler

2. 发布话题，这些topic有：

   • "/laser_cloud_sharp"(sensor_msgs::PointCloud2)
   • "/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)
   • "/laser_cloud_flat"(sensor_msgs::PointCloud2)
   • "/laser_cloud_less_flat"(sensor_msgs::PointCloud2)
   • "/laser_cloud_corner_last"(sensor_msgs::PointCloud2)
   • "/laser_cloud_surf_last"(cloud_msgs::cloud_info)
   • "/outlier_cloud_last"(sensor_msgs::PointCloud2)
   • "/laser_odom_to_init"(nav_msgs::Odometry)

然后初始化各类参数。

laserCloudHandler

laserCloudHandler修改点云数据的时间戳，将点云数据从ROS定义的格式转化到pcl的格式。

laserCloudInfoHandler

函数比较小:

void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn){
   
   
    timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
    segInfo = *msgIn;
    newSegmentedCloudInfo = true;
}

outlierCloudHandler

void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
   
   
    timeNewOutlierCloud = msgIn->header.stamp.toSec();
    outlierCloud->clear();
    pcl::fromROSMsg(*msgIn, *outlierCloud);
    newOutlierCloud = true;
}

imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)接触过很多次，因为它就是LOAM代码里的那个。
函数的实现：

1. 通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角；
2. 对加速度进行坐标变换(坐标变换可以参考下面这张图)；
3. 将欧拉角，加速度，速度保存到循环队列中；
4. 对速度，角速度，加速度进行积分，得到位移，角度和速度(AccumulateIMUShiftAndRotation())；

加速度坐标交换的示意图：

• 进行加速度坐标交换时将重力加速度去除，然后再进行$x$到$z$,$y$到$x$,$z$到$y$的变换。
• 去除重力加速度的影响时，需要把重力加速度分解到三个坐标轴上，然后分别去除他们分量的影响，在去除的过程中需要注意加减号（默认右手坐标系的旋转方向来看）。
• 在上面示意图中，可以简单理解为红色箭头实线分解到红色箭头虚线上（根据$pitch$进行分解）,然后再按找$roll$角进行分解。

runFeatureAssociation

void runFeatureAssociation()是featureAssociation.cpp中最主要的函数，它调用这个cpp文件中的其他函数。算法步骤如下：

1. 如果有新数据进来则执行，否则不执行任何操作；
2. 将点云数据进行坐标变换，进行插补等工作；
3. 进行光滑性计算，并保存结果；
4. 标记阻塞点(阻塞点：点云中可能出现的互相遮挡的点)；
5. 特征抽取，然后分别保存到cornerPointsSharp等等队列中去；
6. 发布cornerPointsSharp等4种类型的点云数据；
7. 预测位姿；
8. 更新变换；
9. 积分总变换；
10. 发布里程计信息及上一次点云信息；

adjustDistortion

void adjustDistortion()将点云数据进行坐标变换，进行插补等工作。

1. 对每个点进行处理，首先进行和laboshin_loam代码中的一样的坐标轴变换过程。

point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;

2. 针对每个点计算偏航角yaw，然后根据不同的偏航角，可以知道激光雷达扫过的位置有没有超过一半，计算的时候有一部分需要注意一下：
   函数原型：

// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI)，在2PI附近
// 下面过程的主要作用是调整ori大小，满足start<ori<end
float ori = -atan2(point.x, point.z);

这里分为4种情况：

• 没有转过一半，但是start-ori>M_PI/2
• 没有转过一半，但是ori-start>3/2*M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]）
• 转过一半，end-ori>3/2*PI,ori太小
• 转过一半，ori-end>M_PI/2,太大

3. 然后进行imu数据与激光数据的时间轴对齐操作。
   对齐时候有两种情况，一种不能用插补来优化，一种可以通过插补进行优化。

• 不能通过插补进行优化：imu数据比激光数据早，但是没有更后面的数据(打个比方,激光在9点时出现，imu现在只有8点的)
  这种情况下while循环是以imuPointerFront == imuPointerLast结束的：

// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
   
   
    if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
   
   
        break;
    }
     imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}

• 可以通过插补来进行数据的优化：
  这种情况只有在imu数据充足的情况下才会发生，
  进行插补时，当前timeScanCur + pointTime < imuTime[imuPointerFront]，
  而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。
  imuPointerBack是imuPointerFront的前一个imu数据指针。
  插补的代码：

int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 
                                 / (imuTime[imuPoi