LeGO-LOAM分析之建图（三）

爱被打了一巴掌 2022-09-12 05:42 2阅读 0赞

转载地址：[https://zhuanlan.zhihu.com/p/386235895][https_zhuanlan.zhihu.com_p_386235895]  
接着上一章的分析，特征提取和特征匹配2步完成了lidar里程计的输出。接着看建图过程的分析。

### 整体介绍 ###

LeGO-LOAM把当前帧扫描到的点云特征 \{  F e t , F p t F\_e^t,F\_p^t Fet,Fpt\}，这里 F e t F\_e^t Fet表示包含线特征的点云，  F p t F\_p^t Fpt 表示包含面特征的点云，和当前帧周围的点云地图  Q − t − 1 Q^\{-t-1\} Q−t−1 做匹配，进一步优化姿势变换。并且这个过程以一个较低的频率运行。

LeGO-LOAM和LOAM的区别在于，LeGO-LOAM保存的是每帧的特征点云集合，而LOAM保存的是一个点云地图。假设  M t − 1 M^\{t-1\} Mt−1=\{ \{  F e 1 , F p 1 F\_e^1,F\_p^1 Fe1,Fp1\},…,\{  F e t − 1 , F p t − 1 F\_e^\{t-1\},F\_p^\{t-1\} Fet−1,Fpt−1\}\}是保存了之前所有点云特征的集合，  M t − 1 M^\{t-1\} Mt−1 中的每个特征集还与传感器的位姿相关联。那么有2种方式从  M t − 1 M^\{t-1\} Mt−1 获取周围的点云 Q − t − 1 Q^\{-t-1\} Q−t−1 。

*  第一种方式是**获取传感器视野范围内的特征集**，我们通过查找离当前位置100米以内的特征集，然后把这些特征融合成一个点云 \[公式\] ，就得到了当前位置附近的点云地图。
 *  第二种方式是通过**建立姿态图(Pose-Graph)**，每个特征集（一帧的点和线特征）的pose都可以当作姿态图中的一个节点，特征\{  F e t , F p t F\_e^t,F\_p^t Fet,Fpt\} 可以视为该节点的传感器测量。由于激光雷达制图模块的姿态估计漂移（drift）非常低，我们可以假设短时间内没有漂移。那么选取最近的一组特征集合  M t − 1 M^\{t-1\} Mt−1=\{ \{  F e t − k , F p t − k F\_e^\{t-k\},F\_p^\{t-k\} Fet−k,Fpt−k\},…,\{  F e t − 1 , F p t − 1 F\_e^\{t-1\},F\_p^\{t-1\} Fet−1,Fpt−1\}\}大小为k，那么新节点和从 Q − t − 1 Q^\{-t-1\} Q−t−1 中选择的节点之间的空间约束可以使用LM优化后得到的变换。  
    我们也可以通过回环检测来进一步消除漂移(draft)，回环检测是通过ICP匹配当前帧和之前的点云，添加新的空间约束。然后通过iSAM2来优化姿态图。

### 代码分析 ###

接着我们还是从主函数开始分析，和特征提取和特征匹配类似，主函数分为2个主要的功能，一是启动单独的线程进行回环检测，二是在循环中执行主流程。

int main(int argc, char** argv)
    {
        
        ros::init(argc, argv, "lego_loam");
    
        ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    
        mapOptimization MO;
        // 1. 回环检测
        std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
        // 2. 可视化线程
        std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    
        ros::Rate rate(200);
        while (ros::ok())
        {
        
            ros::spinOnce();
            // 3. 主流程
            MO.run();
    
            rate.sleep();
        }
    
        loopthread.join();
        visualizeMapThread.join();
        return 0;
    }

### 主流程 ###

lidar制图在构造函数中订阅消息，订阅的消息主要是以下5个。主要是接收特征提取之后的点云，lidar里程计和IMU消息。

subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);
        subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
        subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);
        subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);
        subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);

接下来我们接着看run函数，流程非常清晰

void 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);
                // 设置lidar mapping时间间隔
                if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
        
                    timeLastProcessing = timeLaserOdometry;
                    // 1. 转换到map坐标系
                    transformAssociateToMap();
                    // 2. 提取周围关键帧
                    extractSurroundingKeyFrames();
                    // 3. 下采样当前帧
                    downsampleCurrentScan();
                    // 4. scan到map优化
                    scan2MapOptimization();
                    // 5. 保存关键帧和因子
                    saveKeyFramesAndFactor();
                    // 6. 校正位姿
                    correctPoses();
                    // 7. 发布坐标转换
                    publishTF();
                    // 8. 发布关键帧和姿态
                    publishKeyPosesAndFrames();
                    // 9. 清除点云
                    clearCloud();
                }
            }
        }
    };

### 2. 提取周围关键帧 ###

如果使能了回环检测，则添加过去50个关键帧，如果没有使能回环检测，则添加离当前欧式距离最近的50个关键帧，然后拼接成点云。

void extractSurroundingKeyFrames(){
        
    
            if (cloudKeyPoses3D->points.empty() == true)
                return;	
            // 如果使能回环检测
        	if (loopClosureEnableFlag == true){
        
        	    // only use recent key poses for graph building
                // 1. 添加最近的关键位姿到图
                    if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){
         // queue is not full (the beginning of mapping or a loop is just closed)
                        // clear recent key frames queue
                        recentCornerCloudKeyFrames. clear();
                        recentSurfCloudKeyFrames.   clear();
                        recentOutlierCloudKeyFrames.clear();
                        int numPoses = cloudKeyPoses3D->points.size();
                        for (int i = numPoses-1; i >= 0; --i){
        
                            int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
                            PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
                            updateTransformPointCloudSinCos(&thisTransformation);
                            // extract surrounding map
                            // 提取过去50个关键帧
                            recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
                            recentSurfCloudKeyFrames.   push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
                            recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
                            if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
                                break;
                        }
                    }else{
          // queue is full, pop the oldest key frame and push the latest key frame
                        if (latestFrameID != cloudKeyPoses3D->points.size() - 1){
          // if the robot is not moving, no need to update recent frames
    
                            recentCornerCloudKeyFrames. pop_front();
                            recentSurfCloudKeyFrames.   pop_front();
                            recentOutlierCloudKeyFrames.pop_front();
                            // push latest scan to the end of queue
                            // 2. 弹出队列中时间最久的帧，添加最新的帧到队列
                            latestFrameID = cloudKeyPoses3D->points.size() - 1;
                            PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
                            updateTransformPointCloudSinCos(&thisTransformation);
                            recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
                            recentSurfCloudKeyFrames.   push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
                            recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
                        }
                    }
                    // 3. 拼接为点云
                    for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i){
        
                        *laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
                        *laserCloudSurfFromMap   += *recentSurfCloudKeyFrames[i];
                        *laserCloudSurfFromMap   += *recentOutlierCloudKeyFrames[i];
                    }
        	}else{
          // 如果没有使能回环检测
                surroundingKeyPoses->clear();
                surroundingKeyPosesDS->clear();
        	    // extract all the nearby key poses and downsample them
                // 1. 查找当前pose 50米内的姿态
        	    kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
        	    kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
        	    for (int i = 0; i < pointSearchInd.size(); ++i)
                    surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
        	    downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        	    downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        	    // delete key frames that are not in surrounding region
                // 2. 删除不在周围区域的关键帧
                int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();
                for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){
        
                    bool existingFlag = false;
                    for (int j = 0; j < numSurroundingPosesDS; ++j){
        
                        if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){
        
                            existingFlag = true;
                            break;
                        }
                    }
                    if (existingFlag == false){
        
                        surroundingExistingKeyPosesID.   erase(surroundingExistingKeyPosesID.   begin() + i);
                        surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i);
                        surroundingSurfCloudKeyFrames.   erase(surroundingSurfCloudKeyFrames.   begin() + i);
                        surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);
                        --i;
                    }
                }
        	    // add new key frames that are not in calculated existing key frames
                // 3. 添加新关键帧
                for (int i = 0; i < numSurroundingPosesDS; ++i) {
        
                    bool existingFlag = false;
                    for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){
        
                        if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){
        
                            existingFlag = true;
                            break;
                        }
                    }
                    if (existingFlag == true){
        
                        continue;
                    }else{
        
                        int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;
                        PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
                        updateTransformPointCloudSinCos(&thisTransformation);
                        surroundingExistingKeyPosesID.   push_back(thisKeyInd);
                        surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
                        surroundingSurfCloudKeyFrames.   push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
                        surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
                    }
                }
                // 4. 拼接点云
                for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
        
                    *laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
                    *laserCloudSurfFromMap   += *surroundingSurfCloudKeyFrames[i];
                    *laserCloudSurfFromMap   += *surroundingOutlierCloudKeyFrames[i];
                }
        	}
            // 下采样角特征和面特征
            // Downsample the surrounding corner key frames (or map)
            downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
            downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
            laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();
            // Downsample the surrounding surf key frames (or map)
            downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
            downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
            laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();
        }

### 4. scan到map优化 ###

通过最小二乘法，添加当前的扫描帧到map

void scan2MapOptimization(){
        
    
            if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
        
                // 1. 根据周围关键帧点云创建kdtree（角特征点云、面特征点云）
                kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
                kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
    
                for (int iterCount = 0; iterCount < 10; iterCount++) {
        
    
                    laserCloudOri->clear();
                    coeffSel->clear();
                    // 2.角和面特征计算距离
                    cornerOptimization(iterCount);
                    surfOptimization(iterCount);
                    // 3. LM优化
                    if (LMOptimization(iterCount) == true)
                        break;              
                }
    
                transformUpdate();
            }
        }

### 角特征优化 ###

void cornerOptimization(int iterCount){
        
    
            updatePointAssociateToMapSinCos();
            for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
        
                pointOri = laserCloudCornerLastDS->points[i];
                pointAssociateToMap(&pointOri, &pointSel);
                // 1. 查找最近的5个点
                kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
                // 2. 上述点距离小于1m
                if (pointSearchSqDis[4] < 1.0) {
        
                    float cx = 0, cy = 0, cz = 0;
                    for (int j = 0; j < 5; j++) {
        
                        cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                        cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                        cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                    }
                    // 3. 5个点的质心
                    cx /= 5; cy /= 5;  cz /= 5;
                    
                    float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                    for (int j = 0; j < 5; j++) {
        
                        float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                        float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                        float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
    
                        a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                        a22 += ay * ay; a23 += ay * az;
                        a33 += az * az;
                    }
                    // 4. 协方差
                    a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
    
                    matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                    matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                    matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
    
                    cv::eigen(matA1, matD1, matV1);
    
                    if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
        
    
                        float x0 = pointSel.x;
                        float y0 = pointSel.y;
                        float z0 = pointSel.z;
                        float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                        float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                        float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                        float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                        float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                        float z2 = cz - 0.1 * matV1.at<float>(0, 2);
    
                        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)));
    
                        float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
    
                        float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                  + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
    
                        float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                   - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
    
                        float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                   + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
    
                        float ld2 = a012 / l12;
    
                        float s = 1 - 0.9 * fabs(ld2);
                        // 5. 计算coeff
                        coeff.x = s * la;
                        coeff.y = s * lb;
                        coeff.z = s * lc;
                        coeff.intensity = s * ld2;
    
                        if (s > 0.1) {
        
                            laserCloudOri->push_back(pointOri);
                            coeffSel->push_back(coeff);
                        }
                    }
                }
            }
        }

### 面特征优化 ###

void surfOptimization(int iterCount){
        
            updatePointAssociateToMapSinCos();
            for (int i = 0; i < laserCloudSurfTotalLastDSNum; i++) {
        
                pointOri = laserCloudSurfTotalLastDS->points[i];
                pointAssociateToMap(&pointOri, &pointSel); 
                kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
    
                if (pointSearchSqDis[4] < 1.0) {
        
                    for (int j = 0; j < 5; j++) {
        
                        matA0.at<float>(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                        matA0.at<float>(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                        matA0.at<float>(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                    }
                    cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);
    
                    float pa = matX0.at<float>(0, 0);
                    float pb = matX0.at<float>(1, 0);
                    float pc = matX0.at<float>(2, 0);
                    float pd = 1;
    
                    float ps = sqrt(pa * pa + pb * pb + pc * pc);
                    pa /= ps; pb /= ps; pc /= ps; pd /= ps;
    
                    bool planeValid = true;
                    for (int j = 0; j < 5; j++) {
        
                        if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                                 pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                                 pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
        
                            planeValid = false;
                            break;
                        }
                    }
    
                    if (planeValid) {
        
                        float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
    
                        float s = 1 - 0.9 * 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) {
        
                            laserCloudOri->push_back(pointOri);
                            coeffSel->push_back(coeff);
                        }
                    }
                }
            }
        }

### LM优化过程 ###

bool LMOptimization(int iterCount){
        
            float srx = sin(transformTobeMapped[0]);
            float crx = cos(transformTobeMapped[0]);
            float sry = sin(transformTobeMapped[1]);
            float cry = cos(transformTobeMapped[1]);
            float srz = sin(transformTobeMapped[2]);
            float crz = cos(transformTobeMapped[2]);
    
            int laserCloudSelNum = laserCloudOri->points.size();
            if (laserCloudSelNum < 50) {
        
                return false;
            }
    
            cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
            cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
            cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
            cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
            // 1. 遍历laserCloudSel
            for (int i = 0; i < laserCloudSelNum; i++) {
        
                pointOri = laserCloudOri->points[i];
                coeff = coeffSel->points[i];
    
                float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                          + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                          + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
    
                float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
                          + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                          + ((-cry*crz - srx*sry*srz)*pointOri.x 
                          + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
    
                float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                          + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                          + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
    
                matA.at<float>(i, 0) = arx;
                matA.at<float>(i, 1) = ary;
                matA.at<float>(i, 2) = arz;
                matA.at<float>(i, 3) = coeff.x;
                matA.at<float>(i, 4) = coeff.y;
                matA.at<float>(i, 5) = coeff.z;
                matB.at<float>(i, 0) = -coeff.intensity;
            }
            // 2. 最小二乘法
            cv::transpose(matA, matAt);
            matAtA = matAt * matA;
            matAtB = matAt * matB;
            cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
            if (iterCount == 0) {
        
                cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
                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] = {
        100, 100, 100, 100, 100, 100};
                for (int i = 5; i >= 0; i--) {
        
                    if (matE.at<float>(0, i) < eignThre[i]) {
        
                        for (int j = 0; j < 6; j++) {
        
                            matV2.at<float>(i, j) = 0;
                        }
                        isDegenerate = true;
                    } else {
        
                        break;
                    }
                }
                matP = matV.inv() * matV2;
            }
    
            if (isDegenerate) {
        
                cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
                matX.copyTo(matX2);
                matX = matP * matX2;
            }
            // 3. 获取transform
            transformTobeMapped[0] += matX.at<float>(0, 0);
            transformTobeMapped[1] += matX.at<float>(1, 0);
            transformTobeMapped[2] += matX.at<float>(2, 0);
            transformTobeMapped[3] += matX.at<float>(3, 0);
            transformTobeMapped[4] += matX.at<float>(4, 0);
            transformTobeMapped[5] += matX.at<float>(5, 0);
    
            float deltaR = sqrt(
                                pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
                                pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
                                pow(pcl::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.05 && deltaT < 0.05) {
        
                return true;
            }
            return false;
        }

### 5. 保存关键帧和因子 ###

通过前后2帧进行优化，保存优化后的位姿

void saveKeyFramesAndFactor(){
        
    
            currentRobotPosPoint.x = transformAftMapped[3];
            currentRobotPosPoint.y = transformAftMapped[4];
            currentRobotPosPoint.z = transformAftMapped[5];
            // 当前帧和之前帧的距离小于0.3米
            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){
        
                saveThisKeyFrame = false;
            }
            // saveThisKeyFrame为false，并且cloudKeyPoses3D不为空
            if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
            	return;
    
            previousRobotPosPoint = currentRobotPosPoint;
            /**
             * 把当前pose加入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])));
            }
            /**
             * 更新 iSAM
             */
            isam->update(gtSAMgraph, initialEstimate);
            isam->update();
            
            gtSAMgraph.resize(0);
            initialEstimate.clear();
    
            /**
             * 保存关键poses
             */
            PointType thisPose3D;
            PointTypePose thisPose6D;
            Pose3 latestEstimate;
    
            isamCurrentEstimate = isam->calculateEstimate();
            latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
    
            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);
            // 保存点云
            cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
            surfCloudKeyFrames.push_back(thisSurfKeyFrame);
            outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
        }

### 6. 校正位姿 ###

回环检测如果成功，会设置aLoopIsClosed为true，才会进行这一步。将isam优化后的位姿替换之前的位姿。

void correctPoses(){
        
    	if (aLoopIsClosed == true){
        
            recentCornerCloudKeyFrames. clear();
            recentSurfCloudKeyFrames.   clear();
            recentOutlierCloudKeyFrames.clear();
            // update key poses
            // 1. 将isam优化后的位姿替换之前的位姿
                int numPoses = isamCurrentEstimate.size();
            for (int i = 0; i < numPoses; ++i){
        
            cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().y();
            cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().z();
            cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().x();
    
            cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
            cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
            cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
            cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
            cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
            cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
            }
    
            aLoopIsClosed = false;
        }
    }

### 回环检测 ###

回环检测可以消除漂移(drift)，通过ICP算法对比当前帧和之前帧是否匹配，如果匹配则进行图优化。

回环检测在loopClosureThread中进行

void loopClosureThread(){
        
    
            if (loopClosureEnableFlag == false)
                return;
    
            ros::Rate rate(1);
            while (ros::ok()){
        
                rate.sleep();
                // 进行回环检测
                performLoopClosure();
            }
        }

下面在看回环检测的过程。判断是否进入回环在**detectLoopClosure**中进行，判断条件是**首尾之间的距离小于7米，并且时间相差30s以上**。

bool detectLoopClosure(){
        
    
            latestSurfKeyFrameCloud->clear();
            nearHistorySurfKeyFrameCloud->clear();
            nearHistorySurfKeyFrameCloudDS->clear();
    
            std::lock_guard<std::mutex> lock(mtx);
            // find the closest history key frame
            std::vector<int> pointSearchIndLoop;
            std::vector<float> pointSearchSqDisLoop;
            // 1. 查找当前点7米范围内是否有之前已经采样的点
            kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
            kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
            
            closestHistoryFrameID = -1;
            // 2. 时间相差30s以上
            for (int i = 0; i < pointSearchIndLoop.size(); ++i){
        
                int id = pointSearchIndLoop[i];
                if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0){
        
                    closestHistoryFrameID = id;
                    break;
                }
            }
            if (closestHistoryFrameID == -1){
        
                return false;
            }
            // save latest key frames
            // 保存最新的关键帧
            latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
            *latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
            *latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],   &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
    
            pcl::PointCloud<PointType>::Ptr hahaCloud(new pcl::PointCloud<PointType>());
            int cloudSize = latestSurfKeyFrameCloud->points.size();
            for (int i = 0; i < cloudSize; ++i){
        
                if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0){
        
                    hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]);
                }
            }
            latestSurfKeyFrameCloud->clear();
            *latestSurfKeyFrameCloud = *hahaCloud;
    	   // save history near key frames
            for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
        
                if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
                    continue;
                *nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
                *nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j],   &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
            }
    
            downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);
            downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);
            // publish history near key frames
            if (pubHistoryKeyFrames.getNumSubscribers() != 0){
        
                sensor_msgs::PointCloud2 cloudMsgTemp;
                pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);
                cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                cloudMsgTemp.header.frame_id = "/camera_init";
                pubHistoryKeyFrames.publish(cloudMsgTemp);
            }
    
            return true;
        }

如果检测到回环之后，接着进行ICP匹配，然后进行图优化。作者同时提醒回环检测的\*\*ICP算法当里程计漂移太大时经常失败。\*\*对于更高级的闭环方法，建议采样 SC-LeGO-LOAM ，它的特征采用的是点云描述符。

void performLoopClosure(){
        
    
            if (cloudKeyPoses3D->points.empty() == true)
                return;
            // try to find close key frame if there are any
            // 尝试去寻找回环点
            if (potentialLoopFlag == false){
        
    
                if (detectLoopClosure() == true){
        
                    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;
            // ICP Settings
            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(latestSurfKeyFrameCloud);
            icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
            pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
            icp.align(*unused_result);
    
            if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
                return;
            // publish corrected cloud
            // 发布校正之后的点云
            if (pubIcpKeyFrames.getNumSubscribers() != 0){
        
                pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
                pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
                sensor_msgs::PointCloud2 cloudMsgTemp;
                pcl::toROSMsg(*closed_cloud, cloudMsgTemp);
                cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                cloudMsgTemp.header.frame_id = "/camera_init";
                pubIcpKeyFrames.publish(cloudMsgTemp);
            }   
            /*
            get pose constraint
            */
            float x, y, z, roll, pitch, yaw;
            Eigen::Affine3f correctionCameraFrame;
            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[closestHistoryFrameID]);
            gtsam::Vector Vector6(6);
            float noiseScore = icp.getFitnessScore();
            Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
            constraintNoise = noiseModel::Diagonal::Variances(Vector6);
            /* 
            add constraints
            添加约束
            */
            std::lock_guard<std::mutex> lock(mtx);
            gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
            isam->update(gtSAMgraph);
            isam->update();
            gtSAMgraph.resize(0);
    
            aLoopIsClosed = true;
        }

### 总结 ###

至此LeGO-LOAM建图的过程就分析完成了，LeGO-LOAM加入了回环检测，同时通过scan-2-map的方式生成点云地图。

[https_zhuanlan.zhihu.com_p_386235895]: https://zhuanlan.zhihu.com/p/386235895