lego-loam mapOptmization 源码注释（三）

于 2024-11-05 22:55:42 发布
阅读量845 收藏 10
点赞数 18
分类专栏： lego-loam 文章标签： slam ros 人工智能 算法 自动驾驶
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/Joeybee/article/details/143526524
版权 
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
loopthread.join();

 我们先来看和主线程并列的回环检测线程，然后再来看correctPoses()函数。

loopClosureThread

void loopClosureThread(){
        if (loopClosureEnableFlag == false)
            return;
//默认loopClosureEnableFlag == false，在lego-loam的源码
//中是默认不使用回环检测的！！

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

 performLoopClosure();

void performLoopClosure(){

        if (cloudKeyPoses3D->points.empty() == true)
            return;
        // try to find close key frame if there are any
        if (potentialLoopFlag == false){

//potentialLoopFlag = false;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;
    }

 看一下icp的参数设置：

 // ICP Settings
        pcl::IterativeClosestPoint<PointType, PointType> icp;
        icp.setMaxCorrespondenceDistance(100);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon(1e-6);
//设定变换阈值为1e-6，表示ICP收敛条件之一：若两次变换矩阵之间的差距小于该阈值，则认为已经收敛，提前终止迭代。
        icp.setEuclideanFitnessEpsilon(1e-6);
        icp.setRANSACIterations(0);
//设定RANSAC的迭代次数为0，意味着不使用RANSAC去除异常点对。 
       // Align clouds
        icp.setInputSource(latestSurfKeyFrameCloud);
        icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
        icp.align(*unused_result);

调用 icp.align() 执行ICP配准。配准结果被存储在 unused_result 中，但实际代码中未使用这个结果，因为我们只需要ICP计算的变换矩阵。

/*
        	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);
//BetweenFactor<Pose3> 用于添加回环约束，将最新关键帧与历史关键帧的相对位姿添加到因子图 gtSAMgraph 中。
//调用 isam->update(gtSAMgraph) 更新图优化，修正回环误差。
//清空 gtSAMgraph 并标记 aLoopIsClosed = true，表明回环已处理。
        aLoopIsClosed = true;
    }

 这段代码在检测到回环后，将相应的约束添加到因子图中，以更新地图的位姿优化。

Eigen::Affine3f 是 Eigen 库中的一种 3D 仿射变换类型，专门用于表示三维空间中的旋转和平移。Affine3f 是基于浮点数（float）精度的变换矩阵，通常用在需要表示刚体变换的场合。

icp.getFinalTransformation() 得到相机坐标系下的ICP变换矩阵，并通过 pcl::getTranslationAndEulerAngles 提取平移 (x, y, z) 和旋转角度 (roll, pitch, yaw)。

Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);

将变换矩阵从相机坐标系转换到雷达坐标系。通过 pcl::getTransformation(z, x, y, yaw, roll, pitch) 重新构建 correctionLidarFrame，使其适用于雷达坐标系。

  •  
    gtsam::Vector Vector6(6); float noiseScore = icp.getFitnessScore(); Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore; constraintNoise = noiseModel::Diagonal::Variances(Vector6); 
     
    ICP得分 noiseScore 用于创建噪声模型，代表了配准误差，数值越小表示配准越好。通过 noiseModel::Diagonal::Variances 设置每个自由度的噪声值。
     
  •  
     
 

detectLoopClosure()
 

bool detectLoopClosure(){

        latestSurfKeyFrameCloud->clear();
        nearHistorySurfKeyFrameCloud->clear();
        nearHistorySurfKeyFrameCloudDS->clear();

        std::lock_guard<std::mutex> lock(mtx);

//std::lock_guard 是 C++11 引入的一种锁管理工具，它在作用域内自动锁定给定的互斥锁对象，
//并在作用域结束时自动解锁。这种方式可以确保即使在发生异常时，互斥锁也会自动释放，避免死锁。

        // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
        kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

        closestHistoryFrameID = -1;
        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;
    }
 

 kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
 

//cloudKeyPoses3D中记录了过去所有关键帧的位置。
 

kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
 

//这里是用关键帧的位置建树，然后用当前关键帧的位置去找距离最近的关键帧。
 //currentRobotPosPoint.x = transformAftMapped[3];
 //currentRobotPosPoint.y = transformAftMapped[4];
 //currentRobotPosPoint.z = transformAftMapped[5];
//extern const float historyKeyframeSearchRadius = 7.0;
 // key frame that is within n meters from current pose will be considerd for loop closure
 //extern const int   historyKeyframeSearchNum = 25;
 // 2n+1 number of hostory key frames will be fused into a submap for loop closure
//其实一直有个疑问，我自以为能说通，不知道是不是对的，transformAftMapped是上一帧的位姿，在优化时总是比当前位姿慢一拍，当前的位姿实际上是transformTobeMapped，慢一拍是为了优化，同时主线程快一帧，也保证了回环和可视化都能正常运行。
 

        closestHistoryFrameID = -1;
        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;
        }
 

 这一段代码的目的是寻找一个时间上足够远的历史帧（与当前帧的时间差超过 30 秒），并将其 ID 存储在 closestHistoryFrameID 中。这种检查通常用于回环检测中，以避免选择过于相近的帧，确保回环检测在空间上更具有参考价值。
 

 // save latest key frames
        latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
        *latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
        *latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],   &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
 

这段代码的作用是将两个关键帧点云 (cornerCloudKeyFrames 和 surfCloudKeyFrames) 变换到一个新的位姿，然后将它们合并到 latestSurfKeyFrameCloud 中。这种操作通常在回环检测和位姿图优化中使用，以便重新评估点云的配准和位置一致性。 
 

// 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]);
        }

 

 在closestHistoryFrameID 附近选取多个关键帧，以形成一组历史点云。
 

首先选择closestHistoryFrameID最重要的条件是时间条件，不是距离。
 

其次我们在closestHistoryFrameID前后historyKeyframeSearchNum找历史点云。
 

historyKeyframeSearchNum==7，也就是从closestHistoryFrameID前后共14个点云里找，
 

条件closestHistoryFrameID + j >= 0 and closestHistoryFrameID + j <= latestFrameIDLoopCloure
 

我觉得这个条件不够好，尤其在没有闭环的时候(比如上楼梯，从第一层走到第二层，距离近，时间远，但是不是一个闭环)，这个条件也可以满足，那后面icp得到的变换就意义不明了，我感觉应该把条件再严格一些。 
 

 transformPointCloud
 

pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn){

        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

        PointType *pointFrom;
        PointType pointTo;

        int cloudSize = cloudIn->points.size();
        cloudOut->resize(cloudSize);
        
        for (int i = 0; i < cloudSize; ++i){

            pointFrom = &cloudIn->points[i];
            float x1 = cos(transformIn->yaw) * pointFrom->x - sin(transformIn->yaw) * pointFrom->y;
            float y1 = sin(transformIn->yaw) * pointFrom->x + cos(transformIn->yaw)* pointFrom->y;
            float z1 = pointFrom->z;

            float x2 = x1;
            float y2 = cos(transformIn->roll) * y1 - sin(transformIn->roll) * z1;
            float z2 = sin(transformIn->roll) * y1 + cos(transformIn->roll)* z1;

            pointTo.x = cos(transformIn->pitch) * x2 + sin(transformIn->pitch) * z2 + transformIn->x;
            pointTo.y = y2 + transformIn->y;
            pointTo.z = -sin(transformIn->pitch) * x2 + cos(transformIn->pitch) * z2 + transformIn->z;
            pointTo.intensity = pointFrom->intensity;

            cloudOut->points[i] = pointTo;
        }
        return cloudOut;
    }
 

 这段代码将当前帧上一帧的点云转换到地图坐标系下。
 

 

 
 


                

        

    

  



    



            

  



          




        



    





    



      

      

        
      

      





	

		评论	
	

	
	




  

    

      添加红包
      
    

    

      

        
        

          
          
        

        
请填写红包祝福语或标题

      

      

        
        

          
          个
        

        
红包个数最小为10个

      

      

        
        

          
          元
        

        
红包金额最低5元

      

      

        
        

          当前余额3.43元
          前往充值 >
        

      

      

        

          需支付：10.00元
        

        
        
      

    

  





  

    
    
  

  

    
    
  








  

    

      

        

          

            
            
成就一亿技术人!

          

          

        

        

          

          

            领取后你会自动成为博主和红包主的粉丝
            规则
          

        

      

      

        

          

            
              
            
            

              
hope_wisdom
 发出的红包
            

          

        

        

          

          

          

        

      

    

    

  



      
      

      
实付元

      
使用余额支付

      

        

          

            
            点击重新获取
          

        

        
扫码支付

      

      

        
          
            
          
          
        
      

      

        
        钱包余额
          0
          

            
            

              

                
抵扣说明：

                
 1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
 2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

              

            

          

      

      余额充值