LeGO-LOAM回环检测

1. 总体框架

mapOptimization.cpp中的主函数中，闭环检测是一个单独线程运行的。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;
}

2. 回环检测

在LeGO-LOAM中使用了简单的基于轨迹位姿方法的回环检测，论文假设采用了地图优化方法的位姿优化结果偏移小，因此可以这样做。但是实际上无法应对大距离范围下的尺度漂移，存在着数据同源的问题。

2.1检测回环

回环检测运行频率是1Hz。调用的 performLoopClosure函数中首先调用了detectLoopClosure函数。

    void loopClosureThread()
    {
        if (loopClosureEnableFlag == false)            
            return;
        ros::Rate rate(1);        
        while (ros::ok()){            
            rate.sleep();            
            performLoopClosure();        
        }    
     }

detectLoopClosure函数的流程：
1.使用KDtree寻找当前位姿的历史最近位姿：radiusSearch。当找到的位姿满足时间上与当前帧间隔大于30的条件就认为是合格的最近位置，并保留下来。
2.储存最新的这一帧点云，以作为闭环检测匹配计算中的目标值，并转换到世界坐标系下。
3.储存被找到的那一帧历史点云及其附近的几帧点云，并把他们转换到世界坐标系中。
4.如果订阅者数量不是0，则把数据发布出去。

    bool detectLoopClosure(){

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

        std::lock_guard<std::mutex> lock(mtx);
        // 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;
    }

2.2 添加优化

performLoopClosure函数是回环过程的主体函数。函数的流程如下：
1.调用detect函数，检测出历史位置上的回环，并储存好源和目标点云及位置数据。

    void performLoopClosure(){

        if (cloudKeyPoses3D->points.empty() == true)
            return;
        // try to find close key frame if there are any
        if (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;

2.使用PCL函数库的ICP算法对源和目标进行匹配计算。设计最大的相关性距离是100，最大迭代次数为100。求解出源和目标之间的相对位姿。

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

3.使用gtsam库，构建约束项，在isam中添加约束。
按照凸优化的概念来讲，一个约束的顶点分别是：
$T_1=T_{icp}\ast T_k$；$T_k$ 是当前位姿; $T_{icp}$ 是icp计算出来的当前帧与回环检测帧之间的相对位姿，
$T_2=T_{\text{histrorynearst}}$

        /*
        	get pose constraint
        	*/
        float x, y, z, roll, pitch, yaw;
        Eigen::Affine3f correctionCameraFrame;//仿射矩阵（包括平移旋转放缩剪切等）是一个包含旋转和平移的4*4矩阵
        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（）
        isam->update();
        gtSAMgraph.resize(0);

        aLoopIsClosed = true;
    }

isam->update添加新的因子，更新整个解决方案，并重新线性化。
总结gtsam在这一部分中的使用：
NonlinearFactorGraph::add() //添加因子
ISAM2::update() //更新结构，向解决方案中添加因子