2020/8/5 LeGOLOAM代码阅读(3)——mapOptmization.cpp

最新推荐文章于 2024-08-08 07:55:04 发布

shuang_yu_

最新推荐文章于 2024-08-08 07:55:04 发布

阅读量1.2k

点赞数

本文链接：https://blog.csdn.net/shuang_yu_/article/details/107826532

版权

简述

mapOptmization.cpp 作为LeGO-LOAM的全局姿态优化部分，主要采用了两步优化方法——a. scan-to-model的地图优化；b. 因子图优化，分别由 mapOptimization::scan2MapOptimization() 和 mapOptimization::saveKeyFramesAndFactor() 两个函数承担。

下图图示了每次优化后，位姿变量的变化：

float transformLast[6];  // 上一关键帧 经过因子图优化后的位姿，用于添加因子图节点。
float transformSum[6];   // 里程计的六自由度位姿(p y r x y z)
float transformIncre[6]; // 两次位姿优化时，里程计的相对位姿变化，
float transformTobeMapped[6]; // 机器人的位姿,全局姿态优化的目标变量：位姿优化的初始值->在地图优化过程中得到更新->经过因子图优化后的位姿
float transformBefMapped[6]; //  未经过scan-to-model优化的位姿，即里程计位姿。 
float transformAftMapped[6]; //  由经过scan-to-model优化后的位姿赋值,并在因子图优化后进行修正。

在这里插入图片描述

变量注释

class mapOptimization{
private:
    // 因子图优化相关变量 
    NonlinearFactorGraph gtSAMgraph; 
    Values initialEstimate;
    Values optimizedEstimate;
    ISAM2 *isam;
    Values isamCurrentEstimate;
    noiseModel::Diagonal::shared_ptr priorNoise;
    noiseModel::Diagonal::shared_ptr odometryNoise;
    noiseModel::Diagonal::shared_ptr constraintNoise;
    // ROS句柄
    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;
    // 为了信息发布而定义的msg
    nav_msgs::Odometry odomAftMapped;
    tf::StampedTransform aftMappedTrans;
    tf::TransformBroadcaster tfBroadcaster;
    
    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;  //   关键帧的角点点云集合
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;    //   关键帧的平面点点云集合 
    vector<pcl::PointCloud<PointType>::Ptr> outlierCloudKeyFrames; //   关键帧的离群点点云集合
    deque<pcl::PointCloud<PointType>::Ptr> recentCornerCloudKeyFrames; //
    deque<pcl::PointCloud<PointType>::Ptr> recentSurfCloudKeyFrames;    // 
    deque<pcl::PointCloud<PointType>::Ptr> recentOutlierCloudKeyFrames; // 
    int latestFrameID;
    vector<int> surroundingExistingKeyPosesID;  //  局部关键帧集合(ID集合),(随着最新关键帧的改变而逐步增减的)
    deque<pcl::PointCloud<PointType>::Ptr> surroundingCornerCloudKeyFrames; //局部关键帧角点点云集合
    deque<pcl::PointCloud<PointType>::Ptr> surroundingSurfCloudKeyFrames;    // 局部关键帧平面点点云集合
    deque<pcl::PointCloud<PointType>::Ptr> surroundingOutlierCloudKeyFrames;  // 局部关键帧离群点点云集合
    PointType previousRobotPosPoint;  // 上一帧关键帧位姿点
    PointType currentRobotPosPoint;  //  使用经过 scan-to-model 优化的位姿创建当前位置点
    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;  // 所有关键帧三自由度位置集合 (x,y,z表示位置,i表示位姿点在集合中的索引)（这是经过因子图优化后的位置）
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;  //关键帧六自由度位姿集合 (x,y,z表示位置,i表示索引,rpy表示姿态,time记录里程计时间)（这是经过因子图优化后的位置）
    pcl::PointCloud<PointType>::Ptr surroundingKeyPoses; //  局部关键帧位置集合——目标点附近的位置点集合。 
    pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS; // 上者的降采样结果 
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // 通过Msg接收到的Less角点点云 
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // 通过Msg接收到的Less平面点点云 
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; //  Less角点点云的降采样 
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; //    Less平面点点云的降采样 
    pcl::PointCloud<PointType>::Ptr laserCloudOutlierLast; // 通过Msg接收到的离群点点云 
    pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDS; // 离群点点云的降采样 
    pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLast; // Less平面点和离群点的点云 
    pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLastDS; // 上者的降采样 
    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap; // 局部角点点云地图  (每轮进行清空)
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;   // 局部平面点点云地图  (每轮进行清空)
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;   // 局部角点点云的降采样  (每轮进行清空)
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;    // 局部角点点云的降采样  (每轮进行清空) 
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;  // 局部角点点云地图 的KD树 
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;    // 局部平面点点云地图 的KD树
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;  //所有关键帧位置点集合的KD树
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;    // 
    pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloudDS;
    pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloudDS;
    pcl::PointCloud<PointType>::Ptr latestCornerKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloudDS;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap;
    pcl::PointCloud<PointType>::Ptr globalMapKeyPoses;
    pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS;
    pcl::PointCloud<PointType>::Ptr globalMapKeyFrames;
    pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS;
    std::vector<int> pointSearchInd;
    std::vector<float> pointSearchSqDis;
    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterOutlier;
    pcl::VoxelGrid<PointType> downSizeFilterHistoryKeyFrames;
    pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; 
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;

    // 信息的时间戳 
    double timeLaserCloudCornerLast;   // Less角点的时间
    double timeLaserCloudSurfLast;     // Less平面点的时间 
    double timeLaserOdometry;          // 里程计时间
    double timeLaserCloudOutlierLast;   // 离群点点云 的时间

    double timeLastGloalMapPublish;    

    // 信息接收的标志位 
    bool newLaserCloudCornerLast;   //Less角点的标志位
    bool newLaserCloudSurfLast;     //Less平面点的标志位
    bool newLaserOdometry;          //里程计信息的标志位
    bool newLaserCloudOutlierLast;  //离群点点云的标志位

    float transformLast[6];  // 上一关键帧 经过因子图优化后的位姿
    float transformSum[6];   // 里程计的六自由度位姿(p y r x y z)
    float transformIncre[6]; // 两次位姿优化时，里程计的相对位姿变化
    float transformTobeMapped[6]; // 机器人的位姿,全局姿态优化的目标变量：位姿优化的初始值->在地图优化过程中得到更新->经过因子图优化后的位姿
    float transformBefMapped[6]; //  未经过scan-to-model优化的位姿，即里程计位姿。 
    float transformAftMapped[6]; //  由经过scan-to-model优化后的位姿赋值,并在因子图优化后进行修正。
    int imuPointerFront; //与里程计时间对准的IMU数据索引 
    int imuPointerLast;  //最新的IMU数据索引 
    // IMU时间、数据队列
    double imuTime[imuQueLength];
    float imuRoll[imuQueLength];
    float imuPitch[imuQueLength];
    std::mutex mtx;
    double timeLastProcessing; // 上一次进行全局位姿优化的时间
    PointType pointOri, pointSel, pointProj, coeff;
    cv::Mat matA0; //存放距离平面点最近的五个点(5×3矩阵)
    cv::Mat matB0; //5*1 的矩阵 
    cv::Mat matX0; //3*1 的矩阵
    cv::Mat matA1; //3×3 的协方差矩阵
    cv::Mat matD1; //1*3 的特征值向量
    cv::Mat matV1;// 3个 特征向量
    bool isDegenerate;
    cv::Mat matP;
    // 一些点云的数量
    int laserCloudCornerFromMapDSNum;  // 局部角点点云中点的数量(经过降采样)
    int laserCloudSurfFromMapDSNum;    // 局部平面点点云中点的数量(经过降采样)
    int laserCloudCornerLastDSNum;   
    int laserCloudSurfLastDSNum;
    int laserCloudOutlierLastDSNum;
    int laserCloudSurfTotalLastDSNum;
    bool potentialLoopFlag;
    double timeSaveFirstCurrentScanForLoopClosure;
    int closestHistoryFrameID;
    int latestFrameIDLoopCloure;
    bool aLoopIsClosed;
    float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ; // 点云相对于世界坐标系的关系
    float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ;//用来记录变换的三角函数值
}

msg的订阅

class mapOptimization{
private:
    ros::Subscriber subLaserCloudCornerLast;
    ros::Subscriber subLaserCloudSurfLast;
    ros::Subscriber subOutlierCloudLast;
    ros::Subscriber subLaserOdometry;
    ros::Subscriber subImu;
    ...
public:
	//构造函数
	mapOptimization():nh("~"){
	//五个subscribe的定义
		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);
        ...
	}

    // 三个点云主题("/laser_cloud_corner_last","/laser_cloud_surf_last","/outlier_cloud_last")的回调函数,
    // 分别接收当前帧的 Less角点点云、Less平面点点云、离群点点云
    // 并记录时间
    // timeLaserCloudCornerLast/timeLaserCloudSurfLast/timeLaserCloudOutlierLast
    // 记录点云 
    // laserCloudCornerLast/laserCloudSurfLast/laserCloudOutlierLast 
    // 标志位置true
    // newLaserCloudCornerLast/newLaserCloudSurfLast/newLaserCloudOutlierLast 
	void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        ...
	}
	void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        ...
    }
	void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
		...
    }

	//主题 "/laser_odom_to_init" 的回调函数,接收里程计信息,记录: 
	//     timeLaserOdometry 里程计时间 
    //     transformSum[] 记录六自由度位姿,即里程计位姿
    //     newLaserOdometry 标志位置1
    void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){
    	...
	}
	
    // 主题 imuTopic 的回调函数,接收IMU数据信息,并记录在IMU数据队列中:
    // imuTime  时间队列 
    // imuRoll、imuPitch RPY队列(另外一个姿态值没有用到，所以就只记录了其中2个值)
    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){
    	...
    }

main函数与 mapOptimization.run()

//主函数,
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;
}
class mapOptimization{
private:
	...
public:
	...
	    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)
        {
            //标志位重新置回false 
            newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;

            // 创建对象 lock 对mtx进行上锁,当lock被析构时,自动解锁
            std::lock_guard<std::mutex> lock(mtx);

            // 全局姿态优化的时间判断条件：距离上次优化已经过去0.3s
            if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
                timeLastProcessing = timeLaserOdometry; 

                transformAssociateToMap(); // 
                // 根据当前和上一次全局姿态优化时的里程计 transformSum transformBefMapped 
                // 以及上一次全局姿态优化的结果 transformAftMapped  
                // 计算当前姿态优化的初始值，赋值给 transformTobeMapped 

                extractSurroundingKeyFrames();
                // 函数功能：根据当前位置，提取局部关键帧集合,以及对应的三个关键帧点云集合
                // 步骤：
                //     1. 在 关键帧位置集合cloudKeyPoses3D 中  
                //     检索 当前位置currentRobotPosPoint 附近的姿态点
                //     获得局部位置点，赋值给 局部位置点集合surroundingKeyPoses 
                //     2. 根据 局部位置点集合surroundingKeyPoses 更新
                //     局部关键帧集合 surroundingExistingKeyPosesID
                //     局部关键帧 角点点云集合surroundingCornerCloudKeyFrames 
                //     局部关键帧 平面点点云集合surroundingSurfCloudKeyFrames
                //     局部关键帧 离群点点云集合surroundingOutlierCloudKeyFrames
                //     增加新进入局部的关键帧、并删除离开局部的关键帧。
                //     3. 为局部点云地图赋值 
                //     laserCloudCornerFromMap     所有局部关键帧的角点集合
                //     laserCloudSurfFromMap       所有局部关键帧平面点和离群点的几何

                downsampleCurrentScan();
                // 对来自msg的点云进行降采样 
                // laserCloudCornerLast   //角点点云
                // laserCloudSurfLast     // 平面点点云
                // laserCloudOutlierLast  // 离群点点云
                // laserCloudSurfTotalLast // 平面点+离群点


                //使用 scan-to-model 的策略优化了 变换 transformTobeMapped 
                scan2MapOptimization();
                // 使用scan-to-model位姿优化，获得当前时间点机器人的位姿 transformTobeMapped
                // 参考IMU的姿态 对 transformTobeMapped 进行中值滤波,获得最终的机器人位姿
                // 为 transformBefMapped 赋值为 里程计位姿，即scan-to-model优化前的位姿 
                // 为 transformAftMapped 赋值为 transformTobeMapped 即scan-to-model优化后的位姿 

                saveKeyFramesAndFactor();
                // 1选定关键帧 
                // 2根据新的关键帧更新因子图
                // 3经过因子图优化后，更新当前位姿 
                // transformAftMapped   
                // transformTobeMapped  
                // 并用因子图优化后的位姿 创建关键帧位置点和位姿点,添加到集合 
                // cloudKeyPoses3D  关键帧位置点集合 
                // cloudKeyPoses6D  关键帧位姿点集合
                // 并为 transformLast 赋值 
                // 更新 
                // cornerCloudKeyFrames 关键帧角点点云集合
                // surfCloudKeyFrames   关键帧平面点点云集合
                // outlierCloudKeyFrames关键帧离群点点云集合 

                //闭环处理矫正、暂不考虑
                correctPoses();


                publishTF();
                // 主题"/aft_mapped_to_init"发布优化后的位姿信息 
                // msg.header.stamp 取的是 里程计时间信息 
                // msg.pose 存储本坐标系位姿"/camera_init"  经过全局位姿优化的姿态
                // msg.twist 存储子坐标系位姿"/aft_mapped"  未经过全局优化的姿态,即里程计信息 

                // 发布tf变换
                // msg.frame_id_ = "/camera_init"
                // msg.child_frame_id_ = "/aft_mapped";
                // msg 的姿态变换为 transformAftMapped 即优化后的机器人位姿距离原点的位姿变换 

                publishKeyPosesAndFrames();
                // 主题 "/key_pose_origin" 发布关键帧位姿点集合 
                // 主题 "/recent_cloud" 发布 局部平面点点云 
                // 主题 "/registered_cloud" 发布 当前帧点云 
                clearCloud();
            }
        }
    }
};