上一篇

featureAssociation.cpp

其次featureAssociation这一个node节点,主要是特征提取。代码中先初始化了lego_loam::FeatureAssociation,用来订阅了上一节点发出来的分割出来的点云,点云的属性,外点以及IMU消息,并设置了回调函数。其中IMU消息的订阅函数较为复杂,它从IMU数据中提取出姿态,角速度和线加速度,其中姿态用来消除重力对线加速度的影响。然后函数FeatureAssociation::AccumulateIMUShiftAndRotation用来做积分,包括根据姿态,将加速度往世界坐标系下进行投影。再根据匀加速度运动模型积分得到速度和位移,同时,对角速度也进行了积分。

void FeatureAssociation::imuHandler(const sensor_msgs::Imu::ConstPtr &imuIn) {
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

        // 对加速度进行坐标变换
        // 进行加速度坐标交换时将重力加速度去除,然后再进行xxx到zzz,yyy到xxx,zzz到yyy的变换。
        // 去除重力加速度的影响时,需要把重力加速度分解到三个坐标轴上,然后分别去除他们分量的影响,在去除的过程中需要注意加减号(默认右手坐标系的旋转方向来看)。
        // 在上面示意图中,可以简单理解为红色箭头实线分解到红色箭头虚线上(根据pitchpitchpitch进行分解),然后再按找rollrollroll角进行分解。
        // 原文链接:https://blog.csdn.net/wykxwyc/article/details/98317544
        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

        imuPointerLast = (imuPointerLast + 1) % imuQueLength;

        // 将欧拉角,加速度,速度保存到循环队列中
        imuTime[imuPointerLast] = imuIn->header.stamp.toSec();

        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
        imuYaw[imuPointerLast] = yaw;

        imuAccX[imuPointerLast] = accX;
        imuAccY[imuPointerLast] = accY;
        imuAccZ[imuPointerLast] = accZ;

        imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
        imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
        imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;

        // 积分计算得到位移
        AccumulateIMUShiftAndRotation();
    }

上面的回调函数仅仅是考虑该如何读取数据,而未涉及点云该如何处理,也没有说怎么与IMU数据做融合。由于代码太多而且很多都是纯粹的数学推导计算,为此我们选取runFeatureAssociation中的一些重要的思想来详写。

//  主程序入口
    void FeatureAssociation::runFeatureAssociation() {
        // 有新数据进来才执行
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        } else {
            return;
        }
        /**
            1. Feature Extraction
        */
        adjustDistortion();  //  imu去畸变

        calculateSmoothness(); //  计算光滑性

        markOccludedPoints();  // 距离较大或者距离变动较大的点标记

        extractFeatures();  //  特征提取(未看懂)

        publishCloud(); // cloud for visualization

        /**
        2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();  //  更新初始位姿

        //  一个是找特征平面,通过面之间的对应关系计算出变换矩阵。
        // 另一个部分是通过角、边特征的匹配,计算变换矩阵。
        updateTransformation();

        integrateTransformation();  //  计算旋转角的累积变化量。

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization
    }
  1. adjustDistortion:畸变校正。前面IMU消息的回调函数中对数据进行了转存和积分,这里是根据激光雷达一帧扫描期间,IMU积分得到的位姿变换,通过插值的形式,对点云畸变进行校正。由于激光雷达与IMU坐标系定义不同,因此函数中先对坐标进行了交换,仅仅是为了将点云在IMU坐标系下表示。
    在这里插入图片描述
下面引用下网络上对该畸变校正步骤的举例:对于某个激光`pi`,采集到该点时雷达本身的位姿`Ti`已经与采集第一个点`p0`时的位姿`T0`不同了,如果我们还认为这个点也是在`T0`处获取到的,这是不准确的,特别是当雷达扫描频率比较低,且雷达运动比较剧烈的情况下,所以,**需要根据`T0`和`Ti`之间的相对变换,对点`pi`进行补偿**,补偿的结果,就是这个`Ti`位姿处观测到的`pi`,如果是在`T0`处被观测到的,它的坐标应该是怎样的,这样就消除了雷达本身运动周期的影响,经过畸变消除,可以认为,所有点都是一瞬间在`T0`处采集得到的,而不存在内畸变的问题。然后,**问题就变成了如何确定`Ti`与`T0`之间的相对位姿**,我们知道IMU一直在计算积分,频率远远比雷达扫描要高,但是两者的时间戳是离散的并不是一一对应的,因此,采集到`pi`点的时刻`ti`时雷达的位姿并**不能直接给出来**,但是,IMU可以给出前后两个距离`ti`**非常接近的时刻积分出来的位姿**,这个由于IMU频率非常高所以两者之间时间非常短,可以**通过线性插值的形式确定`ti`时刻的位姿`Ti`**。
```cpp
    void FeatureAssociation::adjustDistortion() {
        bool halfPassed = false;
        int cloudSize = segmentedCloud->points.size();

        PointType point;
        for (int i = 0; i < cloudSize; i++) {
            // 这个坐标变换是什么意思?
            point.x = segmentedCloud->points[i].y;
            point.y = segmentedCloud->points[i].z;
            point.z = segmentedCloud->points[i].x;
            // 计算点的yaw, 根据不同的偏航角,可以知道激光雷达扫过的位置有没有超过一半
            float ori = -atan2(point.x, point.z);
            if (!halfPassed) {
                if (ori < segInfo.startOrientation - M_PI / 2)
                    ori += 2 * M_PI;
                else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
                    ori -= 2 * M_PI;

                if (ori - segInfo.startOrientation > M_PI)
                    halfPassed = true;
            } else {
                ori += 2 * M_PI;

                if (ori < segInfo.endOrientation - M_PI * 3 / 2)
                    ori += 2 * M_PI;
                else if (ori > segInfo.endOrientation + M_PI / 2)
                    ori -= 2 * M_PI;
            }

            float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
            point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;

            // imu与lidar时间轴对齐
            if (imuPointerLast >= 0) {
                float pointTime = relTime * scanPeriod;
                imuPointerFront = imuPointerLastIteration;

                //  imu数据比激光数据早,但是没有更后面的数据,不能通过插补进行优化
                while (imuPointerFront != imuPointerLast) {
                    if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
                        break;
                    }
                    imuPointerFront = (imuPointerFront + 1) % imuQueLength;
                }

                // imu时间在前
                if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
                    imuRollCur = imuRoll[imuPointerFront];
                    imuPitchCur = imuPitch[imuPointerFront];
                    imuYawCur = imuYaw[imuPointerFront];

                    imuVeloXCur = imuVeloX[imuPointerFront];
                    imuVeloYCur = imuVeloY[imuPointerFront];
                    imuVeloZCur = imuVeloZ[imuPointerFront];

                    imuShiftXCur = imuShiftX[imuPointerFront];
                    imuShiftYCur = imuShiftY[imuPointerFront];
                    imuShiftZCur = imuShiftZ[imuPointerFront];
                } else {
                    // 在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[imuPointerFront] - imuTime[imuPointerBack]);
                    float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                      / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);

                    // roll和pitch通常接近0,yaw变化角度较大
                    imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
                    imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
                    if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
                        imuYawCur =
                                imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
                    } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
                        imuYawCur =
                                imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
                    } else {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
                    }

                    // 速度与位置进行插值
                    imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
                    imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
                    imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

                    imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
                    imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
                    imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
                }

                // i = 0 时刻
                if (i == 0) {
                    imuRollStart = imuRollCur;
                    imuPitchStart = imuPitchCur;
                    imuYawStart = imuYawCur;

                    imuVeloXStart = imuVeloXCur;
                    imuVeloYStart = imuVeloYCur;
                    imuVeloZStart = imuVeloZCur;

                    imuShiftXStart = imuShiftXCur;
                    imuShiftYStart = imuShiftYCur;
                    imuShiftZStart = imuShiftZCur;

                    if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                    } else {
                        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                        float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                           / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                          / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront +
                                                 imuAngularRotationX[imuPointerBack] * ratioBack;
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront +
                                                 imuAngularRotationY[imuPointerBack] * ratioBack;
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront +
                                                 imuAngularRotationZ[imuPointerBack] * ratioBack;
                    }

                    imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
                    imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
                    imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

                    imuAngularRotationXLast = imuAngularRotationXCur;
                    imuAngularRotationYLast = imuAngularRotationYCur;
                    imuAngularRotationZLast = imuAngularRotationZCur;

                    updateImuRollPitchYawStartSinCos();
                } else {
                    VeloToStartIMU();
                    TransformToStartIMU(&point);
                }
            }
            segmentedCloud->points[i] = point;
        }

        imuPointerLastIteration = imuPointerLast;
    }
```
  1. calculateSmoothness:根据两侧的10个点,计算曲率,这里的曲率只是一个量的大小的概念。因为曲率并不参加最终的优化,只是衡量一个点光滑与否的标志,是个相对的概念。所以只是将周围十个点累加,并未开方求欧氏距离。最后在cloudSmoothness这个vector中保存了曲率与点索引对,以便后面根据曲率对点进行排序,并根据索引对直接得到点的索引顺序

         void FeatureAssociation::calculateSmoothness() {
             // 计算光滑性
             int cloudSize = segmentedCloud->points.size();
             for (int i = 5; i < cloudSize - 5; i++) {
    
                 float diffRange = segInfo.segmentedCloudRange[i - 5] + segInfo.segmentedCloudRange[i - 4]
                                   + segInfo.segmentedCloudRange[i - 3] + segInfo.segmentedCloudRange[i - 2]
                                   + segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i] * 10
                                   + segInfo.segmentedCloudRange[i + 1] + segInfo.segmentedCloudRange[i + 2]
                                   + segInfo.segmentedCloudRange[i + 3] + segInfo.segmentedCloudRange[i + 4]
                                   + segInfo.segmentedCloudRange[i + 5];
    
                 cloudCurvature[i] = diffRange * diffRange;
    
                 // 在markOccludedPoints()函数中对该参数进行重新修改
                 cloudNeighborPicked[i] = 0;
                 cloudLabel[i] = 0;  //  在extractFeatures()函数中会对标签进行修改
    
                 cloudSmoothness[i].value = cloudCurvature[i];
                 cloudSmoothness[i].ind = i;
             }
         }
    
  2. markOccludedPoints:对于深度变化比较明显的相邻两个点,远处的那一个更容易被遮挡,不稳定如果某个点,和两侧的点距离差距都比较大,那么有可能是噪声等,也不稳定。在cloudNeighborPicked中标注为1,后面的特征提取中不会再考虑。

         void FeatureAssociation::markOccludedPoints() {
             int cloudSize = segmentedCloud->points.size();
    
             for (int i = 5; i < cloudSize - 6; ++i) {
    
                 float depth1 = segInfo.segmentedCloudRange[i];
                 float depth2 = segInfo.segmentedCloudRange[i + 1];
                 int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] - segInfo.segmentedCloudColInd[i]));
    
                 if (columnDiff < 10) {
                     // 选择距离较远的那些点,并将他们标记为1
                     if (depth1 - depth2 > 0.3) {
                         cloudNeighborPicked[i - 5] = 1;
                         cloudNeighborPicked[i - 4] = 1;
                         cloudNeighborPicked[i - 3] = 1;
                         cloudNeighborPicked[i - 2] = 1;
                         cloudNeighborPicked[i - 1] = 1;
                         cloudNeighborPicked[i] = 1;
                     } else if (depth2 - depth1 > 0.3) {
                         cloudNeighborPicked[i + 1] = 1;
                         cloudNeighborPicked[i + 2] = 1;
                         cloudNeighborPicked[i + 3] = 1;
                         cloudNeighborPicked[i + 4] = 1;
                         cloudNeighborPicked[i + 5] = 1;
                         cloudNeighborPicked[i + 6] = 1;
                     }
                 }
    
                 float diff1 = std::abs(float(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i]));
                 float diff2 = std::abs(float(segInfo.segmentedCloudRange[i + 1] - segInfo.segmentedCloudRange[i]));
    
                 // 选择距离变化较大的点,并将他们标记为1
                 if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
                     cloudNeighborPicked[i] = 1;
             }
         }
    
  3. extractFeatures:则是根据曲率的大小,提取四种特征点。这里的流程和LOAM基本一样,将点分为多种类型,把每个线分成了6个区,在每个区内提取一定数量的角点和面点,这是为了让点分布更均匀,匹配时效果会更稳定更好。然后就是在一个区的范围内,按照曲率对索引进行排序,曲率最大的20个非地面点为角点,其中最最大的2个强角点,两者是包含的关系;曲率最最小的4个地面点为强面点,其余所有地面点都是一般面点,点数太多需要进行降采样,一般是五个点取一次。在确定提取某个点后,其周围一定范围内的点cloudNeighborPicked会被标注为1,认为该区域的点已经被选择,不会再次被选中,这是为了防止提取出来的特征点过度聚集在某一处。
    在这里插入图片描述

         void FeatureAssociation::extractFeatures() {
             cornerPointsSharp->clear();
             cornerPointsLessSharp->clear();
             surfPointsFlat->clear();
             surfPointsLessFlat->clear();
    
             //  提取4种特征点
             for (int i = 0; i < N_SCAN; i++) {
                 surfPointsLessFlatScan->clear();
                 for (int j = 0; j < 6; j++) {
    
                     int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;
                     int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
    
                     if (sp >= ep)
                         continue;
    
                     std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, by_value());
    
                     int largestPickedNum = 0;
                     for (int k = ep; k >= sp; k--) {
                         int ind = cloudSmoothness[k].ind;
                         if (cloudNeighborPicked[ind] == 0 &&
                             cloudCurvature[ind] > edgeThreshold &&
                             segInfo.segmentedCloudGroundFlag[ind] == false) {
    
                             largestPickedNum++;
                             if (largestPickedNum <= 2) {
                                 cloudLabel[ind] = 2;
                                 cornerPointsSharp->push_back(segmentedCloud->points[ind]);
                                 cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                             } else if (largestPickedNum <= 20) {
                                 cloudLabel[ind] = 1;
                                 cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                             } else {
                                 break;
                             }
    
                             cloudNeighborPicked[ind] = 1;
                             for (int l = 1; l <= 5; l++) {
                                 int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
                                                               segInfo.segmentedCloudColInd[ind + l - 1]));
                                 if (columnDiff > 10)
                                     break;
                                 cloudNeighborPicked[ind + l] = 1;
                             }
                             for (int l = -1; l >= -5; l--) {
                                 int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
                                                               segInfo.segmentedCloudColInd[ind + l + 1]));
                                 if (columnDiff > 10)
                                     break;
                                 cloudNeighborPicked[ind + l] = 1;
                             }
                         }
                     }
    
                     // 平面点只从地面点中进行选择
                     int smallestPickedNum = 0;
                     for (int k = sp; k <= ep; k++) {
                         int ind = cloudSmoothness[k].ind;
                         if (cloudNeighborPicked[ind] == 0 &&
                             cloudCurvature[ind] < surfThreshold &&
                             segInfo.segmentedCloudGroundFlag[ind] == true) {
    
                             cloudLabel[ind] = -1;
                             surfPointsFlat->push_back(segmentedCloud->points[ind]);
    
                             smallestPickedNum++;
                             if (smallestPickedNum >= 4) {
                                 break;
                             }
    
                             cloudNeighborPicked[ind] = 1;
                             for (int l = 1; l <= 5; l++) {
    
                                 int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
                                                               segInfo.segmentedCloudColInd[ind + l - 1]));
                                 if (columnDiff > 10)
                                     break;
    
                                 cloudNeighborPicked[ind + l] = 1;
                             }
                             for (int l = -1; l >= -5; l--) {
    
                                 int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
                                                               segInfo.segmentedCloudColInd[ind + l + 1]));
                                 if (columnDiff > 10)
                                     break;
    
                                 cloudNeighborPicked[ind + l] = 1;
                             }
                         }
                     }
    
                     for (int k = sp; k <= ep; k++) {
                         if (cloudLabel[k] <= 0) {
                             surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
                         }
                     }
                 }
    
                 // surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大
                 surfPointsLessFlatScanDS->clear();
                 downSizeFilter.setInputCloud(surfPointsLessFlatScan);
                 downSizeFilter.filter(*surfPointsLessFlatScanDS);
    
                 *surfPointsLessFlat += *surfPointsLessFlatScanDS;
             }
         }
    
  4. publishCloud:发布4种特征点,分别是cornerPointsSharpcornerPointsLessSharpsurfPointsFlatsurfPointsLessFlat。用来记录在一帧激光雷达数据下提取的特征点。

     void FeatureAssociation::publishCloud() {
             sensor_msgs::PointCloud2 laserCloudOutMsg;
    
             if (pubCornerPointsSharp.getNumSubscribers() != 0) {
                 pcl::toROSMsg(*cornerPointsSharp, laserCloudOutMsg);
                 laserCloudOutMsg.header.stamp = cloudHeader.stamp;
                 laserCloudOutMsg.header.frame_id = "/camera";
                 pubCornerPointsSharp.publish(laserCloudOutMsg);
             }
    
             if (pubCornerPointsLessSharp.getNumSubscribers() != 0) {
                 pcl::toROSMsg(*cornerPointsLessSharp, laserCloudOutMsg);
                 laserCloudOutMsg.header.stamp = cloudHeader.stamp;
                 laserCloudOutMsg.header.frame_id = "/camera";
                 pubCornerPointsLessSharp.publish(laserCloudOutMsg);
             }
    
             if (pubSurfPointsFlat.getNumSubscribers() != 0) {
                 pcl::toROSMsg(*surfPointsFlat, laserCloudOutMsg);
                 laserCloudOutMsg.header.stamp = cloudHeader.stamp;
                 laserCloudOutMsg.header.frame_id = "/camera";
                 pubSurfPointsFlat.publish(laserCloudOutMsg);
             }
    
             if (pubSurfPointsLessFlat.getNumSubscribers() != 0) {
                 pcl::toROSMsg(*surfPointsLessFlat, laserCloudOutMsg);
                 laserCloudOutMsg.header.stamp = cloudHeader.stamp;
                 laserCloudOutMsg.header.frame_id = "/camera";
                 pubSurfPointsLessFlat.publish(laserCloudOutMsg);
             }
         }
    

    此时根据点云与点云之间曲率来提取符合的特征点信息,下面则是根据特征点的匹配来计算出面与面的变换矩阵。

  5. checkSystemInitialization:该函数主要执行的是激光里程计初始化。激光里程计是连续帧之间匹配的过程,需要有一个起始参考帧,如果系统没有经过初始化,那么就把当前帧作为参考帧,并初始化当前总的位姿transformSum(除了俯仰角和滚转角外,其余参数全部初始化为0)。

     void FeatureAssociation::checkSystemInitialization() {
    
             // 交换两团点云,并发布出去
             pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
             cornerPointsLessSharp = laserCloudCornerLast;
             laserCloudCornerLast = laserCloudTemp;
    
             laserCloudTemp = surfPointsLessFlat;
             surfPointsLessFlat = laserCloudSurfLast;
             laserCloudSurfLast = laserCloudTemp;
    
             kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
             kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
    
             laserCloudCornerLastNum = laserCloudCornerLast->points.size();
             laserCloudSurfLastNum = laserCloudSurfLast->points.size();
    
             sensor_msgs::PointCloud2 laserCloudCornerLast2;
             pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
             laserCloudCornerLast2.header.stamp = cloudHeader.stamp;
             laserCloudCornerLast2.header.frame_id = "/camera";
             pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
    
             sensor_msgs::PointCloud2 laserCloudSurfLast2;
             pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
             laserCloudSurfLast2.header.stamp = cloudHeader.stamp;
             laserCloudSurfLast2.header.frame_id = "/camera";
             pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
    
             transformSum[0] += imuPitchStart;
             transformSum[2] += imuRollStart;
    
             systemInitedLM = true;
         }
    
  6. updateInitialGuess:该函数根据IMU积分的结果,计算出一个初始位姿transformCur,这个位姿指的是雷达旋转一圈后发生的相对位姿变换

         void FeatureAssociation::updateInitialGuess() {
    
             imuPitchLast = imuPitchCur;
             imuYawLast = imuYawCur;
             imuRollLast = imuRollCur;
    
             imuShiftFromStartX = imuShiftFromStartXCur;
             imuShiftFromStartY = imuShiftFromStartYCur;
             imuShiftFromStartZ = imuShiftFromStartZCur;
    
             imuVeloFromStartX = imuVeloFromStartXCur;
             imuVeloFromStartY = imuVeloFromStartYCur;
             imuVeloFromStartZ = imuVeloFromStartZCur;
    
             // imu是在start坐标系下
             if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0) {
                 transformCur[0] = -imuAngularFromStartY;
                 transformCur[1] = -imuAngularFromStartZ;
                 transformCur[2] = -imuAngularFromStartX;
             }
    
             // 速度×时间
             if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) {
                 transformCur[3] -= imuVeloFromStartX * scanPeriod;
                 transformCur[4] -= imuVeloFromStartY * scanPeriod;
                 transformCur[5] -= imuVeloFromStartZ * scanPeriod;
             }
         }
    
  7. updateTransformation:通过约束优化相对位姿。对于当前帧的每一个面点,在上一帧找到最近邻以及另外两个不共线的点组成面,该点到面的距离就是要减小的残差,通过面之间的对应关系计算出变换矩阵。之后通过角、边特征的匹配,计算变换矩阵。即找到最近邻和另外一个点构建约束,被优化的对象就是transformCur,即找到一个相对位姿变换,使得总体的残差最小。然后依次使用平面点和角点,对相对位姿进行优化。优化的对象transformCur特点在于,先使用平面点经历多次优化后,再使用角点(可能因为平面点更多,因而会更稳定。
    在这里插入图片描述

         void FeatureAssociation::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;
             }
         }
    
  8. integrateTransformation:把优化得到的transformCur,累加到transformSum中。

         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] - 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);
    
             // 与accumulateRotatio联合起来更新transformSum的rotation部分的工作
             // 可视为transformToEnd的下部分的逆过程
             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;
         }
    
  1. publishOdometry:发布里程计位姿位姿和tf变换,这里的变换实际上是odo相对于地图原点的,存在持续的误差积累的;
  2. publishCloudsLast:根据优化的结果transformCur,将当前帧特征点云统一到终止时刻,用来作为下次匹配的参考(因为上一帧的点云已经被统一到当前帧的起始时刻,匹配是当前帧的终止位姿相对于起始位姿的转化),同时发布特征点云。

mapOptmization.cpp

最后我们对mapOptmization这一个node进行解析,前两部分已经完成了一个激光雷达里程计该做的处理(点云预处理,连续帧匹配计算出激光里程计信息)。但是这个过程中误差是逐渐累积的,为此我们需要通过回环检测来减小误差,在地图优化部分,相比于LeGO-LOAM而言,文中使用ScanContext来实现scan-to-map的匹配,并检测到回环以及进行优化。
与前面两个node类似,mapOptmization的主要函数均封装在run函数中,同时作者在网上找到了一个较为清晰的流程图,因此下面会挑选重点函数进行详细阐述。

void mapOptimization::run() {
        //  有新数据进来,才执行后续
        if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
            newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
            newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
            newLaserOdometry) {

            newLaserCloudCornerLast = false;
            newLaserCloudSurfLast = false;
            newLaserCloudOutlierLast = false;
            newLaserOdometry = false;

            std::lock_guard<std::mutex> lock(mtx);
            if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
                timeLastProcessing = timeLaserOdometry;

                transformAssociateToMap();  //  坐标系->map

                extractSurroundingKeyFrames();  

                downsampleCurrentScan();

                scan2MapOptimization();

                saveKeyFramesAndFactor();

                correctPoses();

                publishTF();

                publishKeyPosesAndFrames();

                clearCloud();
            }
        }
    }

在这里插入图片描述
在这里插入图片描述
前面的transformAssociateToMapextractSurroundingKeyFrames以及downsampleCurrentScan函数主要是实现了坐标系的转换时间顺序,选择最近的一定数量的关键帧点云,组成局部地图laserCloudCornerFromMap以及当前帧原始点云,角点,面点,离群点进行降采样。主要是对之前的数据进行降采样以进行快速匹配。

  1. scan2MapOptimization和saveKeyFramesAndFactor:这两个函数主要是用来处理因子图优化的功能。scan2MapOptimization内部主要是使用scan-to-model位姿优化,获得当前时间点机器人的位姿transformTobeMapped,使得总体残差最小,连续循环优化多次。此外该部分的优化会参考IMU消息回调所确定的roll和pitch对该位姿进行修正,对 transformTobeMapped 进行中值滤波,获得最终的机器人位姿,到这里,虽然在scan-to-scan之后,又进行了scan-to-map的匹配,但是并未出现回环检测和优化,所以依然是一个误差不断积累的里程计的概念

         void mapOptimization::scan2MapOptimization() {
    
             // laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数
             // laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数
             if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
    
                 // laserCloudCornerFromMapDS和laserCloudSurfFromMapDS的来源有2个:
                 // 当有闭环时,来源是:recentCornerCloudKeyFrames,没有闭环时,来源surroundingCornerCloudKeyFrames
                 kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
                 kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
    
                 for (int iterCount = 0; iterCount < 10; iterCount++) {
                     laserCloudOri->clear();
                     coeffSel->clear();
    
                     cornerOptimization(iterCount);
                     surfOptimization(iterCount);
    
                     if (LMOptimization(iterCount) == true)
                         break;
                 }
                 // 迭代结束更新相关的转移矩阵
                 transformUpdate();
             }
         }
    

    saveKeyFramesAndFactor主要作用是选定关键帧。如果距离上一次保存的关键帧欧式距离最够大,需要保存当前关键帧,则需要我们计算与上一关键帧之间的约束,这种约束可以理解为局部的小回环,加入后端进行优化。将优化的结果保存作为关键帧位姿(保存当前关键帧的3维和6维位姿)和点云,同步到scan-to-map优化环节(即修改transformAftMapped【transformAssociateToMap函数中调用】和transformTobeMapped【scan2MapOptimization函数中调用】)。为了检测全局的大回环,还需要生成当前关键帧的ScanContext,即scManager.makeAndSaveScancontextAndKeys中。

         void mapOptimization::saveKeyFramesAndFactor() {
    
             //  此函数保存关键帧和factor
             currentRobotPosPoint.x = transformAftMapped[3];
             currentRobotPosPoint.y = transformAftMapped[4];
             currentRobotPosPoint.z = transformAftMapped[5];
    
             bool saveThisKeyFrame = true;
             if (sqrt((previousRobotPosPoint.x - currentRobotPosPoint.x) * (previousRobotPosPoint.x - currentRobotPosPoint.x)
                      +
                      (previousRobotPosPoint.y - currentRobotPosPoint.y) * (previousRobotPosPoint.y - currentRobotPosPoint.y)
                      + (previousRobotPosPoint.z - currentRobotPosPoint.z) *
                        (previousRobotPosPoint.z - currentRobotPosPoint.z)) < 0.3) { // save keyframe every 0.3 meter
                 saveThisKeyFrame = false;
             }
             if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
                 return;
    
             previousRobotPosPoint = currentRobotPosPoint;
             /**
              * update grsam graph
              */
             if (cloudKeyPoses3D->points.empty()) {
                 gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0],
                                                                         transformTobeMapped[1]),
                                                            Point3(transformTobeMapped[5], transformTobeMapped[3],
                                                                   transformTobeMapped[4])), priorNoise));
                 initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0],
                                                              transformTobeMapped[1]),
                                                 Point3(transformTobeMapped[5], transformTobeMapped[3],
                                                        transformTobeMapped[4])));
                 for (int i = 0; i < 6; ++i)
                     transformLast[i] = transformTobeMapped[i];
             } else {
                 gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
                                               Point3(transformLast[5], transformLast[3], transformLast[4]));
                 gtsam::Pose3 poseTo = Pose3(
                         Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
                         Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
                 gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size() - 1, cloudKeyPoses3D->points.size(),
                                                     poseFrom.between(poseTo), odometryNoise));
                 initialEstimate.insert(cloudKeyPoses3D->points.size(),
                                        Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0],
                                                           transformAftMapped[1]),
                                              Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
             }
             /**
              * update iSAM
              */
             isam->update(gtSAMgraph, initialEstimate);
             isam->update();
    
             gtSAMgraph.resize(0);
             initialEstimate.clear();
    
             /**
              * save key poses
              */
             PointType thisPose3D;
             PointTypePose thisPose6D;
             Pose3 latestEstimate;
    
             isamCurrentEstimate = isam->calculateEstimate();
             latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1);
    
             //  cloudKeyPoses3D指当前的点云,cloudKeyPoses6D指带pose的
             thisPose3D.x = latestEstimate.translation().y();
             thisPose3D.y = latestEstimate.translation().z();
             thisPose3D.z = latestEstimate.translation().x();
             thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
             cloudKeyPoses3D->push_back(thisPose3D);
    
             thisPose6D.x = thisPose3D.x;
             thisPose6D.y = thisPose3D.y;
             thisPose6D.z = thisPose3D.z;
             thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
             thisPose6D.roll = latestEstimate.rotation().pitch();
             thisPose6D.pitch = latestEstimate.rotation().yaw();
             thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame
             thisPose6D.time = timeLaserOdometry;
             cloudKeyPoses6D->push_back(thisPose6D);
    
             /**
              * save updated transform
              */
             if (cloudKeyPoses3D->points.size() > 1) {
                 transformAftMapped[0] = latestEstimate.rotation().pitch();
                 transformAftMapped[1] = latestEstimate.rotation().yaw();
                 transformAftMapped[2] = latestEstimate.rotation().roll();
                 transformAftMapped[3] = latestEstimate.translation().y();
                 transformAftMapped[4] = latestEstimate.translation().z();
                 transformAftMapped[5] = latestEstimate.translation().x();
    
                 for (int i = 0; i < 6; ++i) {
                     transformLast[i] = transformAftMapped[i];
                     transformTobeMapped[i] = transformAftMapped[i];
                 }
             }
    
             pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
             pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
             pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame(new pcl::PointCloud<PointType>());
    
             pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
             pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
             pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
    
             /*
                 Scan Context loop detector
                 - ver 1: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.)
                 - ver 2: using downsampled original point cloud (/full_cloud_projected + downsampling)
                 */
             bool usingRawCloud = true;
             if (usingRawCloud) { // v2 uses downsampled raw point cloud, more fruitful height information than using feature points (v1)
                 //  这里对点云提取scan context特征
                 pcl::PointCloud<PointType>::Ptr thisRawCloudKeyFrame(new pcl::PointCloud<PointType>());
                 pcl::copyPointCloud(*laserCloudRawDS, *thisRawCloudKeyFrame);
                 scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
             } else { // v1 uses thisSurfKeyFrame, it also works. (empirically checked at Mulran dataset sequences)
                 scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame);
             }
    
             cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
             surfCloudKeyFrames.push_back(thisSurfKeyFrame);
             outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
         } // saveKeyFramesAndFactor
    
  2. correctPoses:如果ScanContext回环检测(即std::thread loopthread)对全局位姿进行了优化,需要同步下来优化后的关键帧所在的位姿。判断是否回环在performLoopClosure函数中。

     void mapOptimization::performLoopClosure(void) {
         if (cloudKeyPoses3D->points.empty() == true)
             return;
    
         // try to find close key frame if there are any
         if (potentialLoopFlag == false) {
             if (detectLoopClosure() == true) {
                 std::cout << std::endl;
                 potentialLoopFlag = true; // find some key frames that is old enough or close enough for loop closure
                 timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
             }
             if (potentialLoopFlag == false) {
                 return;
             }
         }
    
         // reset the flag first no matter icp successes or not
         potentialLoopFlag = false;
    
         // *****
         // Main
         // *****
         // make common variables at forward
         float x, y, z, roll, pitch, yaw;
         Eigen::Affine3f correctionCameraFrame;
         float noiseScore = 0.5; // constant is ok...
         gtsam::Vector Vector6(6);
         Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
         constraintNoise = noiseModel::Diagonal::Variances(Vector6);
         robustNoiseModel = gtsam::noiseModel::Robust::Create(
                 gtsam::noiseModel::mEstimator::Cauchy::Create(1), // optional: replacing Cauchy by DCS or GemanMcClure
                 gtsam::noiseModel::Diagonal::Variances(Vector6)
         ); // - checked it works. but with robust kernel, map modification may be delayed (i.e,. requires more true-positive loop factors)
    
         bool isValidRSloopFactor = false;
         bool isValidSCloopFactor = false;
    
         /*
          * 1. RS loop factor (radius search)
          * 将RS检测到的历史帧和当前帧匹配,求transform, 作为约束边
          */
         if (RSclosestHistoryFrameID != -1) {
             pcl::IterativeClosestPoint<PointType, PointType> icp;
             icp.setMaxCorrespondenceDistance(100);
             icp.setMaximumIterations(100);
             icp.setTransformationEpsilon(1e-6);
             icp.setEuclideanFitnessEpsilon(1e-6);
             icp.setRANSACIterations(0);
    
             // Align clouds
             icp.setInputSource(RSlatestSurfKeyFrameCloud);
             icp.setInputTarget(RSnearHistorySurfKeyFrameCloudDS);
             pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
             icp.align(*unused_result);
    
             //  通过score阈值判定icp是否匹配成功
             std::cout << "[RS] ICP fit score: " << icp.getFitnessScore() << std::endl;
             if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) {
                 std::cout << "[RS] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")"
                           << std::endl;
                 isValidRSloopFactor = false;
             } else {
                 std::cout << "[RS] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure
                           << " ] and RS nearest [ " << RSclosestHistoryFrameID << " ]" << std::endl;
                 isValidRSloopFactor = true;
             }
    
             //  这里RS检测成功,加入约束边
             if (isValidRSloopFactor == true) {
                 correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame)
                 pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
                 Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
                 //  transform from world origin to wrong pose
                 Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(
                         cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
                 //  transform from world origin to corrected pose
                 Eigen::Affine3f tCorrect =
                         correctionLidarFrame * tWrong; // pre-multiplying -> successive rotation about a fixed frame
                 pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
                 gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
                 gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[RSclosestHistoryFrameID]);
                 gtsam::Vector Vector6(6);
    
                 std::lock_guard<std::mutex> lock(mtx);
                 gtSAMgraph.add(
                         BetweenFactor<Pose3>(latestFrameIDLoopCloure, RSclosestHistoryFrameID, poseFrom.between(poseTo),
                                              robustNoiseModel));
                 isam->update(gtSAMgraph);
                 isam->update();
                 gtSAMgraph.resize(0);
             }
         }
    
         /*
          * 2. SC loop factor (scan context)
          * SC检测成功,进行icp匹配
          */
         if (SCclosestHistoryFrameID != -1) {
             pcl::IterativeClosestPoint<PointType, PointType> icp;
             icp.setMaxCorrespondenceDistance(100);
             icp.setMaximumIterations(100);
             icp.setTransformationEpsilon(1e-6);
             icp.setEuclideanFitnessEpsilon(1e-6);
             icp.setRANSACIterations(0);
    
             // Align clouds
             // Eigen::Affine3f icpInitialMatFoo = pcl::getTransformation(0, 0, 0, yawDiffRad, 0, 0); // because within cam coord: (z, x, y, yaw, roll, pitch)
             // Eigen::Matrix4f icpInitialMat = icpInitialMatFoo.matrix();
             icp.setInputSource(SClatestSurfKeyFrameCloud);
             icp.setInputTarget(SCnearHistorySurfKeyFrameCloudDS);
             pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
             icp.align(*unused_result);
             // icp.align(*unused_result, icpInitialMat); // PCL icp non-eye initial is bad ... don't use (LeGO LOAM author also said pcl transform is weird.)
    
             std::cout << "[SC] ICP fit score: " << icp.getFitnessScore() << std::endl;
             if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore) {
                 std::cout << "[SC] Reject this loop (bad icp fit score, > " << historyKeyframeFitnessScore << ")"
                           << std::endl;
                 isValidSCloopFactor = false;
             } else {
                 std::cout << "[SC] The detected loop factor is added between Current [ " << latestFrameIDLoopCloure
                           << " ] and SC nearest [ " << SCclosestHistoryFrameID << " ]" << std::endl;
                 isValidSCloopFactor = true;
             }
    
             // icp匹配成功也加入约束边
             if (isValidSCloopFactor == true) {
                 correctionCameraFrame = icp.getFinalTransformation(); // get transformation in camera frame (because points are in camera frame)
                 pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
                 gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
                 gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
    
                 std::lock_guard<std::mutex> lock(mtx);
                 // gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise)); // original
                 gtSAMgraph.add(
                         BetweenFactor<Pose3>(latestFrameIDLoopCloure, SCclosestHistoryFrameID, poseFrom.between(poseTo),
                                              robustNoiseModel)); // giseop
                 isam->update(gtSAMgraph);
                 isam->update();
                 gtSAMgraph.resize(0);
             }
         }
    
         // flagging
         aLoopIsClosed = true;
    
     } // performLoopClosure
    
  3. publishTF:发布优化后的位姿和tf变换。

  4. publishKeyPosesAndFrames:发布所有关键帧位姿,当前的局部面点地图及当前帧中的面点/角点。
    在这里插入图片描述

到这里对SC-LeGO-LOAM的所有流程已经理清楚,同时这部分代码可以在我个人的Github项目中看到,如果各位感兴趣的可以下载测试学习。


这里感谢26.3分的HITer攻城狮の家shuang_yu_等人的相关博文提供的指导和帮助。