(01)ORB-SLAM2源码无死角解析-(60) 闭环线程→闭环矫正: CorrectLoop→地图点融合、共视关系更新

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

这里需要回顾一个前面的知识点，那就是 mvpCurrentMatchedPoints 这个变量。mvpCurrentMatchedPoints 记录的是经过 SearchBySim3 得到的已经匹配的点对(当前关键帧、闭环候选帧中有效的关键帧)。这里的匹配点我们认为是正确的，因为其是使用Sim3进行匹配的。另外再上一篇博客中，对地图点更新是不包含当前关键帧的，所以还需要对当前关键帧地图点进行更新。那么很显然，最直接的方式就是用 mvpCurrentMatchedPoints 里面的地图点替换当前关键帧的原地图点。

二、当前关键帧地图点更新

遍历 mvpCurrentMatchedPoints，mvpCurrentMatchedPoints 本质上来自于候选闭环帧。取出 mvpCurrentMatchedPoints 的地图点 pLoopMP，以及当前关键帧地图点 pCurMP→索引相同(也就是具备匹配关系)。判断一下 pCurMP 是否存在：
①存在：则把 pCurMP 替换成 pLoopMP
②不存在：直接把 pLoopMP 作为前关键帧地图点添加，

这里的代码注释，紧接着上篇博客：

        // Start Loop Fusion
        // Update matched map points and replace if duplicated
        // Step 3：检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补
        // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
        for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
        {
            if(mvpCurrentMatchedPoints[i])
            {
                //取出同一个索引对应的两种地图点，决定是否要替换
                // 匹配投影得到的地图点
                MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
                // 原来的地图点
                MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); 
                if(pCurMP)
                    // 如果有重复的MapPoint，则用匹配的地图点代替现有的
                    // 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
                    pCurMP->Replace(pLoopMP);
                else
                {
                    // 如果当前帧没有该MapPoint，则直接添加
                    mpCurrentKF->AddMapPoint(pLoopMP,i);
                    pLoopMP->AddObservation(mpCurrentKF,i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        } 

    }
    
    // Project MapPoints observed in the neighborhood of the loop keyframe
    // into the current keyframe and neighbors using corrected poses.
    // Fuse duplications.
    // Step 4：将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点
    // 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
    // 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
    // CorrectedSim3：存放矫正后当前关键帧的共视关键帧，及其世界坐标系下Sim3 变换
    SearchAndFuse(CorrectedSim3);

接下来再对 SearchAndFuse(CorrectedSim3) 进行分析。

三、SearchAndFuse()

通过前面的讲解，已经对当前关键帧以及其共视关键帧都进行姿态以及地图点的矫正，那么来看下图
在这里插入图片描述
也就是说上图中蓝色圈内(除去绿点)都已经完成了姿态以及地图点的矫正。现在来回顾一个变量，那就是 mvpLoopMapPoints→在LoopClosing::ComputeSim3() 函数中有涉及到。存储的是闭环关键帧小组的所有地图点，也就是上图中的绿色圈内所有圆点的地图点(不重复添加)。

也就是说，蓝色圈(除去绿点)完成了地图点矫正，绿色圈没有完成矫正 $\color{red} {但是}$ : 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化(与其他关键帧联系更加紧密)，我们认为是准确的。所以需要将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点。融合地图点。SearchAndFuse() 函数注释如下：

/**
 * @brief 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，新增或替换当前关键帧组中KF的地图点
 * 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
 * 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
 * 
 * @param[in] CorrectedPosesMap         矫正的当前KF对应的共视关键帧及Sim3变换
 */
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{

    // 定义ORB匹配器
    ORBmatcher matcher(0.8);

    // Step 1 遍历待矫正的当前KF的相连关键帧
    for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
    {
        KeyFrame* pKF = mit->first;
        // 矫正过的Sim 变换
        g2o::Sim3 g2oScw = mit->second;
        cv::Mat cvScw = Converter::toCvMat(g2oScw);

        // Step 2 将mvpLoopMapPoints投影到pKF帧匹配，检查地图点冲突并融合
        // mvpLoopMapPoints：与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点
        vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
        // vpReplacePoints：存储mvpLoopMapPoints投影到pKF匹配后需要替换掉的新增地图点,索引和mvpLoopMapPoints一致，初始化为空
        // 搜索区域系数为4
        matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

        // Get Map Mutex
        // 之所以不在上面 Fuse 函数中进行地图点融合更新的原因是需要对地图加锁
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
        const int nLP = mvpLoopMapPoints.size();
        // Step 3 遍历闭环帧组的所有的地图点，替换掉需要替换的地图点
        for(int i=0; i<nLP;i++)
        {
            MapPoint* pRep = vpReplacePoints[i];
            if(pRep)
            {
                // 如果记录了需要替换的地图点
                // 用mvpLoopMapPoints替换掉vpReplacePoints里记录的要替换的地图点
                pRep->Replace(mvpLoopMapPoints[i]);
            }
        }
    }
}

另外大家注意其上的 cv::Mat cvScw = Converter::toCvMat(g2oScw); 函数：

//将仿射矩阵由g2o::Sim3->cv::Mat
cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3)
{
	///首先将仿射矩阵的旋转部分转换成为Eigen下的矩阵格式
    Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();
	///对于仿射矩阵的平移部分也是要转换成为Eigen下的矩阵格式
    Eigen::Vector3d eigt = Sim3.translation();
	///获取仿射矩阵的缩放系数
    double s = Sim3.scale();
	///然后构造cv::Mat格式下的仿射矩阵
	///@todo 感觉这里的sim3就是在se3的基础上多了一个缩放系数，但是实际上真的是这样吗？
    return toCvSE3(s*eigR,eigt);
}

可以很明显的看到 cvScw 的旋转矩阵是乘上尺度之后的。

四、ORBmatcher::Fuse

从上面可以看到，其核心函数为 matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);其逻辑思路如下：

$\color{blue} (1)：$ 获得 pKF(当前关键帧的共视关键帧) 的相机内参，位姿Rcw，tcw(世界坐标系)。以及相机在世界坐标系下的位置Ow，与 pKF 已有的匹配地图点。然后对闭环匹配组关键帧组的所有地图点进行遍历。

$\color{blue} (2)：$ 判断 pMP(闭环组地图点) 是否为坏点，或者已经是 pKF 的地图点了，则跳过。把 pMP 变换到 pKF 相机坐标系下记为 p3Dc。首先判断地图点 p3Dc 是否在相机前面，不是则跳过。然后把 p3Dc 投影到当前帧(pKF) 的图像坐标，如果该地图点不在在图像内，则跳过。

$\color{blue} (3)：$ 据距离地图点是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效。在根据地图点距离相机的距离，估算金字塔层级，进而确定搜索匹配范围，然后获得该范围内的所有候选特征点下标。

$\color{blue} (4)：$ 计算候选特征点(来自于pKF) 与 pMP(闭环组地图点) BRIEF描述子的汉明距离，寻找最佳匹配点。超过指定阈值，如果这个地图点已经存在，则记录要替换信息，如果这个地图点不存在，直接添加。

/**
 * @brief 闭环矫正中使用。将当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点投影到当前关键帧，融合地图点
 * 
 * @param[in] pKF                   当前关键帧
 * @param[in] Scw                   当前关键帧经过闭环Sim3 后的世界到相机坐标系的Sim变换
 * @param[in] vpPoints              与当前关键帧闭环匹配上的关键帧及其共视关键帧组成的地图点
 * @param[in] th                    搜索范围系数
 * @param[out] vpReplacePoint       替换的地图点
 * @return int                      融合（替换和新增）的地图点数目
 */
int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector<MapPoint *> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint)
{
    // Get Calibration Parameters for later projection
    const float &fx = pKF->fx;
    const float &fy = pKF->fy;
    const float &cx = pKF->cx;
    const float &cy = pKF->cy;

    // Decompose Scw
    // Step 1 将Sim3转化为SE3并分解
    cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
    const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));// 计算得到尺度s
    cv::Mat Rcw = sRcw/scw;// 除掉s
    cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;// 除掉s
    cv::Mat Ow = -Rcw.t()*tcw;

    // Set of MapPoints already found in the KeyFrame
    // 当前帧已有的匹配地图点
    const set<MapPoint*> spAlreadyFound = pKF->GetMapPoints();

    int nFused=0;
    // 与当前帧闭环匹配上的关键帧及其共视关键帧组成的地图点
    const int nPoints = vpPoints.size();

    // For each candidate MapPoint project and match
    // 遍历所有的地图点
    for(int iMP=0; iMP<nPoints; iMP++)
    {
        MapPoint* pMP = vpPoints[iMP];

        // Discard Bad MapPoints and already found
        // 地图点无效 或 已经是该帧的地图点（无需融合），跳过
        if(pMP->isBad() || spAlreadyFound.count(pMP))
            continue;

        // Get 3D Coords.
        // Step 2 地图点变换到当前相机坐标系下
        cv::Mat p3Dw = pMP->GetWorldPos();

        // Transform into Camera Coords.
        cv::Mat p3Dc = Rcw*p3Dw+tcw;

        // Depth must be positive
        if(p3Dc.at<float>(2)<0.0f)
            continue;

        // Project into Image
        // Step 3 得到地图点投影到当前帧的图像坐标
        const float invz = 1.0/p3Dc.at<float>(2);
        const float x = p3Dc.at<float>(0)*invz;
        const float y = p3Dc.at<float>(1)*invz;

        const float u = fx*x+cx;
        const float v = fy*y+cy;

        // Point must be inside the image
        // 投影点必须在图像范围内
        if(!pKF->IsInImage(u,v))
            continue;

        // Depth must be inside the scale pyramid of the image
        // Step 4 根据距离是否在图像合理金字塔尺度范围内和观测角度是否小于60度判断该地图点是否有效
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        cv::Mat PO = p3Dw-Ow;
        const float dist3D = cv::norm(PO);

        if(dist3D<minDistance || dist3D>maxDistance)
            continue;

        // Viewing angle must be less than 60 deg
        cv::Mat Pn = pMP->GetNormal();

        if(PO.dot(Pn)<0.5*dist3D)
            continue;

        // Compute predicted scale level
        const int nPredictedLevel = pMP->PredictScale(dist3D,pKF);

        // Search in a radius
        // 计算搜索范围
        const float radius = th*pKF->mvScaleFactors[nPredictedLevel];

        // Step 5 在当前帧内搜索匹配候选点
        const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);

        if(vIndices.empty())
            continue;

        // Match to the most similar keypoint in the radius
        // Step 6 寻找最佳匹配点（没有用到次佳匹配的比例）
        const cv::Mat dMP = pMP->GetDescriptor();

        int bestDist = INT_MAX;
        int bestIdx = -1;
        for(vector<size_t>::const_iterator vit=vIndices.begin(); vit!=vIndices.end(); vit++)
        {
            const size_t idx = *vit;
            const int &kpLevel = pKF->mvKeysUn[idx].octave;

            if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
                continue;

            const cv::Mat &dKF = pKF->mDescriptors.row(idx);

            int dist = DescriptorDistance(dMP,dKF);

            if(dist<bestDist)
            {
                bestDist = dist;
                bestIdx = idx;
            }
        }

        // If there is already a MapPoint replace otherwise add new measurement
        // Step 7 替换或新增地图点
        if(bestDist<=TH_LOW)
        {
            MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
            if(pMPinKF)
            {
                // 如果这个地图点已经存在，则记录要替换信息
                // 这里不能直接替换，原因是需要对地图点加锁后才能替换，否则可能会crash。所以先记录，在加锁后替换
                if(!pMPinKF->isBad())
                    vpReplacePoint[iMP] = pMPinKF;
            }
            else
            {
                // 如果这个地图点不存在，直接添加
                pMP->AddObservation(pKF,bestIdx);
                pKF->AddMapPoint(pMP,bestIdx);
            }
            nFused++;
        }
    }
    // 融合（替换和新增）的地图点数目
    return nFused;
}

五、更新共视相连关系

到目前为止，针对于 src/LoopClosing.cc 中的 LoopClosing::CorrectLoop() 函数已完成了 SearchAndFuse(CorrectedSim3) 的讲解。对于当前关键帧的共视关键帧，又完成了新一轮的地图点优化。竟然根性了地图点，那么当然还需要更新关键帧之间的连接关系。这里更新的是关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系。新的连接关系存储于 LoopConnections 变量之中，代码实现逻辑如下：

$\color{blue} (1)：$ 遍历当前关键帧相连的关键帧组(一级相连)，然后获得相连的关键帧的共视关键帧 vpPreviousNeighbors(二级相连)。

$\color{blue} (2)：$ 更新当前关键帧相连关键帧的连接关系，存储于 LoopConnections 之中。

$\color{blue} (3)：$ vpPreviousNeighbors 与 mvpCurrentConnectedKFs 的连接关系，如果与 LoopConnections 中的连接关系保持已知。说明其没有更新，所以会删除没有更新连接关系的连接帧。

$\color{blue} (3)：$ 最终 LoopConnections 表示的，是当前帧相连关键帧组（一级相连），连接关系发生改变的相连关键帧，以及其对应的连接关系。

   // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
    // Step 5：更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系
    // LoopConnections：存储因为闭环地图点调整而新生成的连接关系
    map<KeyFrame*, set<KeyFrame*> > LoopConnections;

    // Step 5.1：遍历当前帧相连关键帧组（一级相连）
    for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        // Step 5.2：得到与当前帧相连关键帧的相连关键帧（二级相连）
        vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

        // Update connections. Detect new links.
        // Step 5.3：更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)
        pKFi->UpdateConnections();
        // Step 5.4：取出该帧更新后的连接关系
        LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
        // Step 5.5：从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系
        for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);
        }
        // Step 5.6：从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系
        for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);
        }
    }

六、结语

在执行完LoopClosing::SearchAndFuse()之后会有一个什么样的体现呢，先来看前面的图示：
在这里插入图片描述
虽然我们计算出了当前帧(红圆点)与匹配候选关键帧(绿色原点)之间的Sim3变换，但是我们不能直接像图示一样直接使用红色的虚线(Sim3变换)连接起来，这样构建的地图太生硬了，短层感太强烈，通过LoopClosing::SearchAndFuse()之后，大致可以实现如下效果：请添加图片描述
红色虚线变成类似红色的实线的样子，当然，这里是做个比喻而已。因为在经过LoopClosing::SearchAndFuse()之后，还要执行本质图优化 Optimizer::OptimizeEssentialGraph() 以及全局BA优化 RunGlobalBundleAdjustment()，才能实现真正的闭环。这两个内容会在后面的博客中进行讲解。

文内容来自计算机视觉life ORB-SLAM2 课程课件