lego-loam代码分析（3）-激光里程计

上节分析了其点云特征提取，目的是用于帧间匹配获取激光里程计。而像常见的点云匹配如NDT、ICP等匹配算法，均是对整个点云进行匹配。而loam为提高效率和实时性，则采用特征点进行匹配。

匹配初始化

由于激光里程计相临两帧点云进行匹配获取，即当前帧与上一帧进行匹配，故开启slam收到的第一帧点云数据则需要对cloud_last进行赋值，即对上一帧进行记录和存储，不做匹配运算。

  pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;   // 第一次last 和 curr 应一样， 即赋初始状态
  cornerPointsLessSharp = laserCloudCornerLast;
  laserCloudCornerLast = laserCloudTemp;

  laserCloudTemp = surfPointsLessFlat;
  surfPointsLessFlat = laserCloudSurfLast;
  laserCloudSurfLast = laserCloudTemp;

  kdtreeCornerLast.setInputCloud(laserCloudCornerLast);                      // 初始化KD TREE
  kdtreeSurfLast.setInputCloud(laserCloudSurfLast);

  laserCloudCornerLastNum = laserCloudCornerLast->points.size();             // 获取两种特征点云个数
  laserCloudSurfLastNum = laserCloudSurfLast->points.size();

TransformToStart

激光点云进行运动补偿很容易理解，由于雷达扫描一圈是需要时间的，如果激光雷达静止不动，则一圈所有激光光束测量的距离为基于同一个原点测量所得；而若雷达在运动，显然每一次扫描中的光束则基于不同原点进行测量，换算成笛卡尔坐标系时，显然会发现产生了畸变。因此需要进行运动补偿，即根据每个点的时刻计算其对应的原点，然后重新计算在笛卡尔坐标系下的坐标。常用的方法即采用imu等方法估计雷达运动速度，根据每条光速的时刻进行积分，可获取每条光束对应的原点坐标，进行修正。

但是TransformToStart根据他人的解释为运动补偿，即将每帧中点云均转换为以第一个扫描点为同一坐标系，即为运动补偿。但是分析其代码一直不太理解其具体意义（望理解的人分享一下）。

有人如此分析，
LeGo-LOAM源码阅读笔记(三)—featureAssociation.cpp,

// s代表的其实是一个比例，s的计算方法应该如下：  
// s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD 

表示无法理解，因为(pi->intensity - int(pi->intensity))为强度信息中的小数部分，小数部分应该为_scan_period * relTime，可看下面代码，这是预处理坐标系更改void adjustDistortion()中赋值。

    point.intensity =
        int(segmentedCloud->points[i].intensity) + _scan_period * relTime; 

查找多方面资料，都没有找到详细解释，望有高手分享

void FeatureAssociation::TransformToStart(PointType const *const pi,
                                          PointType *const po) {
  // 插值系数，为相对与第一个点的时间差
  // 其中10为系数，可调整，感觉像假设10是个速度？？？？？，但是速度如何来的呢
  float s = 10 * (pi->intensity - int(pi->intensity));       // 由于强度为thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; // 强度根据不同层则不一样，可用于显示区分
                                                             // 而在特征提取进行了修改 intensity = int(segmentedCloud->points[i].intensity) + _scan_period * relTime; 
                                                             // 如此可看出， s应该为每扫描行中点云与第一个点云的时间差即10*_scan_period * relTime
  // 线性插值， 如果静止，显然无需插值，则每个点相对激光原点（或者相对于上刻位置），全部一致
  // 由于匀速运动，因此认为点云中的每个点相对于上刻的位置有一定的偏移，其偏移量为s
  // 如果s不是速度，像是个比例系数，即每个原点都在如此的比例系数下进行平移和旋转？？？？？
  // 不太理解，s针对一圈中固定索引的点云基本相当于一个常数
  // 获取了每个点的原点坐标，然后校准新的笛卡尔坐标
  float ry = s * transformCur[1];
  float rx = s * transformCur[0];                            
  float rz = s * transformCur[2];
  float tx = s * transformCur[3];
  float ty = s * transformCur[4];
  float tz = s * transformCur[5];

  // 将该点根据自己的畸变进行旋转和平移
  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);

  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  po->x = cos(ry) * x2 - sin(ry) * z2;
  po->y = y2;
  po->z = sin(ry) * x2 + cos(ry) * z2;
  po->intensity = pi->intensity;
}

TransformToEnd

与TransformToStart方法一致，应该是将所有点统一到扫描的最后一点时刻的坐标系，其方法是先采用TransformToStart方法转换到第一个点，然后再转换到最后一点。

两次LM匹配

由于已经获取前后两帧的角点和平面点特征点云，因此进行帧间点云匹配，则可获取相邻两帧平移旋转变换，由初始位姿进行累计，从而获得激光里程计信息。
lego-loam采用将两次即两种特征点分别匹配，平面匹配和2维位姿匹配。
其中LM优化的流程为构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解。

平面匹配

匹配点查找

采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点，然后在找到前后两个扫描线中最近的两个点。如下图所示。其中i为当前时刻中一个点云，而m，j，l为上一帧中对应的三个点，此三个点可构成一个平面。

// 代码略，较为简单，容易理解

目标点到匹配平面的距离

匹配采用的方法为点到平面匹配，即需求点到平面的距离，具体公式截图论文。

公式不容易理解，简单解释就是求出三个点构成的平面的方向量并求出平面方程。
具体可参考点、平面和法线关系一文中有具体推导。其中法线计算为平面上两个不平行的向量叉乘。
代码中A、B、C三个点为对应的平面上的点；

1. 求出两个向量AB和AC
2. 计算AB和AC的叉乘，即获取方向量
3. 带回平面方程，计算出平面方程常量D，获取平面方程；
4. 求出方向量的模，获取单位方向量；
5. 计算目标点到达法向量的投影，即为目标点到达平面的距离；

 // 获取三个点，构成一个平面的三角形
      PointType tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; // A点
      PointType tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; // B点
      PointType tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; // C点


      // 平面向量
      // AB (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
      // AC (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)

      // 向量叉乘获得平面法向量AB × AC
      // 法向量三个分量(可套公式获得)
      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);
            
      // 平面方程 Ax+By+Cz+D = 0      
      // 获取平面方程的常数D
      float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);

      // 平面法向量的模
      float ps = sqrt(pa * pa + pb * pb + pc * pc);

      // 换算成单位向量
      pa /= ps;
      pb /= ps;
      pc /= ps;
      pd /= ps;

      // 计算目标点到达平面的距离， 即目标点在方向量上的投影
      float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

      // 考虑权重，距离越小，权重越大，应该是经验值??????
      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));
      }

      // 记录平面参数和点到平面的距离,权重过小则忽略
      if (s > 0.1 && pd2 != 0) {
        PointType coeff;
        coeff.x = s * pa;
        coeff.y = s * pb;
        coeff.z = s * pc;
        coeff.intensity = s * pd2;

        laserCloudOri->push_back(surfPointsFlat->points[i]);        // 存放原始点云
        coeffSel->push_back(coeff);                                 // 存放平面信息和点到平面距离
      }

LM求解

获取矩阵方程A*X=B，即求出矩阵A和B。其中A为一阶偏导数矩阵，而B即为对应点到面的距离矩阵。考虑到矩阵A不一定满秩，无法直接QR分解求解，因此矩阵方程左右两侧均左乘以A的转置。即$A^T\ast A\ast x=A^T\ast B$。然后再进行QR分解求值。

  for (int i = 0; i < pointSelNum; i++) {
    PointType pointOri = laserCloudOri->points[i];
    PointType coeff = coeffSel->points[i];
    // 偏导数
    float arx =
        (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x +
        (a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y +
        (a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z;

    float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x +
                (c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y +
                (c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z;

    float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;

    // 距离
    float d2 = coeff.intensity;
    // A 矩阵
    matA(i, 0) = arx;
    matA(i, 1) = arz;
    matA(i, 2) = aty;
    // B 矩阵
    matB(i, 0) = -0.05 * d2;
  }

  // A的转置
  matAt = matA.transpose();
  // A和B 同左乘A的转置， 目的是希望左侧矩阵满秩
  matAtA = matAt * matA;
  matAtB = matAt * matB;
  // matAtA * X = matAt * matB
  // QR分解求解
  matX = matAtA.colPivHouseholderQr().solve(matAtB);

  // ????不是太明白，矩阵退化判断
  if (iterCount == 0) {
    Eigen::Matrix<float,1,3> matE;
    Eigen::Matrix<float,3,3> matV;
    Eigen::Matrix<float,3,3> matV2;
    
    Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float,3,3> > esolver(matAtA);
    matE = esolver.eigenvalues().real();
    matV = esolver.eigenvectors().real();
    matV2 = matV;

    isDegenerate = false;
    float eignThre[3] = {10, 10, 10};
    for (int i = 2; i >= 0; i--) {
      if (matE(0, i) < eignThre[i]) {
        for (int j = 0; j < 3; j++) {
          matV2(i, j) = 0;
        }
        isDegenerate = true;
      } else {
        break;
      }
    }
    matP = matV.inverse() * matV2;
  }
  //
  if (isDegenerate) {
    Eigen::Matrix<float,3,1> matX2;
    matX2 = matX;
    matX = matP * matX2;
  }

  // 叠加每次迭代的旋转和平移量
  transformCur[0] += matX(0, 0);
  transformCur[2] += matX(1, 0);
  // y 轴平移 （原点云的z的平移）
  transformCur[4] += matX(2, 0);

  // 无效值就恢复0
  for (int i = 0; i < 6; i++) {
    if (std::isnan(transformCur[i])) transformCur[i] = 0;
  }

  // 计算旋转平移误差矩阵
  float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) +
                      pow(RAD2DEG * (matX(1, 0)), 2));
  float deltaT = sqrt(pow(matX(2, 0) * 100, 2));

  // 旋转平移误差小于一定阈值，则停止迭代
  if (deltaR < 0.1 && deltaT < 0.1) {
    return false;
  }

注：1.没有认真进行偏导数的推导；2.矩阵退化没有完全理解；3.作者完全手写推导出来的，效果估计会现成的优化库快

角点匹配

角点的整个匹配与平面匹配方法完全一致，其具体实现和代码注释不再详述。

匹配点查找

采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点，然后在找到前后两个扫描线中最近的一个点。如下图所示。 其中i为当前时刻中一个点云，而j，l为上一帧中对应的两个点，此两点可构成一条直线。

目标点到直线的距离

匹配采用的是目标点到达参考直线的距离，方法如下，已知A和O两个点，求出Q到达OA直线的距离。

具体计算可参考点到直线的距离一文中有具体公式推导。

但是从loam代码中却看出计算公式如下，即求出i到直线j，l的最短距离，公式却是
$$d=\frac{|\overrightarrow{ij}\times \overrightarrow{il|}}{|\overrightarrow{jl}|}$$

按照平面法向量的思想可知，其结果一致。

odometry 发布

经过两次LM匹配，即可认为是经过帧间匹配可获取相临帧的位姿转移矩阵，通过里程计思想进行累加，则可获取当前帧的全局位姿，即获取激光里程计。

// 矩阵转换，根据变换的矩阵， 更新全局位姿
// 先旋转， 在平移
// 已知初始全局位置和相临两帧的位姿转移矩阵，
// 累加获取当前帧全局位置
void FeatureAssociation::integrateTransformation() {
  float rx, ry, rz, tx, ty, tz;
  // 旋转累加
  AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
                     -transformCur[0], -transformCur[1], -transformCur[2], rx,
                     ry, rz);

  float x1 = cos(rz) * (transformCur[3] ) -
             sin(rz) * (transformCur[4] );
  float y1 = sin(rz) * (transformCur[3] ) +
             cos(rz) * (transformCur[4] );
  float z1 = transformCur[5];

  float x2 = x1;
  float y2 = cos(rx) * y1 - sin(rx) * z1;
  float z2 = sin(rx) * y1 + cos(rx) * z1;

  // 平移累加
  tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);   
  ty = transformSum[4] - y2;
  tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

  // 获取当前帧全局位置，即构建里程计
  transformSum[0] = rx;
  transformSum[1] = ry;     // 
  transformSum[2] = rz;     // 绕z轴方向的yaw 航向角
  transformSum[3] = tx;     // x 坐标
  transformSum[4] = ty;     // y 坐标
  transformSum[5] = tz;     // z 坐标
}

发布建图数据

整个特征点提取其目的包括两个：用于获取高频率的激光里程计和用于低频率的建图匹配。而获取的激光里程计频率较高，可为后期建图匹配提供预测值。可以说前3节所有处理均可认为是slam中传感器的预处理，将预处理后的数据输入slam主流程。
其预处理后的数据包括：

1. 平面点云；
2. 角点点云；
3. 非特征点云；
4. 激光里程计；

其中1,2,3三种点云的集合则为原始点云。而激光里程计也可以有其他里程计进行替换，如编码器，imu等。

其整个特征提取以及里程计处理主流程代码如下：

void FeatureAssociation::runFeatureAssociation() {
  while (ros::ok()) {
    ProjectionOut projection;
    _input_channel.receive(projection);              // 接收三类数据

    if( !ros::ok() ) break;

    //--------------                        
    outlierCloud = projection.outlier_cloud;         //分别为未被分类的孤立点云簇
    segmentedCloud = projection.segmented_cloud;     //被分类的包含地面的点云
    segInfo = std::move(projection.seg_msg);         //分类的相关信息
    _scan_msg = std::move(projection.scan_msg);

    cloudHeader = segInfo.header;
    timeScanCur = cloudHeader.stamp.toSec();

    /**  1. Feature Extraction  */
    adjustDistortion();                              // 坐标系转换

    calculateSmoothness();                           // 计算平滑性

    markOccludedPoints();                            // 标记在水平扫描方向上，屏蔽不可靠点

    extractFeatures();                               // 特征提取包括，角点和平坦点

    publishCloud();  // cloud for visualization

    // Feature Association
    if (!systemInitedLM) {                           // 仅执行一次，用于初始化
      checkSystemInitialization();
      continue;
    }

    updateTransformation();                          // 计算两帧激光数据之间转换矩阵

    integrateTransformation();                       // 迭代更新，将每两帧之间变换，进行累计变换，构建里程计

    publishOdometry();

    publishCloudsLast();                             // cloud to mapOptimization

    // 以上为高频率的激光里程计获取
    //--------------
    _cycle_count++;

    // 建图_mapping_frequency_div倍降频更新
    if (_cycle_count == _mapping_frequency_div) {
      _cycle_count = 0;
      AssociationOut out;
      out.cloud_corner_last.reset(new pcl::PointCloud<PointType>());
      out.cloud_surf_last.reset(new pcl::PointCloud<PointType>());
      out.cloud_outlier_last.reset(new pcl::PointCloud<PointType>());

      *out.cloud_corner_last = *laserCloudCornerLast;    // 将平面信息 和角点发出给建图算法
      *out.cloud_surf_last = *laserCloudSurfLast;
      *out.cloud_outlier_last = *outlierCloud;           // 将非特征点发给建图算法

      out.laser_odometry = laserOdometry;                // 激光里程计
    }
  }