Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件中的特征匹配部分。
该专栏的其他章节链接如下
https://blog.csdn.net/HUASHUDEYANJING/article/details/130332367

一、Feature Extraction

经过上一节的点云特征提取，我们已经获得了稳健的角点和面点如下图所示，下面将介绍如何配准得到位姿Feature Association部分的源码解析

 1. Feature Extraction特征提取
// 1.1 点云运动补偿
        adjustDistortion();
// 1.2 计算平滑度
        calculateSmoothness();
// 1.3 标记遮挡点
        markOccludedPoints();
// 1.4 提取特征
        extractFeatures();
// 1.5 发布点云
        publishCloud();
2. Feature Association特征匹配
    if (!systemInitedLM) {
            // 2.1 检查系统初始化，点云投影到结束点
                checkSystemInitialization();
                return;}
// 2.2 更新初始猜测位置
        updateInitialGuess();
// 2.3 特征匹配，输出Transformation
        updateTransformation();
// 2.4 融合IMU坐标Transformation
        integrateTransformation();

二、函数解析

2.1 publishCloudsLast

  • 作用：将当前帧点云投影到结束点并发布，作为下一帧匹配的对象，（匹配之前需要有一个匹配对象点云）
  • 输入： 
     cornerPointsSharp角特征  
 cornerPointsLessSharp缓角特征  
 surfPointsFlat面特征  
 surfPointsLessFlat缓面特征 
 adjustCloud修正点云
  • 输出： 
     /laser_cloud_corner_last
 /laser_cloud_surf_last
 /outlier_cloud_last
 /laser_odom_to_init
  • 代码： 
    void publishCloudsLast(){
//把特征点云投影到每帧的结尾时刻
updateImuRollPitchYawStartSinCos();
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++) {
  TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); }
  int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++) {
  TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}
//用KD树存储当前帧点云，为后续匹配做准备
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
if (frameCount >= skipFrameNum + 1) {
          frameCount = 0;
//发布上一帧outlier
pubOutlierCloudLast.publish(outlierCloudLast2);
//发布上一帧线特征
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
//发布上一帧面特征
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}
    下图中红色为前一帧，绿色为当前帧

    2.2 updateInitialGuess

  • 作用：把当前IMU数值作为位姿先验
  • 输入：IMU数值
  • 输出：transformCur位姿矩阵 
    // 如果IMU有旋转，则更新变换矩阵以反映新的旋
void updateInitialGuess(){
if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY;  // 根据Y轴旋转更新变换矩阵的第一个元素
transformCur[1] = - imuAngularFromStartZ;  // 根据Z轴旋转更新变换矩阵的第二个元素
transformCur[2] = - imuAngularFromStartX;  // 根据X轴旋转更新变换矩阵的第三个元素}
// 如果IMU有移动，则更新变换矩阵以反映新的平移
if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0)
{transformCur[3] -= imuVeloFromStartX * scanPeriod;  // 根据X轴移动更新变换矩阵的第四个元素
transformCur[4] -= imuVeloFromStartY * scanPeriod;  // 根据Y轴移动更新变换矩阵的第五个元素
transformCur[5] -= imuVeloFromStartZ * scanPeriod;  // 根据Z轴移动更新变换矩阵的第六个元素 }   }

    2.3 updateTransformation

  • 作用：匹配角和面特征点，计算位姿
  • 输入： 
     laserCloudSurfLast
 surfPointsFlat
 laserCloudCornerLast
 cornerPointsSharp
  • 输出：transformCur位姿矩阵

  • 代码

    void updateTransformation(){
     //检查特征点
     if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
         return;
//  面特征匹配
     for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
         laserCloudOri->clear(); 
         coeffSel->clear(); 
         findCorrespondingSurfFeatures(iterCount1); 
      if (laserCloudOri->points.size() < 10)
             continue;
         // 通过面特征的匹配，计算变换矩阵
         if (calculateTransformationSurf(iterCount1) == false)
             break;
     }
// 线特征匹配
     for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

         laserCloudOri->clear();
         coeffSel->clear();

         findCorrespondingCornerFeatures(iterCount2); 
         if (laserCloudOri->points.size() < 10)
             continue;
         if (calculateTransformationCorner(iterCount2) == false) 
             break;
     }
 }

    2.4 findCorrespondingFeatures

    计算残差之前，需要先找到对应点，对于角点找点到最近的直线，对于面点找点到最近的平面，，如下图所示

    2.4.1 findCorrespondingCornerFeatures

  • 作用：找到当前角点的对应点
  • 输入：laserCloudCornerLast 、cornerPointsSharp
  • 输出：coeffSel所有角点的特征匹配集
  • 代码
    void findCorrespondingCornerFeatures(int iterCount){
for (int i = 0; i < cornerPointsSharpNum; i++) {
   TransformToStart(&cornerPointsSharp->points[i], &pointSel); //转换到起始点
   tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
   tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];
              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;
       if (s > 0.1 && ld2 != 0) {
                  coeff.x = s * la; 
                  coeff.y = s * lb;
                  coeff.z = s * lc;
                  coeff.intensity = s * ld2; //线外一点到直线的距离
                  laserCloudOri->push_back(cornerPointsSharp->points[i]);
                  coeffSel->push_back(coeff);}

    2.4.2 findCorrespondingSurfFeatures

  • 作用：找到当前面点的对应面
  • 输入：laserCloudCornerLast、surfPointsFlat
  • 输出：coeffSel所有面点的特征匹配集
  • 代码：
    void findCorrespondingSurfFeatures(int iterCount){
TransformToStart(&surfPointsFlat->points[i], &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {
// 分别取这3个点(在前一帧的平面点取)
  tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];  //a
  tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];  //b
  tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];  //c
if (s > 0.1 && pd2 != 0) {
  // [x,y,z]是整个平面的单位法量
  // intensity是平面外一点到该平面的距离
  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);

    2.5 calculateTransformation

  • 作用：优化参数目标
  • 输入： coeffSel
  • 输出：transformCur位姿参数
  • 步骤
    损失函数

    待优化变量

    平移旋转矩阵

    通过平移旋转矩阵将点云投影，构建残差方程

    使用列文虎克迭代求解变量

    旋转平移矩阵变化足够小，停止迭代 
    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) {
  return false;

    2.6 integrateTransformation

  • 作用：将IMU信息融入到位姿更新当中
  • 输入：transformCur transformSum IMU当前数据
  • 输出：transformCur transformSum
  • 代码 
    void integrateTransformation(){
  float rx, ry, rz, tx, ty, tz;
  //transformSum 是 IMU累计的变化量 ，0，1，2 分别代表的是 pitch yaw roll 的内容，
  //transformCur 是当前的分量，函数的作用是局部坐标转换成全局坐标
  AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);
  float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) - sin(rz) * (transformCur[4] - imuShiftFromStartY);
  float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) + cos(rz) * (transformCur[4] - imuShiftFromStartY);
  float z1 = transformCur[5] - imuShiftFromStartZ;
  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);
  //加入 imu 当前数据 更新位姿
  PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
  imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
  transformSum[0] = rx;
  transformSum[1] = ry;
  transformSum[2] = rz;
  transformSum[3] = tx;
  transformSum[4] = ty;
  transformSum[5] = tz;
}

    2.7 publishOdometry

    作用：发布位姿信息
    输入： transformSum
    输出： /laser_odom_to_init
    代码 
    void publishOdometry(){
  //从转角创建四元数
  geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]); 
  // rx,ry,rz转化为四元数发布
  laserOdometry.header.stamp = cloudHeader.stamp;
  laserOdometry.pose.pose.orientation.x = -geoQuat.y;
  laserOdometry.pose.pose.orientation.y = -geoQuat.z;
  laserOdometry.pose.pose.orientation.z = geoQuat.x;
  laserOdometry.pose.pose.orientation.w = geoQuat.w;
  laserOdometry.pose.pose.position.x = transformSum[3];
  laserOdometry.pose.pose.position.y = transformSum[4];
  laserOdometry.pose.pose.position.z = transformSum[5];
  pubLaserOdometry.publish(laserOdometry);  //scan与scan的位姿变换
  // laserOdometryTrans 是用于tf广播
  laserOdometryTrans.stamp_ = cloudHeader.stamp;
  laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
  laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
  tfBroadcaster.sendTransform(laserOdometryTrans); //发布TF
  }
}
    最终发布位姿，格式如下图所示

    下一节将介绍MapOptmization的源码解析，如果喜欢本专栏请关注吧