【3D激光SLAM】LOAM源代码解析--laserOdometry.cpp

系列文章目录

·【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp
·【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp
·【3D激光SLAM】LOAM源代码解析–laserMapping.cpp
·【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp

写在前面

本系列文章将对LOAM源代码进行讲解，在讲解过程中，涉及到论文中提到的部分，会结合论文以及我自己的理解进行解读，尤其是对于其中坐标变换的部分，将会进行详细的讲解。

本来是懒得写的，一个是怕自己以后忘了，另外是我在学习过程中，其实没有感觉哪一个博主能讲解的通篇都能让我很明白，特别是坐标变换部分的代码，所以想着自己学完之后，按照自己的理解，也写一个LOAM解读，希望能对后续学习LOAM的同学们有所帮助。

之后也打算录一个LOAM讲解的视频，大家可以关注一下。

整体框架

LOAM多牛逼就不用多说了，直接开始

先贴一下我详细注释的LOAM代码，在这个版本的代码上加入了我自己的理解。

我觉得最重要也是最恶心的一部分是其中的坐标变换，在代码里面真的看着头大，所以先明确一下坐标系(都是右手坐标系)：

• IMU(IMU坐标系imu)：x轴向前，y轴向左，z轴向上
• LIDAR(激光雷达坐标系l)：x轴向前，y轴向左，z轴向上
• CAMERA(相机坐标系，也可以理解为里程计坐标系c)：z轴向前，x轴向左，y轴向上
• WORLD(世界坐标系w，也叫全局坐标系，与里程计第一帧init重合)：z轴向前，x轴向左，y轴向上
• MAP(地图坐标系map，一定程度上可以理解为里程计第一帧init)：z轴向前，x轴向左，y轴向上

坐标变换约定： 为了清晰，变换矩阵的形式与《SLAM十四讲中一样》，即：$R_{A\_B}$表示B坐标系相对于A坐标系的变换，B中一个向量通过$R_{A\_B}$可以变换到A中的向量。

首先对照ros的节点图和论文中提到的算法框架来看一下：

可以看到节点图和论文中的框架是一一对应的，这几个模块的功能如下：

• scanRegistration：对原始点云进行预处理，计算曲率，提取特征点
• laserOdometry：对当前sweep与上一次sweep进行特征匹配，计算一个快速(10Hz)但粗略的位姿估计
• laserMapping：对当前sweep与一个局部子图进行特征匹配，计算一个慢速(1Hz)比较精确的位姿估计
• transformMaintenance：对两个模块计算出的位姿进行融合，得到最终的精确地位姿估计

本文介绍laserOdometry模块，它相当于SLAM的前端，它其实是一个scan-to-scan的过程，可以得到高频率但精度略低的位姿变换，它的具体功能如下：

1. 接收特征点话题、全部点云话题、IMU话题，并保存到对应的变量中
2. 将当前sweep与上一次sweep进行特征匹配，得到edge point匹配对应的直线以及planar point匹配对应的平面
3. 计算雅可比矩阵，使用高斯牛顿法(论文中说的是LM法)进行优化，得到估计出的相邻两帧的位姿变换
4. 累积位姿变换，并用IMU修正，得到当前帧到初始帧的累积位姿变换
5. 发布话题并更新tf变换

一、main()函数以及回调函数

main()函数是很简单的，就是定义了一系列的发布者和订阅者，订阅了来自scanRegistration节点发布的话题；然后定义了一个tf发布器，发布当前帧(/laser_odom)到初始帧(/camera_init)的坐标变换；然后定义了一些列下面会用到的变量。

其中有6个订阅者，分别看一下它们的回调函数。

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserOdometry");
  ros::NodeHandle nh;

  ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_sharp", 2, laserCloudSharpHandler);

  ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
                                             ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);

  ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                      ("/laser_cloud_flat", 2, laserCloudFlatHandler);

  ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
                                          ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);

  ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> 
                                         ("/velodyne_cloud_2", 2, laserCloudFullResHandler);

  ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> 
                                ("/imu_trans", 5, imuTransHandler);

  ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_corner_last", 2);

  ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
                                         ("/laser_cloud_surf_last", 2);

  ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> 
                                        ("/velodyne_cloud_3", 2);

  ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
  nav_msgs::Odometry laserOdometry;
  laserOdometry.header.frame_id = "/camera_init";
  laserOdometry.child_frame_id = "/laser_odom";

  tf::TransformBroadcaster tfBroadcaster;
  tf::StampedTransform laserOdometryTrans;
  laserOdometryTrans.frame_id_ = "/camera_init";
  laserOdometryTrans.child_frame_id_ = "/laser_odom";

  std::vector<int> pointSearchInd;//搜索到的点序
  std::vector<float> pointSearchSqDis;//搜索到的点平方距离

  PointType pointOri, pointSel;         /*选中的特征点*/
  PointType tripod1, tripod2, tripod3;  /*特征点的对应点*/
  PointType pointProj;                  /*unused*/
  PointType coeff;                      /*直线或平面的系数*/

  //退化标志
  bool isDegenerate = false;
  //P矩阵，预测矩阵，用来处理退化情况
  cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

  int frameCount = skipFrameNum;    // skipFrameNum = 1
  ros::Rate rate(100);
  bool status = ros::ok();

接收特征点的回调函数

下面这五个回调函数的作用和代码结构都类似，都是接收scanRegistration发布的话题，分别接收了edge point、less edge point、planar point、less planar point、full cloud point(按scanID排列的全部点云)。

对于接收到点云之后都是如下操作：

• 记录时间戳
• 保存到相应变量中
• 滤除无效点
• 将接收标志设置为true

void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2)
{
  timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec();

  cornerPointsSharp->clear();
  pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
  newCornerPointsSharp = true;
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2)
{
  timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec();

  cornerPointsLessSharp->clear();
  pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices);
  newCornerPointsLessSharp = true;
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2)
{
  timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec();

  surfPointsFlat->clear();
  pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices);
  newSurfPointsFlat = true;
}

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2)
{
  timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec();

  surfPointsLessFlat->clear();
  pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices);
  newSurfPointsLessFlat = true;
}

//接收全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{
  timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();

  laserCloudFullRes->clear();
  pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices);
  newLaserCloudFullRes = true;
}

接收/imu_trans消息
这个回调函数主要是接受了scanRegistration中发布的自定义imu话题，包括当前sweep点云数据的IMU起始角、终止角、由于加减速产生的位移和速度畸变，保存到相应变量中。

//接收imu消息
void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2)
{
  timeImuTrans = imuTrans2->header.stamp.toSec();

  imuTrans->clear();
  pcl::fromROSMsg(*imuTrans2, *imuTrans);

  //根据发来的消息提取imu信息
  imuPitchStart = imuTrans->points[0].x;
  imuYawStart = imuTrans->points[0].y;
  imuRollStart = imuTrans->points[0].z;

  imuPitchLast = imuTrans->points[1].x;
  imuYawLast = imuTrans->points[1].y;
  imuRollLast = imuTrans->points[1].z;

  imuShiftFromStartX = imuTrans->points[2].x;
  imuShiftFromStartY = imuTrans->points[2].y;
  imuShiftFromStartZ = imuTrans->points[2].z;

  imuVeloFromStartX = imuTrans->points[3].x;
  imuVeloFromStartY = imuTrans->points[3].y;
  imuVeloFromStartZ = imuTrans->points[3].z;

  newImuTrans = true;
}

二、特征匹配

2.1 初始化

接收到第一帧点云数据时，先进行一次初始化，因为第一帧点云没法匹配…

这里就是直接把这一帧点云发送给laserMapping节点。

      //初始化：将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理
      if (!systemInited) {
        //将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值到laserCloudCornerLast中下轮使用
        pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
        cornerPointsLessSharp = laserCloudCornerLast;
        laserCloudCornerLast = laserCloudTemp;

        //将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值到laserCloudSurfLast中下轮使用
        laserCloudTemp = surfPointsLessFlat;
        surfPointsLessFlat = laserCloudSurfLast;
        laserCloudSurfLast = laserCloudTemp;

        //使用上一帧的特征点构建kd-tree
        kdtreeCornerLast->setInputCloud(laserCloudCornerLast);  //所有的边沿点集合
        kdtreeSurfLast->setInputCloud(laserCloudSurfLast);      //所有的平面点集合

        //将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMapping
        sensor_msgs::PointCloud2 laserCloudCornerLast2;
        pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
        laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudCornerLast2.header.frame_id = "/camera";
        pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

        sensor_msgs::PointCloud2 laserCloudSurfLast2;
        pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
        laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
        laserCloudSurfLast2.header.frame_id = "/camera";
        pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

        //记住原点的翻滚角和俯仰角
        transformSum[0] += imuPitchStart;   // IMU的y方向
        transformSum[2] += imuRollStart;    // IMU的x方向

        systemInited = true;
        continue;
      }

2.2 TransformToStart()函数

在scanRegistration模块中，有一个TransformToStartIMU()函数，上篇文章提到它的作用是：没有对点云坐标系进行变换，第i个点云依然处在里程计坐标系下的curr时刻坐标系中，只是对点云的位置进行调整。

而这个函数呢，就是要对点云的坐标系进行变换，变换到里程计坐标系下的初始时刻start坐标系中，用于与上一帧的点云数据进行匹配。

在这个函数中，首先根据每个点强度值intensity，提取出scanRegistration中计算的插值系数$s=\frac{t-t_{start}}{t_{end}-t_{start}}$，下面开始了！

首先要明确：transform中保存的变量是上一帧相对于当前帧的变换，也就是$T_{end\_start}$

而这里也体现了匀速运动假设，因为我们在这里使用transform变量时，还没有对其进行更新(迭代更新过程在特征匹配和非线性最小二乘优化中才进行)，所以这里使用的transform变量与上上帧相对于上一帧的变换相同，这也就是作者假设了每个扫描周期车辆都进行了完全相同的运动，用数学公式表示如下：
$$T_{transform}^k=T_{transform}^{k-1}=T_{end\_start}^k$$

那么问题如下：

现在我们已知的量是：当前时刻坐标系下保持imustart角的的点云$p_{curr}^{imustart}$，上一帧相对于当前帧的变换$T_{transform}$，也就是$T_{end\_start}$，使用s进行插值后有：$T_{curr\_start}=s\ast T_{end\_start}$。

需要求解的是当前sweep初始时刻坐标系的点云$p_{start}$。

推导过程：

根据坐标变换公式有：
$$\begin{gathered} p_{curr}^{imustart}=R_{curr\_start}\ast p_{start}^{imustart}+t_{curr\_start} \\ {p}_{start}^{imustart}=R_{curr\_start}^{-1}\ast (p_{curr}^{imustart}-t_{curr\_start}) \end{gathered}$$
而$R_{curr\_start}$为YXZ变换顺序(解释：当前帧相对于上一帧的变换顺序为ZXY，呢么反过来，这里是上一帧相当于当前帧的变换，就变成了YXZ)，所以$R_{curr\_start}=R_z{R}_x{R}_y$，代入得：
$$\begin{gathered} p_{start}^{imustart}=(R_z{R}_x{R}_y{)}^{-1}\ast (p_{curr}^{imustart}-t_{curr\_start}) \\ {p}_{start}^{imustart}=R_{-y}{R}_{-x}{R}_{-z}\ast (p_{curr}^{imustart}-t_{curr\_start}) \end{gathered}$$
这就推出了和代码一致的变换顺序：先减去$t_{curr\_start}$，然后绕z周旋转-rz，绕x轴旋转-rx，绕y轴旋转-ry。

//将该次sweep中每个点通过插值，变换到初始时刻start
void TransformToStart(PointType const * const pi, PointType * const po)
{
  //插值系数计算，云中每个点的相对时间/点云周期10
  float s = 10 * (pi->intensity - int(pi->intensity));

  //线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数
  //这里的transform数值上和上次一样，这里体现了匀速运动模型，就是每次时间间隔下，相对于上一次sweep的变换都是一样的
  //而在使用该函数之前进行了一次操作：transform[3] -= imuVeloFromStartX * scanPeriod;
  //这个操作是在匀速模型的基础上，去除了由于加减速造成的畸变
  
  // R_curr_start = R_YXZ
  float rx = s * transform[0];
  float ry = s * transform[1];
  float rz = s * transform[2];
  float tx = s * transform[3];
  float ty = s * transform[4];
  float tz = s * transform[5];

  /*pi是在curr坐标系下 p_curr，需要变换到当前sweep的初始时刻start坐标系下
   * 现在有关系：p_curr = R_curr_start * p_start + t_curr_start
   * 变换一下有：变换一下有：p_start = R_curr_start^{-1} * (p_curr - t_curr_start) = R_YXZ^{-1} * (p_curr - t_curr_start)
   * 代入定义量：p_start = R_transform^{-1} * (p_curr - t_transform)
   * 展开已知角：p_start = R_-ry * R_-rx * R_-rz * (p_curr - t_transform)
   */
  
  //平移后绕z轴旋转（-rz）
  float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
  float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
  float z1 = (pi->z - tz);

  //绕x轴旋转（-rx）
  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  //绕y轴旋转（-ry）
  po->x = cos(ry) * x2 - sin(ry) * z2;
  po->y = y2;
  po->z = sin(ry) * x2 + cos(ry) * z2;
  po->intensity = pi->intensity;
}

2.2 edge point匹配

寻找匹配直线：

将该次sweep中每个点通过插值，变换到初始时刻start后，迭代五次时，重新查找最近点，相当于每个匹配迭代优化4次，A-LOAM代码中的Ceres代码的最大迭代次数为4。

然后先试用PCL中的KD-tree查找当前点在上一帧中的最近邻点，对应论文中提到的j点，最近邻在上一帧中的索引保存在pointSearchInd中，距离保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd对应论文中的j点，minPointInd2对应论文中的l点

int closestPointInd = -1, minPointInd2 = -1;

如果查找到了最近邻的j点，就按照论文中所说，需要查找上一帧中，与j线号scanID相邻且j的最近邻点l，j和l构成与待匹配点i的匹配直线edge。

下面这两个for循环做了这么一件事情：向索引增大的方向查找(scanID>=j点的scanID)，如果查找到了相邻帧有距离更小的点，就更新最小距离和索引；然后向索引减小的方向查找(scanID<=j点的scanID)，如果查找到了相邻帧有距离更小的点，就更新最小距离和索引。然后将每个待匹配点对应的j和l点的索引保存在pointSearchCornerInd1[]和pointSearchCornerInd2[]中。

计算直线参数：

当找到了j点和l点，就可以进行特征匹配，计算匹配直线的系数，对应论文中的公式为：
$$d_{ϵ}=\frac{|({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L)\times ({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,l)}^L)|}{|{\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L|}$$

这个公式的分子是i和j构成的向量与i和l构成的向量的叉乘，得到了一个与ijl三点构成平面垂直的向量，而叉乘取模其实就是一个平行四边形面积；分母是j和l构成的向量，取模后为平行四边形面积的底，二者相除就得到了i到直线jl的距离，下面是图示：

代码中变量代表的含义：

• x0：i点
• x1：j点
• x2：l点
• a012：论文中残差的分子(两个向量的叉乘)
• la、lb、lc：i点到jl线垂线方向的向量(jl方向的单位向量与ijl平面的单位法向量的叉乘得到)在xyz三个轴上的分量
• ld2：点到直线的距离，即$d_{ϵ}$

下面这个s可以看做一个权重，距离越大权重越小，距离越小权重越大，得到的权重范围<=1,其实就是在求解非线性最小二乘问题时的核函数，s的本质定义如下：
$$s=\{\begin{array}{l}1,if||f(x)||\le 0.5\\ 0,otherwise\end{array}$$

最后将权重大(>0.1)的，即距离比较小，且距离不为零的点放入laserCloudOri，对应的直线系数放入coeffSel。

        //论文中提到的Levenberg-Marquardt算法(L-M method)，其实是高斯牛顿算法
        for (int iterCount = 0; iterCount < 25; iterCount++) {
          laserCloudOri->clear();
          coeffSel->clear();

          //处理当前点云中的曲率最大的特征点,从上个点云中曲率比较大的特征点中找两个最近距离点，一个点使用kd-tree查找，另一个根据找到的点在其相邻线找另外一个最近距离的点
          for (int i = 0; i < cornerPointsSharpNum; i++) {
            //将该次sweep中每个点通过插值，变换到初始时刻start
            TransformToStart(&cornerPointsSharp->points[i], &pointSel);

            //每迭代五次，重新查找最近点，相当于每个匹配迭代优化4次，ALOAM代码中的Ceres代码的最大迭代次数为4
            if (iterCount % 5 == 0) {
              std::vector<int> indices;
              pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
              //kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波
              kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
              int closestPointInd = -1, minPointInd2 = -1;
              //closestPointInd对应论文中的j点，minPointInd2对应论文中的l点
              
              //循环是寻找相邻线最近点l
              //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
              if (pointSearchSqDis[0] < 25) {//找到的最近点距离的确很近的话
                closestPointInd = pointSearchInd[0];
                //提取最近点线号
                int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                float pointSqDis, minPointSqDis2 = 25;//初始门槛值25米，可大致过滤掉scanID相邻，但实际线不相邻的值
                //寻找距离目标点最近距离的平方和最小的点
                for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {//向scanID增大的方向查找
                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {//非相邻线
                    break;
                  }

                  pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                               (laserCloudCornerLast->points[j].x - pointSel.x) + 
                               (laserCloudCornerLast->points[j].y - pointSel.y) * 
                               (laserCloudCornerLast->points[j].y - pointSel.y) + 
                               (laserCloudCornerLast->points[j].z - pointSel.z) * 
                               (laserCloudCornerLast->points[j].z - pointSel.z);

                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
                    if (pointSqDis < minPointSqDis2) {//距离更近，要小于初始值25米
                        //更新最小距离与点序
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  }
                }

                for (int j = closestPointInd - 1; j >= 0; j--) {//向scanID减小的方向查找
                  if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
                    break;
                  }

                  pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                               (laserCloudCornerLast->points[j].x - pointSel.x) + 
                               (laserCloudCornerLast->points[j].y - pointSel.y) * 
                               (laserCloudCornerLast->points[j].y - pointSel.y) + 
                               (laserCloudCornerLast->points[j].z - pointSel.z) * 
                               (laserCloudCornerLast->points[j].z - pointSel.z);

                  if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) {
                    if (pointSqDis < minPointSqDis2) {
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  }
                }
              }

              //记录每一个需要匹配的edge point找到的点组成线的点序
              pointSearchCornerInd1[i] = closestPointInd;   //kd-tree最近距离点，-1表示未找到满足的点
              pointSearchCornerInd2[i] = minPointInd2;      //另一个最近的，-1表示未找到满足的点
            }

            if (pointSearchCornerInd2[i] >= 0) {//大于等于0，不等于-1，说明两个点都找到了，那么就开始计算残差
              tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]]; //j点
              tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]]; //l点

              //选择的特征点记为i，kd-tree最近距离点记为j，另一个最近距离点记为l
              float x0 = pointSel.x;
              float y0 = pointSel.y;
              float z0 = pointSel.z;
              float x1 = tripod1.x;
              float y1 = tripod1.y;
              float z1 = tripod1.z;
              float x2 = tripod2.x;
              float y2 = tripod2.y;
              float z2 = tripod2.z;

              //向量ij = (x0 - x1, y0 - y1, z0 - z1), 向量il = (x0 - x2, y0 - y2, z0 - z2)，向量jl = （x1 - x2, y1 - y2, z1 - z2）
              //向量ij il的向量积(即叉乘)为：
              //|  i      j      k  |
              //|x0-x1  y0-y1  z0-z1|
              //|x0-x2  y0-y2  z0-z2|
              //模(论文中的分子)为：
              float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                         * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                         + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                         * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                         + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                         * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

              //两个最近距离点之间的距离，即向量jl的模(论文中的分母)
              float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

              //jl方向的单位向量与ijl平面的单位法向量的叉乘在各轴上的分量（点到直线距离的方向）
              //x轴分量i
              float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                       + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

              //y轴分量j
              float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                       - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

              //z轴分量k
              float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                       + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

              //点到线的距离，d = |向量OA 叉乘 向量OB|/|AB|
              float ld2 = a012 / l12;

              //unused
              pointProj = pointSel;
              pointProj.x -= la * ld2;
              pointProj.y -= lb * ld2;
              pointProj.z -= lc * ld2;

              //这个s就是核函数
              //  {1 - 1.8 * ||f(X)||,   ||f(X)||<0.5
              //s=|
              //  {0,   otherwise
              //权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1
              float s = 1;
              if (iterCount >= 5) {//5次迭代之后开始增加权重因素
                s = 1 - 1.8 * fabs(ld2);
              }

              //考虑权重
              coeff.x = s * la;
              coeff.y = s * lb;
              coeff.z = s * lc;
              coeff.intensity = s * ld2;

              if (s > 0.1 && ld2 != 0) {//只保留权重大的，也即距离比较小的点，同时也舍弃距离为零的
                laserCloudOri->push_back(cornerPointsSharp->points[i]);
                coeffSel->push_back(coeff);
              }
            }
          }

2.3 planar point匹配

寻找匹配平面：

将该次sweep中每个点通过插值，变换到初始时刻start后，迭代五次时，重新查找最近点，相当于每个匹配迭代优化4次，A-LOAM代码中的Ceres代码的最大迭代次数为4。

然后先试用PCL中的KD-tree查找当前点在上一帧中的最近邻点，对应论文中提到的j点，最近邻在上一帧中的索引保存在pointSearchInd中，距离保存在pointSearchSqDis中。

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd对应论文中的j点、minPointInd2对应论文中的l点、minPointInd3对应论文中的m点。

int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;

如果查找到了最近邻的j点，就按照论文中所说，需要查找上一帧中，与j线号scanID相同且j的最近邻点l，然后查找一个与j线号scanID相邻且最近邻的点m。

下面这两个for循环做了这么一件事情：向索引增大的方向查找(scanID>=j点的scanID)，如果查找到了相邻线号的点j和相同线号的点m，有距离更小的点，就更新最小距离和索引；然后向索引减小的方向查找(scanID<=j点的scanID)，如果查找到了相邻线号的点j和相同线号的点m，就更新最小距离和索引。然后将每个待匹配点对应的j、l点、m点的索引保存在pointSearchSurfInd1[]、pointSearchSurfInd2[]和pointSearchSurfInd3[]中。

计算平面参数：

当找到了j点、l点和m点，就可以进行特征匹配，计算匹配直线的系数，对应论文中的公式为：
$$d_H=\frac{|({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L)\cdot (({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L)\times ({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L))|}{|({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L)\times ({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L)|}$$

这个公式的分母是一个lj向量和mj向量叉乘的形式，表示的是jlm平面的法向量的模；而分子是ij向量与jlm平面的法向量的向量积，向量积有一个意义是一个向量在另一个向量方向上的投影，这里就表示ij向量在jlm平面的法向量方向上的投影，也就是i到平面的距离，下面是图示：

代码中变量代表的含义：

• tripod1：j点
• tripod2：l点
• tripod3：m点
• pa、pb、pc：jlm平面的法向量在xyz三个轴上的分量，也可以理解为平面方程Ax+By+Cz+D=0的ABC系数
• pd：为平面方程的D系数
• ps：法向量的模
• pd2：点到平面的距离(将平面方程的系数归一化后，代入点的坐标，其实就是点到平面距离公式，即可得到点到平面的距离)

下面这个s可以看做一个权重，对应于论文中的这一部分

距离越大权重越小，距离越小权重越大，得到的权重范围<=1,其实就是在求解非线性最小二乘问题时的核函数，s的本质定义如下：
$$s=\{\begin{array}{l}1,if||f(x)||\le 0.5\\ 0,otherwise\end{array}$$

最后将权重大(>0.1)的，即距离比较小，且距离不为零的点放入laserCloudOri，对应的平面系数放入coeffSel。

          //对本次接收到的曲率最小的点,从上次接收到的点云曲率比较小的点中找三点组成平面，一个使用kd-tree查找，另外一个在同一线上查找满足要求的，第三个在不同线上查找满足要求的
          for (int i = 0; i < surfPointsFlatNum; i++) {
            //将该次sweep中每个点通过插值，变换到初始时刻start
            TransformToStart(&surfPointsFlat->points[i], &pointSel);

            if (iterCount % 5 == 0) {
              //kd-tree最近点查找，在经过体素栅格滤波之后的平面点中查找，一般平面点太多，滤波后最近点查找数据量小
              kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
              
              //closestPointInd对应论文中的j、minPointInd2对应论文中的l、minPointInd3对应论文中的m
              int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
              if (pointSearchSqDis[0] < 25) {
                closestPointInd = pointSearchInd[0];
                int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);

                float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
                for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { //向上查找
                  if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) {
                    break;
                  }

                  pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 
                               (laserCloudSurfLast->points[j].x - pointSel.x) + 
                               (laserCloudSurfLast->points[j].y - pointSel.y) * 
                               (laserCloudSurfLast->points[j].y - pointSel.y) + 
                               (laserCloudSurfLast->points[j].z - pointSel.z) * 
                               (laserCloudSurfLast->points[j].z - pointSel.z);

                  if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) {//如果点的线号小于等于最近点的线号(应该最多取等，也即同一线上的点)
                     if (pointSqDis < minPointSqDis2) {
                       minPointSqDis2 = pointSqDis;
                       minPointInd2 = j;
                     }
                  } else {//如果点处在大于该线上，也就是在相邻线上的m点
                     if (pointSqDis < minPointSqDis3) {
                       minPointSqDis3 = pointSqDis;
                       minPointInd3 = j;
                     }
                  }
                }

                
                for (int j = closestPointInd - 1; j >= 0; j--) {    //向下查找
                  if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) {
                    break;
                  }

                  pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 
                               (laserCloudSurfLast->points[j].x - pointSel.x) + 
                               (laserCloudSurfLast->points[j].y - pointSel.y) * 
                               (laserCloudSurfLast->points[j].y - pointSel.y) + 
                               (laserCloudSurfLast->points[j].z - pointSel.z) * 
                               (laserCloudSurfLast->points[j].z - pointSel.z);

                  if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) {//同一线上
                    if (pointSqDis < minPointSqDis2) {
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  } else {
                    if (pointSqDis < minPointSqDis3) {//下面线上
                      minPointSqDis3 = pointSqDis;
                      minPointInd3 = j;
                    }
                  }
                }
              }

              pointSearchSurfInd1[i] = closestPointInd;//kd-tree最近距离点,-1表示未找到满足要求的点
              pointSearchSurfInd2[i] = minPointInd2;//同一线号上的距离最近的点，-1表示未找到满足要求的点
              pointSearchSurfInd3[i] = minPointInd3;//不同线号上的距离最近的点，-1表示未找到满足要求的点
            }

            if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {//找到了三个点
              tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//j点
              tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//l点
              tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//m点

              //向量jl = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
              //向量jm = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)

              //向量jl jm的向量积（即叉乘），得到的是法向量，pa、pb、pc为其在三个方向上的分量
              //平面方程Ax+By+Cz+D=0,pa、pb、pc、pd分别为归一化系数
              float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 
                       - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
              float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 
                       - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
              float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 
                       - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
              float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);

              //法向量的模
              float ps = sqrt(pa * pa + pb * pb + pc * pc);
              //pa pb pc为法向量各方向上的单位向量
              pa /= ps;
              pb /= ps;
              pc /= ps;
              pd /= ps;

              //点到面的距离：向量OA与与法向量的点积除以法向量的模
              //也可以理解为将A点代入平面方程
              float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

              //unused
              pointProj = pointSel;
              pointProj.x -= pa * pd2;
              pointProj.y -= pb * pd2;
              pointProj.z -= pc * pd2;

              //同理计算权重
              float s = 1;
              if (iterCount >= 5) {
                s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                  + pointSel.y * pointSel.y + pointSel.z * pointSel.z));
              }

              //考虑权重后，平面的系数
              coeff.x = s * pa;
              coeff.y = s * pb;
              coeff.z = s * pc;
              coeff.intensity = s * pd2;

              if (s > 0.1 && pd2 != 0) {
                  //保存原始点与相应的系数
                laserCloudOri->push_back(surfPointsFlat->points[i]);
                coeffSel->push_back(coeff);
              }
            }
          }

三、高斯牛顿优化

3.1 计算雅可比矩阵并求解增量

在代码中，作者是纯手推的高斯牛顿算法，这种方式相比于使用Ceres等工具，会提高运算速度，但是计算雅克比矩阵比较麻烦，需要清晰的思路和扎实的数学功底，下面我们一起来推导一下。

以edge point匹配为例，planar point是一样的。

设误差函数(点到直线的距离)为：
$$f(X)=D(p_{start}^i,p_{start}^t)$$
其中，X为待优化变量，也就是transform[6]中存储的变量，表示3轴旋转rx、ry、rz和3轴平移量tx、ty、tz；D()表示两点之间的距离，其计算公式为：
$$D(p_{start}^i,p_{start}^t)=\sqrt{(p_{start}^i-p_{start}^t{)}^T(p_{start}^i-p_{start}^t)}$$

$p_{start}^i$表示start时刻坐标系下待匹配点i；$p_{start}^t$表示start时刻坐标系下点i到直线jl的垂点；另外根据之前TransformToStart()函数推导过的坐标变换有：
p s t a r t i = R c u r r _ s t a r t − 1 ∗ ( p c u r r − 1 − t c u r r _ s t a r t ) = [ c r y c r z − s r x s r y s r z − c r y s r z + s r x s r y c r z − c r x s r y − c r x s r z c r x c r z s r x s r y c r z + s r x c r y s r z s r y s r z − s r x c r y c r z c r x c r y ] ⋅ [ p c u r r − t x p c u r r − t y p c u r r − t z ] p_{start}^i = R_{curr\_start}^{-1}*(p_{curr}^{-1}-t_{curr\_start}) = \left[ \begin{matrix} crycrz-srxsrysrz& -crysrz+srxsrycrz& -crxsry\\ -crxsrz& crxcrz& srx\\ srycrz+srxcrysrz& srysrz-srxcrycrz& crxcry\\ \end{matrix} \right] \cdot \left[ \begin{array}{c} p_{curr}-tx\\ p_{curr}-ty\\ p_{curr}-tz\\ \end{array} \right] pstarti​=Rcurr_start−1​∗(pcurr−1​−tcurr_start​)= ​crycrz−srxsrysrz−crxsrzsrycrz+srxcrysrz​−crysrz+srxsrycrzcrxcrzsrysrz−srxcrycrz​−crxsrysrxcrxcry​ ​⋅ ​pcurr​−txpcurr​−typcurr​−tz​ ​

根据链式法则，f(x)对X求导有：
$$\frac{\partial f(x)}{\partial X}=\frac{\partial f(x)}{\partial D(p_{start}^i,p_{start}^t)}\ast \frac{\partial D(p_{start}^i,p_{start}^t)}{\partial p_{start}^i}\ast \frac{\partial p_{start}^i}{\partial X}$$

对其中每一项进行计算：
$$\begin{gathered} \frac{\partial f(x)}{\partial D(p_{start}^i,p_{start}^t)}=1 \\ \frac{\partial D(p_{start}^i,p_{start}^t)}{\partial p_{start}^i}=\frac{\partial \sqrt{(p_{start}^i-p_{start}^t{)}^T(p_{start}^i-p_{start}^t)}}{\partial p_{start}^i}=\frac{p_{start}^i-p_{start}^t}{D(p_{start}^i,p_{start}^t)} \end{gathered}$$
D对$p_{start}^i$求导的结果其实就是 进行归一化后的点到直线向量，它在xyz三个轴的分量就是前面求解出来的la、lb、lc变量。

$$\frac{\partial p_{start}^i}{\partial X}=\frac{\partial R_{curr\_start}^{-1}\ast (p_{curr}^{-1}-t_{curr\_start})}{\partial [rx,ry,rz,tx,ty,tz{]}^T}$$

用上面$p_{start}^i$的结果，分别对rx,ry,rz,tx,ty,tz求导，将得到的结果(3x6矩阵)再与D对$p_{start}^i$求导的结果(1x3矩阵)相乘，就可以得到代码中显示的结果(1x6矩阵)，分别赋值到matA的6个位置，matB是残差项。

最后使用opencv的QR分解求解增量X即可。

          //满足要求的特征点至少10个，特征匹配数量太少弃用此帧数据，不再进行优化步骤
          if (pointSelNum < 10) {
            continue;
          }

          // Hx=g
          cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));     // J
          cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));    // J^T
          cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));             // H = J^T * J
          cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));     // e
          cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));             // g = -J * e
          cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));               // x

          //计算matA,matB矩阵
          for (int i = 0; i < pointSelNum; i++) {
            pointOri = laserCloudOri->points[i];
            coeff = coeffSel->points[i];

            float s = 1;

            float srx = sin(s * transform[0]);
            float crx = cos(s * transform[0]);
            float sry = sin(s * transform[1]);
            float cry = cos(s * transform[1]);
            float srz = sin(s * transform[2]);
            float crz = cos(s * transform[2]);
            float tx = s * transform[3];
            float ty = s * transform[4];
            float tz = s * transform[5];

            float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z 
                      + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
                      + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
                      + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
                      + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
                      + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;

            float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x 
                      + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z 
                      + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) 
                      + s*tz*crx*cry) * coeff.x
                      + ((s*cry*crz - s*srx*sry*srz)*pointOri.x 
                      + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
                      + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) 
                      - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;

            float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
                      + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
                      + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
                      + s*ty*crx*srz + s*tx*crx*crz) * coeff.y
                      + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
                      + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;

            float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y 
                      - s*(crz*sry + cry*srx*srz) * coeff.z;
  
            float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y 
                      - s*(sry*srz - cry*crz*srx) * coeff.z;
  
            float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;

            float d2 = coeff.intensity;

            matA.at<float>(i, 0) = arx;
            matA.at<float>(i, 1) = ary;
            matA.at<float>(i, 2) = arz;
            matA.at<float>(i, 3) = atx;
            matA.at<float>(i, 4) = aty;
            matA.at<float>(i, 5) = atz;
            matB.at<float>(i, 0) = -0.05 * d2;
          }
          cv::transpose(matA, matAt);
          matAtA = matAt * matA;
          matAtB = matAt * matB;
          //求解matAtA * matX = matAtB
          cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

3.2 退化处理

代码中使用的退化处理在Ji Zhang的这篇论文(《On Degeneracy of Optimization-based State Estimation Problems》)中有详细论述。

简单来说，作者认为退化只可能发生在第一次迭代时，先对$H=J^T\ast J$矩阵求特征值，然后将得到的特征值与阈值(代码中为10)进行比较，如果小于阈值则认为该特征值对应的特征向量方向发生了退化，将对应的特征向量置为0，然后按照论文中所述，计算一个P矩阵：
$$P=V_f^{-1}\ast V_u$$

$V_f$是原来的特征向量矩阵，$V_u$是将退化方向置为0后的特征向量矩阵，然后用P矩阵与原来得到的解相乘，得到最终解：

$$X‘=P\ast X^{\ast }$$

          if (iterCount == 0) {
            //特征值1*6矩阵
            cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
            //特征向量6*6矩阵
            cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

            //求解特征值/特征向量
            cv::eigen(matAtA, matE, matV);
            matV.copyTo(matV2);

            isDegenerate = false;
            //特征值取值门槛
            float eignThre[6] = {10, 10, 10, 10, 10, 10};
            for (int i = 5; i >= 0; i--) {//从小到大查找
              if (matE.at<float>(0, i) < eignThre[i]) {//特征值太小，则认为处在退化环境中，发生了退化
                for (int j = 0; j < 6; j++) {//对应的特征向量置为0
                  matV2.at<float>(i, j) = 0;
                }
                isDegenerate = true;
              } else {
                break;
              }
            }

            //计算P矩阵
            matP = matV.inv() * matV2;  // 论文中对应的Vf^-1 * V_u`
          }

          if (isDegenerate) {//如果发生退化，只使用预测矩阵P计算
            cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
            matX.copyTo(matX2);
            matX = matP * matX2;
          }

3.3 迭代更新

最后进行迭代更新，如果更新量很小则终止迭代。

          //累加每次迭代的旋转平移量
          transform[0] += matX.at<float>(0, 0);
          transform[1] += matX.at<float>(1, 0);
          transform[2] += matX.at<float>(2, 0);
          transform[3] += matX.at<float>(3, 0);
          transform[4] += matX.at<float>(4, 0);
          transform[5] += matX.at<float>(5, 0);

          for(int i=0; i<6; i++){
            if(isnan(transform[i]))//判断是否非数字
              transform[i]=0;
          }
          //计算旋转平移量，如果很小就停止迭代
          float deltaR = sqrt(
                              pow(rad2deg(matX.at<float>(0, 0)), 2) +
                              pow(rad2deg(matX.at<float>(1, 0)), 2) +
                              pow(rad2deg(matX.at<float>(2, 0)), 2));
          float deltaT = sqrt(
                              pow(matX.at<float>(3, 0) * 100, 2) +
                              pow(matX.at<float>(4, 0) * 100, 2) +
                              pow(matX.at<float>(5, 0) * 100, 2));

          if (deltaR < 0.1 && deltaT < 0.1) {//迭代终止条件
            break;
          }

再次总结一下整个流程：

1. 一共迭代25次，每次分为edge point和planar point两个处理过程
2. 每迭代5次时，重新寻找最近点
3. 其他情况时，根据找到的最近点和待匹配点，计算匹配得到的直线和平面方程
4. 根据计算匹配得到的直线和平面方程，计算雅可比矩阵，并求解迭代增量
5. 如果是第一次迭代，判断是否发生退化
6. 更新迭代增量，并判断终止条件

四、更新当前帧到世界坐标系的累积变换

4.1 旋转量累积–AccumulateRotation()函数

这里需要更新的变量是transformSum，它表示的是里程计坐标系下当前帧(end)相对于初始帧(init)的变换，用数学公式表达为：
$$R_{transformSum}=R_{init\_end}=R_{init\_start}\ast R_{start\_end}$$

用已知量 上一帧相对于当前帧的变换transform和上一帧相对于初始时刻的变换transformSum表示为：
$$\begin{gathered} R_{transformSum}=R_{init\_start}\ast R_{start\_end}=R_{transformSum}\ast R_{transform}^{-1} \\ {R}_{transformSum}=R_{ZXY}=R_y{R}_x{R}_z \\ {R}_{transform}^{-1}=R_{YXZ}^{-1}=R_{-y}{R}_{-x}{R}_{-z} \end{gathered}$$

注意了，该函数传入变量时，传入的是transform变量的负值，如下：

AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);

那么安装程序中的表达，$R_{transformSum}$和$R_{transform}$有如下表达：
R t r a n s f o r m − 1 = [ c l y c l z + s l x s l y s l z c l y s l z + s l x s l y c l z c l x s l y c l x s l z c l x c l z − s l x − s l y c l z + s l x c l y s l z s l y s l z + s l x c l y c l z c l x c l y ] R t r a n s f o r m S u m = [ c c y c c z + s c x s c y s c z c c y s c z + s c x s c y c c z c c x s c y c c x s c z c c x c c z − s c x − s c y c c z + s c x c c y s c z s c y s c z + s c x c c y c c z c c x c c y ] R_{transform}^{-1} = \left[ \begin{matrix} clyclz+slxslyslz& clyslz+slxslyclz& clxsly\\ clxslz& clxclz& -slx\\ -slyclz+slxclyslz& slyslz+slxclyclz& clxcly\\ \end{matrix} \right] \\ R_{transformSum}=\left[ \begin{matrix} ccyccz+scxscyscz& ccyscz+scxscyccz& ccxscy\\ ccxscz& ccxccz& -scx\\ -scyccz+scxccyscz& scyscz+scxccyccz& ccxccy\\ \end{matrix} \right] Rtransform−1​= ​clyclz+slxslyslzclxslz−slyclz+slxclyslz​clyslz+slxslyclzclxclzslyslz+slxclyclz​clxsly−slxclxcly​ ​RtransformSum​= ​ccyccz+scxscysczccxscz−scyccz+scxccyscz​ccyscz+scxscycczccxcczscyscz+scxccyccz​ccxscy−scxccxccy​ ​

二者相乘后的到矩阵的形式与$R_{transformSum}$一致，那么通过对应位置相等可以得到：
$$\begin{gathered} ox=-arcsin(R_{2,3})=-arcsin(ccx\ast scz\ast clx\ast sly-ccx\ast ccz\ast slx-scx\ast clx\ast cly) \\ oy=arctan2(R_{1,3}/R_{3,3}) \\ oz=arctan2(R_{2,1}/R_{2,2}) \end{gathered}$$

oy、oz展开运算，就得到了代码中的样子

//相对于第一个点云即原点，积累旋转量,计算R_transformSum
void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, 
                        float &ox, float &oy, float &oz)
{
  /*  R_transformSum
   *  = R_init_end
   *  = R_init_start * R_start_end
   *  = R_transformSum * R_transform^{-1}
   *  = R_ZXY * R_YXZ^{-1}
   *  = R_cy * R_cx * R_cz * R_ly * R_lx * R_rz (这里本来应该是负的，但是这里传入函数时已经加了负号)
   */
  float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
  ox = -asin(srx);
  // ox = arcsin(R_2:3)

  float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
               + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
  float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
               - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
  oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

  float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
               + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
  float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
               - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
  oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
}

4.2 平移量修正与累积

先对这里涉及到的几个变量进行说明：

• transform：里程计end时刻坐标系下，相对位移增量$t_{end\_start}$
• imuShiftFromStartXYZ：里程计start时刻坐标系中由于加减速产生的运动畸变
• rx、ry、rz：构成累积后的当前帧end相对于第一帧init的变换$R_{init\_end}$

IMU修正的意义是： transform变量，纯粹是我们在匀速运动假设前提下使用特征点匹配计算出的变换，实际情况下汽车肯定不是纯粹匀速运动，特别是自动驾驶车，频繁地加减速是很常见的，这种情况下匀速运动假设非常不合理，而IMU的好处就是可以计算出由于加减速产生的运动畸变，二者融合使得最终得到的结果更加接近真实值。

那么transform-imuShiftFromStartXYZ表示的就是：里程计end时刻坐标系中的位移增量减去了由于加减速产生的运动畸变(相当于用IMU修正了位移)

然后变换到里程计初始帧init中，使用的旋转矩阵为：$R_{transformSum}=R_{ZXY}=R_y{R}_x{R}_z$

最后一步，更新当前帧end相当于里程计初始帧init的位移变换：
$$t_{init\_end}=t_{init\_start}+t_{start\_end}=t_{init\_start}-t_{end\_start}$$

这样就完成了IMU对平移量的修正以及累计过程，代码如下：

/* transform：end系中相对位移增量t_end_start
       * imuShiftFromStartX：start系中由于加减速产生的运动畸变
       * - imuShiftFromStartX：end系中由于加减速产生的运动畸变
       * 所以这里的操作为：将t_end_start去除畸变后，变换到init坐标系下
       */
      //绕z轴旋转 rz
      float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 
               - sin(rz) * (transform[4] - imuShiftFromStartY);
      float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 
               + cos(rz) * (transform[4] - imuShiftFromStartY);
      float z1 = transform[5] * 1.05 - imuShiftFromStartZ;

      //绕x轴旋转 rx
      float x2 = x1;
      float y2 = cos(rx) * y1 - sin(rx) * z1;
      float z2 = sin(rx) * y1 + cos(rx) * z1;

      //绕y轴旋转 ty
      //然后求相对于原点的平移量:t_init_end = t_init_start - t_end_start
      tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
      ty = transformSum[4] - y2;
      tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

4.3 旋转量修正–PluginIMURotation()函数

终于来到了坐标变换最恶心的一部分！

现在我们手里有之前由特征匹配计算得到的在里程计坐标系下当前时刻end相对于初始时刻init的坐标变换$R_{init\_end}$，有IMU测量得到的当前sweep的起始RPY角组成的在世界坐标系下的位姿变换$R_{w\_start}$和终止RPY角组成的在世界坐标系下$R_{w\_end}$；由于有一些"剧烈"的"漂移"，我们的旋转矩阵可能会有一些误差，我们既然有了IMU，那么就需要用IMU对这个值进行稍微的修正。

首先我们理一下这部分逻辑：基于匀速运动假设，从当前帧的start时刻到当前帧的end时刻，所有点云都将保持一个姿态imuRPYStart，但事实是由于加减速的影响，到end时刻时，产生了运动畸变，这时就需要使用IMU测得的imuRPYLast进行运动畸变的去除。

这部分我的理解是：类比于平移量的修正，平移量修正中求解了一个imuShiftFromStartXYZ这样的量，这是由于加减速产生的运动畸变，是用"imuShiftCur-imuShiftStart-imuVelStart * period"得到的，也就是当前"位移-匀速运动位移"；那么在旋转这里匀速运动下的旋转矩阵就是I单位矩阵，那么由于加减速产生的在end系中的旋转畸变就是"$R_{w\_end}^{-1}\ast (I\ast R_{w\_start})$"，现在我们优化得到了在里程计坐标系下当前时刻end相对于初始时刻init的坐标变换$R_{init\_end}$，需要去除这个畸变，那么就得到了下式：

我也不知道对不对，因为我也没看到或者想到一个很合理的解释

计算公式为：
$$\begin{array}{rl}R{‘}_{init\_end}& =R_{init\_end}\ast (R_{w\_end}^{-1}\ast (I\ast R_{w\_start}){)}^{-1}\\ & =R_{init\_end}\ast R_{w\_start}^{-1}\ast R_{w\_end}\\ & =R_{ZXY}\ast R_{ZXY}^{-1}\ast R_{ZXY}\end{array}$$
R ‘ i n i t _ e n d = [ c a c y c a c z + s a c x s a c y s a c z c a c y s a c z + s a c x s a c y c a c z c a c x s a c y c a c x s a c z c a c x c a c z − s a c x − s a c y c a c z + s a c x c a c y s a c z s a c y s a c z + s a c x c a c y c a c z c a c x c a c y ] R`_{init\_end}=\left[ \begin{matrix} cacycacz+sacxsacysacz& cacysacz+sacxsacycacz& cacxsacy\\ cacxsacz& cacxcacz& -sacx\\ -sacycacz+sacxcacysacz& sacysacz+sacxcacycacz& cacxcacy\\ \end{matrix} \right] R‘init_end​= ​cacycacz+sacxsacysaczcacxsacz−sacycacz+sacxcacysacz​cacysacz+sacxsacycaczcacxcaczsacysacz+sacxcacycacz​cacxsacy−sacxcacxcacy​ ​
R w _ s t a r t − 1 = [ c b l y c b l z − s b l x s b l y s b l z − c b l x s b l z s b l y c b l z + s b l x c b l y s b l z − c b l y s b l z + s b l x s b l y c b l z c b l x c b l z s b l y s b l z − s b l x c b l y c b l z − c b l x s b l y s b l x c b l x c b l y ] R_{w\_start}^{-1}=\left[ \begin{matrix} cblycblz-sblxsblysblz& -cblxsblz& sblycblz+sblxcblysblz\\ -cblysblz+sblxsblycblz& cblxcblz& sblysblz-sblxcblycblz\\ -cblxsbly& sblx& cblxcbly\\ \end{matrix} \right] Rw_start−1​= ​cblycblz−sblxsblysblz−cblysblz+sblxsblycblz−cblxsbly​−cblxsblzcblxcblzsblx​sblycblz+sblxcblysblzsblysblz−sblxcblycblzcblxcbly​ ​
R w _ e n d = [ c a l y c a l z + s a l x s a l y s a l z c a l y s a l z + s a l x s a l y c a l z c a l x s a l y c a l x s a l z c a l x c a l z − s a l x − s a l y c a l z + s a l x c a l y s a l z s a l y s a l z + s a l x c a l y c a l z c a l x c a l y ] R_{w\_end}=\left[ \begin{matrix} calycalz+salxsalysalz& calysalz+salxsalycalz& calxsaly\\ calxsalz& calxcalz& -salx\\ -salycalz+salxcalysalz& salysalz+salxcalycalz& calxcaly\\ \end{matrix} \right] Rw_end​= ​calycalz+salxsalysalzcalxsalz−salycalz+salxcalysalz​calysalz+salxsalycalzcalxcalzsalysalz+salxcalycalz​calxsaly−salxcalxcaly​ ​
R i n i t _ e n d = [ c b c y c b c z + s b c x s b c y s b c z c b c y s b c z + s b c x s b c y c b c z c b c x s b c y c b c x s b c z c b c x c b c z − s b c x − s b c y c b c z + s b c x c b c y s b c z s b c y s b c z + s b c x c b c y c b c z c b c x c b c y ] R_{init\_end}=\left[ \begin{matrix} cbcycbcz+sbcxsbcysbcz& cbcysbcz+sbcxsbcycbcz& cbcxsbcy\\ cbcxsbcz& cbcxcbcz& -sbcx\\ -sbcycbcz+sbcxcbcysbcz& sbcysbcz+sbcxcbcycbcz& cbcxcbcy\\ \end{matrix} \right] Rinit_end​= ​cbcycbcz+sbcxsbcysbczcbcxsbcz−sbcycbcz+sbcxcbcysbcz​cbcysbcz+sbcxsbcycbczcbcxcbczsbcysbcz+sbcxcbcycbcz​cbcxsbcy−sbcxcbcxcbcy​ ​
然后使用对应位置的值相等，就得到了修正后的累计变换acx、acy、acz，计算如下：
$$\begin{gathered} acx=-arcsin(R_{2,3})=-arcsin(-sbcx\ast (salx\ast sblx+calx\ast caly\ast cblx\ast cbly+calx\ast cblx\ast saly\ast sbly)-cbcx\ast cbcz\ast (calx\ast saly\ast (cbly\ast sblz-cblz\ast sblx\ast sbly)-calx\ast caly\ast (sbly\ast sblz+cbly\ast cblz\ast sblx)+cblx\ast cblz\ast salx)-cbcx\ast sbcz\ast (calx\ast caly\ast (cblz\ast sbly-cbly\ast sblx\ast sblz)-calx\ast saly\ast (cbly\ast cblz+sblx\ast sbly\ast sblz)+cblx\ast salx\ast sblz)) \\ acy=arctan(R_{1,3}/R_{3,3}) \\ acz=arctan(R_{2,1}/R_{2,2}) \end{gathered}$$