LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）

LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。

本文原地址：wykxwyc的博客

github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：

1.地图优化代码理解

2.图像重投影代码理解

3.特征关联代码理解

4.LeGO-LOAM中的数学公式推导

以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。

mapOptmization.cpp总体功能论述

mapOptmization.cpp进行的内容主要是地图优化，将得到的局部地图信息融合到全局地图中去。

main

main()函数的关键代码就三条，也就是三个不同的线程，最重要的是run()函数：

std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
MO.run();

详细的main()函数的内容如下：

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lego_loam");
    
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    
    mapOptimization MO;
    
    // std::thread 构造函数，将MO作为参数传入构造的线程中使用
    // 进行闭环检测与闭环的功能
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
    	
    // 该线程中进行的工作是publishGlobalMap(),将数据发布到ros中，可视化
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    
    ros::Rate rate(200);
    while (ros::ok())
    {
	    ros::spinOnce();
	    
	    MO.run();
	    
	    rate.sleep();
    }
    loopthread.join();
    visualizeMapThread.join();
    
    return 0;
}

loopthread

分析一下std::thread loopthread(...)部分的代码，它的主要功能是进行闭环检测和闭环修正。
关于std::thread的构造函数可以参考这里。
其中关于std::thread 的构造函数之一：

template <class Fn, class... Args>
explicit thread (Fn&& fn, Args&&... args);

fn是一个函数指针，指向成员函数（此处是loopClosureThread()）或一个可移动构造函数，关于fn的解释：

fn
A pointer to function, pointer to member, or any kind of move-constructible function object (i.e., an object whose class defines ***operator()***, including closures and function objects).
The return value (if any) is ignored.

loopClosureThread()函数：

void loopClosureThread(){
   

    if (loopClosureEnableFlag == false)
        return;

    ros::Rate rate(1);
    while (ros::ok()){
   
        rate.sleep();
        performLoopClosure();
    }
}

上面主要的performLoopClosure()函数流程：
1.先进行闭环检测detectLoopClosure()，如果返回true,则可能可以进行闭环，否则直接返回，程序结束。
2.接着使用icp迭代进行对齐。
3.对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数。否则进行迭代后的数据发布处理。
4.接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。
5.然后进行图优化过程。

RANSAC（Random Sample Consensus）是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。

performLoopClosure()函数代码：

void performLoopClosure(){

    if (cloudKeyPoses3D->points.empty() == true)
        return;

    if (potentialLoopFlag == false){

        if (detectLoopClosure() == true){
            potentialLoopFlag = true;
            timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
        }
        if (potentialLoopFlag == false)
            return;
    }

    potentialLoopFlag = false;

    pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setMaxCorrespondenceDistance(100);
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-6);
    icp.setEuclideanFitnessEpsilon(1e-6);
    // 设置RANSAC运行次数
    icp.setRANSACIterations(0);

    icp.setInputSource(latestSurfKeyFrameCloud);
    // 使用detectLoopClosure()函数中下采样刚刚更新nearHistorySurfKeyFrameCloudDS
    icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
    pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    // 进行icp点云对齐
    icp.align(*unused_result);

    // 为什么匹配分数高直接返回???分数高代表噪声太多
    if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
        return;

    // 以下在点云icp收敛并且噪声量在一定范围内上进行
    if (pubIcpKeyFrames.getNumSubscribers() != 0){
        pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
		// icp.getFinalTransformation()的返回值是Eigen::Matrix<Scalar, 4, 4>
        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);
    }   

    float x, y, z, roll, pitch, yaw;
    Eigen::Affine3f correctionCameraFrame;
    correctionCameraFrame = icp.getFinalTransformation();
	// 得到平移和旋转的角度
    pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
    Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
    Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
    Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
    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);

    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;
}

---

visualizeMapThread

visualizeGlobalMapThread()代码：

void visualizeGlobalMapThread(){
   
    ros::Rate rate(0.2);
    while (ros::ok()){
   
        rate.sleep();
        publishGlobalMap();
    }
}

publishGlobalMap()主要进行了3个步骤：
1.通过KDTree进行最近邻搜索;
2.通过搜索得到的索引放进队列;
3.通过两次下采样，减小数据量;

publishGlobalMap()代码：

void publishGlobalMap(){

    if (pubLaserCloudSurround.getNumSubscribers() == 0)
        return;

    if (cloudKeyPoses3D->points.empty() == true)
        return;

    std::vector<int> pointSearchIndGlobalMap;
    std::vector<float> pointSearchSqDisGlobalMap;

    mtx.lock();
    kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
    // 通过KDTree进行最近邻搜索
    kdtreeGlobalMap->radiusSearch(currentRobotPosPoint, globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
    mtx.unlock();

    for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i)
      globalMapKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);

    // 对globalMapKeyPoses进行下采样
    downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
    downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);

    for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i){
		int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
		*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
		*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
		*globalMapKeyFrames += *transformPointCloud(outlierCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
    }

    // 对globalMapKeyFrames进行下采样
    downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
    downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);

    sensor_msgs::PointCloud2 cloudMsgTemp;
    pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp);
    cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
    cloudMsgTemp.header.frame_id = "/camera_init";
    pubLaserCloudSurround.publish(cloudMsgTemp);  

    globalMapKeyPoses->clear();
    globalMapKeyPosesDS->clear();
    globalMapKeyFrames->clear();
    globalMapKeyFramesDS->clear();     
}

---

run

run()是mapOptimization类的一个成员变量
run()的运行流程：
1.判断是否有新的数据到来并且时间差值小于0.005；
2.如果timeLaserOdometry - timeLastProcessing >= mappingProcessInterval，则进行以下操作：
2.1.将坐标转移到世界坐标系下，得到可用于建图的Lidar坐标，即修改transformTobeMapped的值；
2.2.抽取周围的关键帧；
2.3.下采样当前scan；
2.4.当前scan进行图优化过程；
2.5.保存关键帧和因子；
2.6.校正位姿；
2.7.发布Tf；
2.8.发布关键位姿和帧数据；

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);

        if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
   

            timeLastProcessing = timeLaserOdometry;

            transformAssociateToMap();

            extractSurroundingKeyFrames();

            downsampleCurrentScan();

            scan2MapOptimization();

            saveKeyFramesAndFactor();

            correctPoses();

            publishTF();

            publishKeyPosesAndFrames(