lego-loam mapOptmization 源码注释（一）

我们先看主函数：

一、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 loopthread(&mapOptimization::loopClosureThread, &MO);
    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;
}

我估计能看到这的朋友，对lego-loam已经很熟悉了，这个main函数，总体上和featureAssociation的main函数一模一样，只是多了四行，我们着重看多出来部分的作用：

std::thread loopthread(&mapOptimization::loopClosureThread, &MO);

//这行代码创建了一个名为loopthread的线程对象，它将执行mapOptimization类的loopClosureThread成员函数。&MO是传递给线程的参数，意味着loopClosureThread函数将在MO这个mapOptimization类的实例上被调用。

std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);

//这行代码创建了另一个名为visualizeMapThread的线程对象，它将执行mapOptimization类的visualizeGlobalMapThread成员函数，同样传递MO作为参数。

loopthread.join();

//这行代码会阻塞当前线程（通常是主线程），直到loopthread线程完成执行。join函数用于等待线程结束，确保在程序继续执行之前，该线程的所有任务都已完成。

visualizeMapThread.join();

//这行代码同样会阻塞当前线程，直到visualizeMapThread线程完成执行。

这两个线程的作用是并行处理地图优化中的两个重要任务，以提高SLAM系统的效率和准确性。闭环检测线程负责提高地图的准确性，而全局地图可视化线程则提供了地图的可视化展示。

这两个线程是独立于主线程的。在C++中，使用std::thread创建的线程是并发执行的，这意味着它们可以同时运行，而不受主线程的直接控制。每个线程都有自己的执行路径和执行栈。 

主线程则负责处理ROS回调、调用MO.run()执行地图优化的主要逻辑，并通过ros::spinOnce()与ROS通信。主线程通过调用join()方法等待这两个线程完成，确保在程序退出前，所有线程都已经正确地结束了它们的任务。

我的想法是先看主线程，然后再看回环检测，最后看可视化的内容。

先来看mapOptimization类！！

mapOptimization

补充一下：之前一直没看到gtsam在lego-loam的作用，看来应该是在地图优化上发挥作用了。

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>

 下面我们来看mapOptimization的私有变量定义，不详细解释，筛一些有意思的说一下。

private:

class mapOptimization{

private:

    NonlinearFactorGraph gtSAMgraph;

//这是一个非线性因子图，用于表示非线性最小二乘问题中的约束。
在SLAM中，这些约束通常来自于传感器测量，如激光雷达、雷达或视觉里程计数据。
每个因子代表一个测量，而每个节点代表一个状态变量（如机器人的位置和方向）。

    Values initialEstimate;

//Values是一个键值对集合，用于存储变量的初始估计值。
在SLAM中，这些初始估计可能来自于先验知识、传感器校准或其他来源。
这些值作为优化算法的起点。

    Values optimizedEstimate;

//这是一个Values对象，用于存储优化后的估计值。
在执行优化算法后，这个对象将包含更新的状态变量值，这些值最小化了因子图中所有因子的误差。

    ISAM2 *isam;

//ISAM2是gtSAM库中的一个增量状态调整和映射（Incremental Smoothing and Mapping）类的实例。
这个类用于在线更新状态估计，同时处理新的测量数据和先前的估计。
ISAM2是一个高效的算法，可以处理大规模的非线性优化问题。

    Values isamCurrentEstimate;

//这是一个Values对象，用于存储ISAM2当前的状态估计。
随着新的测量数据不断到来，ISAM2会更新其内部的状态估计，这个对象就用来存储这些最新的估计值。
!!具体的用法要在代码中去看，这里只是有个印象！！

    noiseModel::Diagonal::shared_ptr priorNoise;

//这是一个指向noiseModel::Diagonal对象的智能指针，用于表示先验噪声模型。
在SLAM中，先验噪声模型通常用于表示对初始状态估计的不确定性。
例如，在机器人的初始位置和方向估计中，先验噪声模型可以表示为位置和方向的不确定性。

    noiseModel::Diagonal::shared_ptr odometryNoise;

//这是一个指向noiseModel::Diagonal对象的智能指针，用于表示里程计噪声模型。
里程计噪声模型用于表示机器人运动测量（如轮式里程计或视觉里程计）的不确定性。
这些测量通常包含噪声，需要通过噪声模型来量化。

    noiseModel::Diagonal::shared_ptr constraintNoise;

//这是一个指向noiseModel::Diagonal对象的智能指针，用于表示约束噪声模型。
在SLAM中，约束通常来自于传感器测量，如激光雷达、雷达或视觉里程计数据。
约束噪声模型用于表示这些测量的不确定性。

    ros::NodeHandle nh;

    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubOdomAftMapped;
    ros::Publisher pubKeyPoses;

    ros::Publisher pubHistoryKeyFrames;
    ros::Publisher pubIcpKeyFrames;
    ros::Publisher pubRecentKeyFrames;
    ros::Publisher pubRegisteredCloud;

    ros::Subscriber subLaserCloudCornerLast;
    ros::Subscriber subLaserCloudSurfLast;
    ros::Subscriber subOutlierCloudLast;
    ros::Subscriber subLaserOdometry;
    ros::Subscriber subImu;

    nav_msgs::Odometry odomAftMapped;
    tf::StampedTransform aftMappedTrans;
    tf::TransformBroadcaster tfBroadcaster;

    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
    vector<