(01)ORB-SLAM2源码无死角解析-(59) 闭环线程→闭环矫正: CorrectLoop→位姿传播，地图点矫正

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过上一篇前面两篇博客，对计算 Sim3 的理论以及源码进行了详细的分析，也就是说 src/LoopClosing.cc 文件中被LoopClosing::Run() 调用的三个函数：DetectLoop()，ComputeSim3()已经梳理完成，接下来要讲解的函数就是 CorrectLoop() →闭环矫正。该函数还是十分复杂的。主要的步骤如下：

$(1)：$ 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置(相连关键帧—当前帧)
$(2)：$将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧—闭环帧+相连关键帧）
$(3)：$通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph
$(4)：$对 Essential Graph(Pose Graph) 进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整
$(5)：$创建线程进行全局Bundle Adjustment

$提示：$如果对闭环矫正不了解的朋友，看了之后应该是比较蒙蔽的，不过没有关系，在接下来的过程中，本人会一点一点的进行详细的讲解。其讲解的代码为 src/LoopClosing.cc 中的 void LoopClosing::CorrectLoop() 函数。

$核心：$ 这里需要注意一个核心提示，前面博客通过 ComputeSim3() 函数，主要获得了两个变量：

mScw: 世界坐标系到相机坐标系(当前关键帧)的sim参数
mvpCurrentMatchedPoints: 经过SearchBySim3得到的已经匹配的点对(当前关键帧、闭环候选帧中有效的关键帧)

这里需要注意的一个点就是：$认为mScw是完全正确的，其后的分析围绕其展开$。因为这里已经解决了尺度漂移的问题，所以认为其是正确的。
 

二、预备工作

正式操作之前，需要做一些预备操作，主要流程如下：
$(1)：$ 请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
$(2)：$ 如果有全局BA在运行，终止掉，迎接新的全局BA
$(3)：$ 等待局部地图线程结束
$(4)：$ 请求局部地图线程停止之后，局部线程执行 LocalMapping::Run() 执行不到 SearchInNeighbors() 函数(内部会调用mpCurrentKeyFrame->UpdateConnections())，所以会导致创建了地图点，却没有更新当前帧的共视连接关系。所以我们还需要更新当前帧的共视连接关系。

代码注释如下：

void LoopClosing::CorrectLoop()
{
    cout << "Loop detected!" << endl;
    mpLocalMapper->RequestStop();

    if(isRunningGBA())
    {
        // 如果有全局BA在运行，终止掉，迎接新的全局BA
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;
        // 记录全局BA次数
        mnFullBAIdx++;
        if(mpThreadGBA)
        {
            // 停止全局BA线程
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

    // Wait until Local Mapping has effectively stopped
    // 一直等到局部地图线程结束再继续
    while(!mpLocalMapper->isStopped())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    // Ensure current keyframe is updated
    // Step 1：根据共视关系更新当前关键帧与其它关键帧之间的连接关系
    // 因为请求局部地图线程停止之后，局部线程执行 LocalMapping::Run() 执行不到 SearchInNeighbors() 函数(内部会调用mpCurrentKeyFrame->UpdateConnections())，所以会导致创建了地图点，却没有更新当前帧的共视连接关系。
    mpCurrentKF->UpdateConnections();

对于 UpdateConnections(); 函数在前面已经讲解过，所以这里就不再进行重复了。

 

三、矫正共视关键帧位置

上图是前面博客的图像，通过分析，已经知道如何求解出闭环候选帧(绿色原点)到当前关键帧(红色原点)的sim3变换。即 LoopClosing::ComputeSim3() 函数中的 Scm 或者 gScm，闭环候选帧(绿色原点)的位姿为 gSmw(尺度为1)已知。根据 gScm$\ast$gSmw; 世界坐标系到当前关键帧(红色原点)的Sm3变换。代码中的变量为 mg2oScw。

前面提到，我们认为 mg2oScw 是完全正确的。那么现在思考一下，当前关键帧的连接关键帧(黄色点)是不是没有当前关键帧(红色原点)的位姿更加准确一些。因为 g2oScw 是经过Sim3变换矫正过的。而关键帧的连接关键帧(黄色点)使用的还是带有尺度漂移的位姿。那么是否可以把它们的位姿也矫正一下呢？

当然是可以的，因为红色点与黄色点的间隔帧非常的少，可以认为红色点到黄色点是没有尺度漂移的，或者说尺度漂移可以忽略不计。也就是说，因为红色点的位姿(经过矫正→mg2oScw)已知，又已知红色点到黄色点的 Sim3变换(尺度为1)。那么则可以求得黄点经过 Sim3 矫正过后的位姿。简介的思路先看下图：

$注意上图绿色标号①,②,③,④：$首先其上的基准①=mg2oScw)认为是正确的，②=g2oSic(可求)→已知，③=g2oCorrectedSiw=g2oSic $\ast$ mg2oScw→可求。经过校准之后很明显③是优于④=g2oSiw的。代码实现的思路如下：

$(1)：$ 获得取出当前关键帧及其共视关键帧，称为“当前关键帧组”，也就是上图中蓝色圈内的所有点，绿色点部不包含在内，因为虽然根据共视关系更新了连接关系，但是在闭环检测以及计算 Sim3 的时候，都没有改变该关键帧的地图点。

$(2)：$ 创建变量 CorrectedSim3, NonCorrectedSim3; 分别用于存储矫正过以及没有矫正过的Sim3。把 mg2oScw 先存储于 CorrectedSim3 之中，因为认为其是准的，所以固定不动。同时获得 当前关键帧到世界坐标系下的变换矩阵 Twc。同时给地图点上锁，避免在操作是，地图点更新或者变动。然后对当前关键帧组进行遍历。

$(3)：$ 首先得到 当前关键帧mpCurrentKF 到其共视关键帧 pKFi 的相对变换 g2oSic。然后执行 g2oCorrectedSiw = g2oSic $\ast$ mg2oScw; 共视关键帧 pKFi 就获得了经过 Sim3矫正之后的位姿。同时存储于 CorrectedSim3 之中。并且计算 g2oSiw(不准确的)存储于 NonCorrectedSim3 之中。代码注释如下(紧接着前面的注释)：

   // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
    // Step 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点
    // 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，
    // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换

    // 取出当前关键帧及其共视关键帧，称为“当前关键帧组”
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);

    // CorrectedSim3：存放闭环g2o优化后当前关键帧的共视关键帧的世界坐标系下Sim3 变换
    // NonCorrectedSim3：存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim3 变换
    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    // 先将mpCurrentKF的Sim3变换存入，认为是准的，所以固定不动
    CorrectedSim3[mpCurrentKF]=mg2oScw;
    // 当前关键帧到世界坐标系下的变换矩阵
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

    // 对地图点操作
    {
        // Get Map Mutex
        // 锁定地图点
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

        // Step 2.1：通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿
        // 遍历"当前关键帧组""
        for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKFi = *vit;
            cv::Mat Tiw = pKFi->GetPose();
            if(pKFi!=mpCurrentKF)      //跳过当前关键帧，因为当前关键帧的位姿已经在前面优化过了，在这里是参考基准
            {
                // 得到当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的相对变换
                cv::Mat Tic = Tiw*Twc;
                cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
                cv::Mat tic = Tic.rowRange(0,3).col(3);

                // g2oSic：当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
                // 这里是non-correct, 所以scale=1.0
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
                // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
                g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
                // Pose corrected with the Sim3 of the loop closure
                // 存放闭环g2o优化后当前关键帧的共视关键帧的Sim3 位姿
                CorrectedSim3[pKFi]=g2oCorrectedSiw;
            }

            cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
            cv::Mat tiw = Tiw.rowRange(0,3).col(3);
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
            // Pose without correction
            // 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
            NonCorrectedSim3[pKFi]=g2oSiw;
        }

 

四、矫正共视关键帧地图点

前面已经对共视关键帧的位姿进行了矫正，那么接下来就是要对其地图点进行矫正。首先查看下图：

我们先思考一个问题，如果要对地图点进行纠正，那么我们首先要找到地图点。比如上图的 pKFi 的对应的地图点(红色五角星)。获取这个地图点还是比较简单的，调用 vpMPsi = pKFi->GetMapPointMatches() 即可。该些地图点，就是 pKFi 特征点能匹配得上的地图点。那么问题出来了，这些地图点都是在尺度为 1的世界坐标系下，简单的说，地图点的位姿是不太准确的。那么应该怎么去矫正呢？

大致的思路是这样的，首先求得地图点在相机坐标系下的位置(利用①=g2oSiw)。然后再把地图点重新变换到世界坐标系下，但是注意，这里使用的是 ②=g2oCorrectedSwi，也就是经过矫正后的 Sim3 变换。下面就来看看代码逻辑是如何实现的吧。

$(1)：$ 循环遍历经过矫正的当前帧的共视关键帧pKFi，即 CorrectedSim3。获得共视关键帧的 g2oCorrectedSiw ，再前面已经求得。取逆则得到 g2oCorrectedSwi。然后再获得 g2oSiw(s=1)。

$(2)：$ 获得共视关键帧 pKFi 的所有地图点赋值给 vpMPsi，然后进行遍历，如果地图点无效则跳过，否则先进行标记，防止重复矫正。然后把地图点进行 world →g2oSiw→ i →g2oCorrectedSwi→ world 操作。
完成之后更新平均观测方向以及观测距离范围。代码注释如下：

$(3)：$ 对地图点进行矫正之后，还需要更新 pKFi(不包括当前关键帧) 的旋转矩阵 $R$ 以及平移向量。根据前面的博客：(01)ORB-SLAM2源码无死角解析-(56) 闭环线程→计算Sim3:理论推导(1) 求解s,t 中的(16)式可以得知:
$$\mathbf{t}=\bar{Q}-s\mathbf{R}\bar{P}\text{\hspace{1em}(01)}$$计算出来的平移 $\mathbf{t}$ 是与尺度有关的，所以还需要对 $\mathbf{t}$ 进行归一化处理。pKFi->SetPose(correctedTiw) 只设置尺度为 1 的旋转矩阵与平移向量。然后 pKFi 还需要根据共视关系更新当前帧与其它关键帧之间。
 

        // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
        // Step 2.2：得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点
        // 遍历待矫正的共视关键帧（不包括当前关键帧）
        for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
        {
            // 取出当前关键帧连接关键帧
            KeyFrame* pKFi = mit->first;
            // 取出经过位姿传播后的Sim3变换
            g2o::Sim3 g2oCorrectedSiw = mit->second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
            // 取出未经过位姿传播的Sim3变换
            g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];

            vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
            // 遍历待矫正共视关键帧中的每一个地图点
            for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
            {
                MapPoint* pMPi = vpMPsi[iMP];
                // 跳过无效的地图点
                if(!pMPi)
                    continue;
                if(pMPi->isBad())
                    continue;
                // 标记，防止重复矫正
                if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) 
                    continue;

                // 矫正过程本质上也是基于当前关键帧的优化后的位姿展开的
                // Project with non-corrected pose and project back with corrected pose
                // 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系，然后再反映射到校正后的世界坐标系下
                cv::Mat P3Dw = pMPi->GetWorldPos();
                // 地图点世界坐标系下坐标
                Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
                // map(P) 内部做了相似变换 s*R*P +t  
                // 下面变换是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world
                Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                // 记录矫正该地图点的关键帧id，防止重复
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                // 记录该地图点所在的关键帧id
                pMPi->mnCorrectedReference = pKFi->mnId;
                // 因为地图点更新了，需要更新其平均观测方向以及观测距离范围
                pMPi->UpdateNormalAndDepth();
            }
            
            // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
            // Step 2.3：将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
            // 其实是现在已经有了更新后的关键帧组中关键帧的位姿,但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中,还没有写回到关键帧对象中
            // 调用toRotationMatrix 可以自动归一化旋转矩阵
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); 
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                  
            double s = g2oCorrectedSiw.scale();
            // 平移向量中包含有尺度信息，还需要用尺度归一化
            eigt *=(1./s); 

            cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
            // 设置矫正后的新的pose
            pKFi->SetPose(correctedTiw);

            // Make sure connections are updated
            // Step 2.4：根据共视关系更新当前帧与其它关键帧之间的连接
            // 地图点的位置改变了,可能会引起共视关系\权值的改变 
            pKFi->UpdateConnections();
        }

另外map函数为：

     Vector3d map (const Vector3d& xyz) const {
      return s*(r*xyz) + t;

 

五、结语

通过该篇博客，我们了解了 闭环线程中的位姿传播以及地图点矫正，但是对于 LoopClosing::CorrectLoop() 函数，还没有分析完成，下面博客继续进行讲解。
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件