5.2ORB-SLAM3之回环矫正

1.简介

在上一章《回环检测之检测是否存共视区域》已经介绍了检测共视区域的部分，接下来就是对共视区域进行回环矫正或者地图融合。

回环矫正和之前的ORBSLAM系列一致，就是消除因为长时间运动产生的位姿累计误差和尺度漂移。在ORBSLAM3中新增了多地图系统，因此出现共视区域的地方有可能出现在不同的子地图中，需要对两个子地图进行融合。本篇主要对回环矫正部分进行介绍。

2.回环有效性验证

在寻找共视区域步骤中初步得到了当前关键帧与候选关键帧之间的相似变换，通过相似变换可进一步初步得到当前关键帧矫正后的位姿mg2oLoopScw。在IMU模式下会通过比较矫正前和矫正后当前关键帧位姿的差异判断回环的好坏，如果矫正误差的rpy三个方向上的角度低于一定阈值，认为当前回环是有效回环：

if(mpCurrentKF->GetMap()->IsInertial()){
    // 拿到当前关键帧相对于世界坐标系的位姿
    Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast<double>();
    g2o::Sim3 g2oTwc(Twc.unit_quaternion(),Twc.translation(),1.0);
    
    // mg2oLoopScw是通过回环检测的Sim3计算出的回环矫正后的当前关键帧的初始位姿, Twc(g2oTwc)是当前关键帧回环矫正前的位姿.
    // g2oSww_new 可以理解为correction,即初次矫正前后当前关键帧之间的位姿差异
    g2o::Sim3 g2oSww_new = g2oTwc*mg2oLoopScw;
    // 拿到 roll ,pitch ,yaw
    Eigen::Vector3d phi = LogSO3(g2oSww_new.rotation().toRotationMatrix());
    cout << "phi = " << phi.transpose() << endl; 
    // 这里算是通过imu重力方向验证回环结果, 如果pitch或roll角度偏差稍微有一点大,则回环失败. 对yaw容忍比较大(20度)
    if (fabs(phi(0))<0.008f && fabs(phi(1))<0.008f && fabs(phi(2))<0.349f){
        // 如果是imu模式
        if(mpCurrentKF->GetMap()->IsInertial()){
            // If inertial, force only yaw
            // 如果是imu模式,强制将焊接变换的的 roll 和 pitch 设为0
            if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&
                    mpCurrentKF->GetMap()->GetIniertialBA2()){
                phi(0)=0;
                phi(1)=0;
                g2oSww_new = g2o::Sim3(ExpSO3(phi),g2oSww_new.translation(),1.0);
                mg2oLoopScw = g2oTwc.inverse()*g2oSww_new;
            }
        }
    }
}

3.回环矫正

该函数的作用是在检测到回环后，根据回环关系对当前帧及其共视关键帧进行位姿矫正。

void LoopClosing::CorrectLoop();

在回环矫正前需要暂时停止局部建图线程(暂时不需要新的关键帧)和全局BA(回环矫正后会统一再进行全局BA)，为闭环矫正做准备。

3.1 根据共视关系更新当前关键帧与其它关键帧之间的连接关系

void KeyFrame::UpdateConnections(bool upParent)

1. 首先获得该关键帧的所有MapPoint点，统计有多少关键帧与当前关键帧存在共视关系(是否观测到了同一个3D点)，统计结果放在KFcounter(可以通过地图点的observations属性获得)。
2. 只要共视点数量大于阈值th=15,就为关键帧与当前帧互相添加连接关系，边的权重为共视点的数量。
3. 如果遍历完所有的共视关键帧，没有连接到关键帧（权重超过阈值），则对权重最大的关键帧建立连接关系

前面三步都是为了寻找满足要求的关键帧，找到有资格和当前KF建立连接关系的共视关键帧，添加关键帧的核心函数如下：

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(!mConnectedKeyFrameWeights.count(pKF))// 如果没建立共视关系则直接建立连接关系，本质上是一个map类型的变量
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight)// 如果已经建立共视关系了更新权重(共视点的数量)
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }

   // 按照权重对当前关键帧的共视关键帧进行排序
   UpdateBestCovisibles();
}

4. 更新生成树的连接，初始化该关键帧的父关键帧为共视程度最高的那个关键帧，将当前关键帧作为其子关键帧

3.2 通过位姿传播，得到Sim3优化后与当前帧相连的关键帧的位姿以及它们的地图点

• 调整当前关键帧的共视帧位姿
• 调整共视帧的地图点坐标
• 对调整后的当前帧地图点进行融合和替换
• 对调整后的当前帧共视关键帧的地图点进行融合和替换
• 更新当前关键帧的共视关键帧的连接关系
• 对闭环共视关键帧进行本质图优化
• 进行全局BA优化

3.2.1 通过mg2oLoopScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿（还没有修正）

已知在检测共视区域时已经得到了当前关键帧调整之后的sim3位姿，则当前关键帧的共视关键帧位姿可以通过其与关键帧之间的相对运动变换得到。

共视帧sim3坐标 = sim3相对变换 * 当前帧sim3坐标

g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oLoopScw;

3.2.2 得到矫正的当前关键帧的共视关键帧位姿后，修正这些关键帧的地图点

矫正前世界系坐标矫正前位姿 -> 矫正前相机系坐标---->g2oSiw.map(P3Dw)
矫正前相机系坐标矫正后位姿 -> 矫正后世界系坐标---->g2oCorrectedSwi.map(g2oSiw.map(P3Dw))

// 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系，然后再反映射到校正后的世界坐标系下                
Eigen::Vector3d P3Dw = pMPi->GetWorldPos().cast<double>();
Eigen::Vector3d eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(P3Dw));

地图点矫正完成后更新地图点id，平均观测方向和平均观测距离，关键帧对应的恒速模型(因为位姿出现了变化)和关键帧链接关系(主要是权重，因为地图点出现了变化)

3.3 检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补

对于存在闭环关系的当前关键帧和回环关键帧，原则上讲对于相互匹配的特征点会对应到同一个地图点(如下图)，由于累计误差和漂移的存在相互匹配的一对特征点对应的地图点可能不一致，因此需要替换。考虑到匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差，因此对于当前关键帧如果有重复的MapPoint，则用匹配的地图点代替现有的

// 取出同一个索引对应的两种地图点，决定是否要替换
MapPoint* pLoopMP = mvpLoopMatchedMPs[i];// 匹配投影得到的地图点
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);// 原来的地图点

if(pCurMP)
    // 对于重复的Mappoint使用匹配的地图点替换
    pCurMP->Replace(pLoopMP);
else
{
    // 如果当前帧没有该MapPoint，则直接添加
    mpCurrentKF->AddMapPoint(pLoopMP,i);
    pLoopMP->AddObservation(mpCurrentKF,i);
    pLoopMP->ComputeDistinctiveDescriptors();
}

3.4 将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中进行匹配，融合、新增或替换当前关键帧组中KF的地图点

上一步是对当前关键帧中的地图点进行检查，这一步是对当前关键帧的共视关键帧地图点进行处理。将回环处的共视关键帧组中的Mappoint投影到当前帧组中的每一个共视关键帧上，对于匹配上的特征点对应的地图点进行类似的替换、填补和融合。

SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint);

3.5 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系

同样的，因为当前关键帧的共视关键帧地图点坐标发生了变化，因此对于关键帧之间的连接权重可能会发生变化，需要进行更新。

pKFi->UpdateConnections();

然后,从所有当前关键帧的一级共视关键帧和二级共视关键帧中删除闭环前的一级共视关键帧和二级共视关键帧，得到由闭环产生的一二级共视关键帧，为本质图优化作准备。

3.6 对闭环共视关键帧进行本质图优化

参与本质图优化的是当前关键帧和因为回环而产生的共视关键帧。

/**
    * @brief Construct a new Optimizer:: Optimize Essential Graph object
    * @param pLoopMap 当前活跃子地图
    * @param mpLoopMatchedKF 几何验证时成功匹配的候选关键帧
    * @param mpCurrent 当前关键帧
    * @param NonCorrectedSim3 当前关键帧共视关键帧矫正前的位姿
    * @param CorrectedSim3 当前关键帧共视关键帧矫正后的位姿
    * @param LoopConnections 因为回环产生的当前关键帧的共视关键帧
    * @param bFixedScale 是否固定尺度
    */
Optimizer::OptimizeEssentialGraph(pLoopMap, mpLoopMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, bFixedScale);

3.7 新建一个线程用于全局BA优化

OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints

mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, pLoopMap, mpCurrentKF->mnId);