LIO-SAM 详读代码笔记 -- 5.MapOptimization

这个模块主要的功能就是接收featureExtraction 传来的coud_info 数据，把点云特征点根据初始化的里程计信息转换到map坐标系下，然后对当前帧的点云特征点信息进行scan-map 匹配优化，这里用到loam里面的点到线，点到面的距离作为残差项，用高斯牛顿法优化里程计信息，然后按照匀速运动模型计算一个超前一个lidar周期的odom_imcremental里程计，传送到IMU 预积分模块使用。

图2.ros topic 发布与订阅关系图关系图（还是叫图2，因为是同一个图）

图1.代码架构图(数据流图)

成员变量列表

代码注解

这里的代码的主要逻辑从 cloud_info 的回调函数为入口：

/**
 * @brief  CloudInfo 的回调函数
 * 
 * @param msgIn 
 */
    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
        // extract time stamp
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();
 
        // extract info and feature cloud
        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);  // 当前帧的角点
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);    // 当前帧的平面点
 
        std::lock_guard lock(mtx);
 
        static double timeLastProcessing = -1;
        // seconds, regulate mapping frequencymapOptimization 的执行时间间隔，地图优化的执行周期,比lidar 频率慢
        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval) 
        {
            timeLastProcessing = timeLaserInfoCur;
            /**
             * @brief 把上一次有优化的结果 transformTobeMapped存放到incrementalOdometryAffineFront 之后
             * 用imu预积分的里程计信息或者IMU 信息(只做旋转变换)初始化当前lidar的位姿transformTobeMapped
             * 
             */
            updateInitialGuess();
            /**
             * @brief Construct a new extract Surrounding Key Frames object
             * 在cloudKeyPoses3D 这个数组中搜索 以loudKeyPoses3D.back()为中心 的周边50米的距离的关键帧数据 构建局部地图
             * 并构造角点局部地图和平面点局部地图 ，
             * 存放在laserCloudCornerFromMapDS
             * 和laserCloudSurfFromMapDS
             */
            extractSurroundingKeyFrames();
            /**
             * @brief 对当前帧的点云角点 和平面点 进行下采样
             * 
             */
            downsampleCurrentScan();
            //用局部构建的地图去优化当前帧的位姿 
            scan2MapOptimization();
            // 添加因子图和保存优化后的关键帧数据
            saveKeyFramesAndFactor();
            // 如果发生回环 ，需要把回环对应的历史位姿更新以下
            correctPoses();
 
            publishOdometry();
 
            publishFrames();
        }
    }

做值优化问题，都需要有一个初始值，在loam 和lego_loam 中都是通过scan-scan 的方式得出scan-map的初始值，有了imu 数据之后，可以通过imu 预积分数据作scan-map的初始值。

 
/**
 * @brief 把上一次有优化的结果 transformTobeMapped存放到incrementalOdometryAffineFront 之后
 * 用imu预积分的里程计信息或者IMU 信息(只做旋转变换)初始化当前lidar的位姿transformTobeMapped
 * 
 */
    void updateInitialGuess()
    {
        // save current transformation before any processing
        incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped); // 把上一次优化的结果 保存在incrementalOdometryAffineFront
 
        static Eigen::Affine3f lastImuTransformation; // 保存IMU 里程计到当前帧时刻的里程计信息
        // initialization
        if (cloudKeyPoses3D->points.empty()) // 地图里面没有数据，表示当前为初始位置，
        {
            // 初始时刻 机器人的朝向取 IMU 的朝向
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;
 
            // 超参数：在yaml 文件中设置 if using GPS data, set to "true" ，useImuHeadingInitialization 如果不用 imu朝向 ，把yaw 置为0  
            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;
 
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
        }
 
        // use imu pre-integration estimation for pose guess
        static bool lastImuPreTransAvailable = false;
        //这个变量保存IMU 的预积分的里程计信息，关联两次优化
        static Eigen::Affine3f lastImuPreTransformation;
        if (cloudInfo.odomAvailable == true)
        {
            // 这里看到一对 XXXFront 和 XXXBack ，之后肯定是要作差的，表示之前的时刻到当前时刻的位姿差
            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                               cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
            if (lastImuPreTransAvailable == false)
            {   // 第一次设置时， 
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            } else {
                // 两次优化之间的发生的位姿差 delta_T = T_i^T * T_j
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                // T_j'' = T_i' * delta_T 其中 T_j'' 为待优化的变量的初始值
                Eigen::Affine3f transFinal = transTobe * transIncre;
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
 
                lastImuPreTransformation = transBack; // 一直保存IMU 预积分里程计的信息，传递下去
 
                lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                return;
            }
        }
 
        // use imu incremental estimation for pose guess (only rotation)
        if (cloudInfo.imuAvailable == true)
        {
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
 
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
 
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
        }
    }

在scan-map 优化执行之前，首先需要有一个map，淡然不能是全局map,内存撑爆，且运算时间剧增，也没必要用全局map ,这里用的是距离最新历史关键帧50米范围内的关键帧数据作为一个局部地图取匹配优化当前帧的位姿。

/**
 * 这个函数就是调用了extractNearby函数
 * */
    void extractSurroundingKeyFrames()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return; 
        // ？
        // if (loopClosureEnableFlag == true)
        // {
        //     extractForLoopClosure();    
        // } else {
        //     extractNearby();
        // }
 
        extractNearby();
    }

上面这个函数就是调用了extractNearby函数，在cloudKeyPoses3D 这个数组中搜索 以loudKeyPoses3D.back()为中心 的周边50米的距离的关键帧数据 构建局部地图。

/**
 * @brief 在cloudKeyPoses3D 这个数组中搜索 以loudKeyPoses3D.back()为中心 的周边50米的距离的关键帧数据 构建局部地图
 * 
 */
    void extractNearby()
    {   // 搜索出来距离50米的关键帧位姿（位置）存放在这个点云容器中
        pcl::PointCloud::Ptr surroundingKeyPoses(new pcl::PointCloud());
        pcl::PointCloud::Ptr surroundingKeyPosesDS(new pcl::PointCloud());
        // 这两个变量是 KDtree 搜索用到的两个临时变量
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;
 
        // extract all the nearby key poses and downsample them
        kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
        // 以cloudKeyPoses3D->back() 为中心 以surroundingKeyframeSearchRadius（默认50米）为半径 搜索最近的点
        kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            int id = pointSearchInd[i];
            surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
        }
 
        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        // 下采样之后，点云里的点不完全时原始的点，有些是融合过 ，所以时间戳信息 需要重新获取一下
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        for(auto& pt : surroundingKeyPosesDS->points)
        {   // 再找一遍，把原始点云的最近的点的intensity赋值给新的位置点 ，intensity 在这个是在这个地图中的索引 也是laserCloudMapContainer容器的索引
            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
            pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
        }
 
        // also extract some latest key frames in case the robot rotates in one position
        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses-1; i >= 0; --i)
        {
            // 过滤完之后 ，surroundingKeyPosesDS在添加一些  时间比较接近的位置点
            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
            else
                break;
        }
 
        extractCloud(surroundingKeyPosesDS);
    }

然后再调用extractCloud 函数,搜索出来距离50米的关键帧位姿（位置）的角点点云和平面点点云,存放在laserCloudCornerFromMapDS和laserCloudSurfFromMapDS中

/**
 * @brief  搜索出来距离50米的关键帧位姿（位置）的角点点云和平面点点云
 * 存放在laserCloudCornerFromMapDS
 * 和laserCloudSurfFromMapDS
 * 
 * @param cloudToExtract 
 */
    void extractCloud(pcl::PointCloud::Ptr cloudToExtract)
    {
        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear(); 
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {   // 添加关键帧的搜索半径 ， 局部地图的距离最新帧中心阈值
            if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                continue;
 
            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) 
            {   // 局部地图 角点地图 和平面点地图
                // transformed cloud available
                *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                *laserCloudSurfFromMap   += laserCloudMapContainer[thisKeyInd].second;
            } else {
                // transformed cloud not available
                // 把点云 从lidar 坐标系转到world 坐标系
                pcl::PointCloud laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
                pcl::PointCloud laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
                *laserCloudCornerFromMap += laserCloudCornerTemp;
                *laserCloudSurfFromMap   += laserCloudSurfTemp;
                // 添加到 地图容器中
                laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
            }
            
        }
 
        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
 
        // clear map cache if too large
        // 地图数据清空 缓存1000帧
        if (laserCloudMapContainer.size() > 1000)
            laserCloudMapContainer.clear();
    }

有了初始值，有了局部地图，就可以执行scan-map 匹配优化了。

分三部：1.根据初始值变换角点到map 坐标系下，计算点到线的残差项，及其对变换后的点的雅可比，保存起来。

2. 根据初始值变换平面点到map 坐标系下，计算点到面的残差项，及其对变换后的点的雅可比，保存起来。

3. 计算变换后的点对变换矩阵中 R 和t 的雅可比，然后用高斯牛顿法优化待优化变量。

这里详细讲解一下计算过程，可能这部分内容源自loam 或者是其他文献，所以著者也没有详细给出讲解：

(1)先准备以下数据：由于有了初始值 transformTobeMapped ，我们可以得到初始的旋转角,, ,,,。

所与可以得到旋转矩阵

(2) 论文中给出 点到线的距离公式，这里直接沿用，至于怎么来loam中讲得很明白，这里旨在读懂代码，明确代码里的变量怎么来。

点到线的距离公式：,为了简化公式编写且与代码中的变量标号相对应，点到线的距离公式简化成 ，首先要确定这几个点是怎么来的，其中 为从lidar 坐标系变换到map 坐标系下的点，即 ,p1p1和p2p2主要来自与主成分分析法(PCA),在map 中找出距离 最近的5个点，作主成分分析，如果这5个的点协方差矩阵的其中一个特征值远大于其他两个，则这5个点成线性分布，最大特征值对应的特征向量即为该直线的方向，在这5个点的中心沿着直线的正反方向各取一个点，即为p1和p2。

（3）这里先给出 对R和t的雅可比，因为无论是平面点 或者是角点特征，都经过了这个变换，雅可比是一致，代码中也是这么分开来实现的。

∂p0∂θ=∂(Rpe+t)∂θ=limθ−>0exp(I+θ×)Rpe−Rpeθ=limθ−>0(I+θ×)Rpe−Rpeθ=θ×Rpeθ=−(Rpe)×θθ=−(Rpe)×

对t的雅可比 为I。

（4）在函数cornerOptimization中求得点到线距离的残差项 存放在coeff.intensity 中，雅可比项就是存放在coeff的x,y,z 中。现在来推到点到线距离残差项d对的雅可比。

1）首先要知道 ∂‖x‖∂x=x‖x‖  各位可以 用（x,y,z）的模来推导一下.

2)  ∂d∂p0=1‖p1−p2‖∂‖(p0−p1)×(p0−p2)‖∂p0

=1‖p1−p2‖(p0−p1)×(p0−p2)‖(p0−p1)×(p0−p2)‖∂(p0−p1)×(p0−p2)∂p0

=1‖p1−p2‖(p0−p1)×(p0−p2)‖(p0−p1)×(p0−p2)‖(∂(p0−p1)×(p0−p2)∂p0+(p0−p1)×∂(p0−p2)∂p0)=1‖p1−p2‖(p0−p1)×(p0−p2)‖(p0−p1)×(p0−p2)‖(−(p0−p2)×∂(p0−p1)∂p0+(p0−p1)×∂(p0−p2)∂p0)=1‖p1−p2‖(p0−p1)×(p0−p2)‖(p0−p1)×(p0−p2)‖(−(p0−p2)×+(p0−p1)×)

=1‖p1−p2‖(p0−p1)×(p0−p2)‖(p0−p1)×(p0−p2)‖(−(p1−p2)×)

然后一项一项对应到代码里面去就好了。这样比看代码好理解一些。点到平面的距离就不写了（懒）。

void cornerOptimization()
    {   // 把初始化过后的待优化变量transformTobeMapped 转换成一个转换矩阵transPointAssociateToMap，
        // 用于对当前帧点云的坐标系转化到map 坐标系 
        updatePointAssociateToMap();
 
        #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {
            // pointOri lidar 坐标系下的特征点    
            // pointSel 是pointOri 转化到Map 坐标系下的点
            // coeff 距离函数对pointSel 的雅可比
            PointType pointOri, pointSel, coeff;
            // kdtree 搜索近邻点所需要的两个临时变量
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;
 
            pointOri = laserCloudCornerLastDS->points[i]; //当前帧的特征点
            // 把点云点从lidar 坐标系映射到Map 坐标系 
            pointAssociateToMap(&pointOri, &pointSel);
            // 以pointSel 为中心在局部地图的KdTree中找最近的5个点
            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
 
            cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); // matA1 为这5个点的协方差矩阵
            cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); // matD1 为协方差矩阵的特征值
            cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); // matV1 为协方差矩阵的特征向量
             //从KdTree中找最近的5个点中最远距离的那个的距离小于1M，认为中的找到的这5个点有效       
            if (pointSearchSqDis[4] < 1.0) {
                // C =[cx,cy,cz]^T 为这5个点的中心
                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;
                }
                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;
                }
                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;
                // 求取matA1 的特征值和特征向量  
                cv::eigen(matA1, matD1, matV1);
                // matD1 是按从达到小的顺序分布的，如果特征值的第1个值比第二值足够大  则认为 这5个点是成线型分布的
                // 并且 其对应的特征向量则为直线的方向
                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
                    // 构造3个点，这样就可计算一个点到其他两个点所在的直线的距离
                    // 1.当前帧lidar 在map坐标系下的映射点
                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    // 2. 在5点的均值点处沿着 直线方向 距离0.1 取一个点
                    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);
                    // 3. 在5点的均值点处沿着 直线反方向 距离0.1 取一个点
                    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);
 
                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;
 
                    if (s > 0.1) {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;
                    }
                }
            }
        }
    }
 
    void surfOptimization()
    {
        updatePointAssociateToMap();
 
        #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudSurfLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;
 
            pointOri = laserCloudSurfLastDS->points[i];
            pointAssociateToMap(&pointOri, &pointSel); 
            kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
 
            Eigen::Matrix<float, 5, 3> matA0;
            Eigen::Matrix<float, 5, 1> matB0;
            Eigen::Vector3f matX0;
 
            matA0.setZero();
            matB0.fill(-1);
            matX0.setZero();
 
            if (pointSearchSqDis[4] < 1.0) {
                for (int j = 0; j < 5; j++) {
                    matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }
 
                matX0 = matA0.colPivHouseholderQr().solve(matB0);
 
                float pa = matX0(0, 0);
                float pb = matX0(1, 0);
                float pc = matX0(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(pointOri.x * pointOri.x
                            + pointOri.y * pointOri.y + pointOri.z * pointOri.z));
 
                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;
 
                    if (s > 0.1) {
                        laserCloudOriSurfVec[i] = pointOri;
                        coeffSelSurfVec[i] = coeff;
                        laserCloudOriSurfFlag[i] = true;
                    }
                }
            }
        }
    }
 
    void combineOptimizationCoeffs()
    {
        // combine corner coeffs
        for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
            if (laserCloudOriCornerFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriCornerVec[i]);
                coeffSel->push_back(coeffSelCornerVec[i]);
            }
        }
        // combine surf coeffs
        for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
            if (laserCloudOriSurfFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                coeffSel->push_back(coeffSelSurfVec[i]);
            }
        }
        // reset flag for next iteration
        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
    }
 
    bool LMOptimization(int iterCount)
    {
        // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
        // lidar <- camera      ---     camera <- lidar
        // x = z                ---     x = y
        // y = x                ---     y = z
        // z = y                ---     z = x
        // roll = yaw           ---     roll = pitch
        // pitch = roll         ---     pitch = yaw
        // yaw = pitch          ---     yaw = roll
 
        // lidar -> camera
        float srx = sin(transformTobeMapped[1]);
        float crx = cos(transformTobeMapped[1]);
        float sry = sin(transformTobeMapped[2]);
        float cry = cos(transformTobeMapped[2]);
        float srz = sin(transformTobeMapped[0]);
        float crz = cos(transformTobeMapped[0]);
 
        int laserCloudSelNum = laserCloudOri->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));
 
        PointType pointOri, coeff;
 
        for (int i = 0; i < laserCloudSelNum; i++) {
            // 把LIdar 坐标系(xyz-前左上)的数据转成跟Camera 坐标系（xyz-左 上 前）一样的数据（这么做纯粹是为了抄代码吗？）
            // lidar -> camera
            pointOri.x = laserCloudOri->points[i].y;
            pointOri.y = laserCloudOri->points[i].z;
            pointOri.z = laserCloudOri->points[i].x;
            // lidar -> camera
            coeff.x = coeffSel->points[i].y;
            coeff.y = coeffSel->points[i].z;
            coeff.z = coeffSel->points[i].x;
            coeff.intensity = coeffSel->points[i].intensity;
            // in camera
            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;
            // camera -> lidar
            matA.at<float>(i, 0) = arz;
            matA.at<float>(i, 1) = arx;
            matA.at<float>(i, 2) = ary;
            matA.at<float>(i, 3) = coeff.z;
            matA.at<float>(i, 4) = coeff.x;
            matA.at<float>(i, 5) = coeff.y;
            matB.at<float>(i, 0) = -coeff.intensity;
        }
 
        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;
        }
 
        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; // converged
        }
        return false; // keep optimizing
    }

后面关于因子图的代码就不说了，在imupreintegration 中有讲了，关键是这个文件太长了，乏了。