ORBSLAM2源码小记(6)--建图器LocalMapping

ORBSLAM2的建图器LocalMapping

主要是记录下相对容易忘的内容，不会写的很细

建图器LocalMapping

LocalMapping的所有输入都是关键帧，从一个列表中不断pop出由Tracking送进来的关键帧，然后逐步执行下列操作：

• 为关键帧更新观测、创建Bow等初始化关键的操作
• 通过Tracking中统计的相关指标来剔除不好的地图点
• 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
• 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
• 局部地图BA
• 检测并剔除当前帧相邻的关键帧中冗余的关键帧

LocalMapping里的每个模块都是互相独立的，而且步骤也都很简单，因此就不画图了，文字描述一下就好了。

初始化关键帧

这里主要负责计算对刚刚送进来的关键帧初始化，因为通过普通帧创建的关键帧只保留了特征点和位姿的信息，其余的比如词袋、地图点的观测（地图点的平均方向要用关键帧来算）和共视关系之类的数据统统没有，因此这一步主要就是计算这些信息。

/**
 * @brief 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
 *
 */
void LocalMapping::ProcessNewKeyFrame()
{
    // 从缓冲队列中取出一帧关键帧
    ...

    // 计算该关键帧特征点的词袋向量
    mpCurrentKeyFrame->ComputeBoW();

    // 当前处理关键帧中有效的地图点，更新normal，描述子等信息
    const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    // 对当前处理的这个关键帧中的所有的地图点展开遍历
    for (size_t i = 0; i < vpMapPointMatches.size(); i++)
    {
        MapPoint *pMP = vpMapPointMatches[i];
        if (pMP)
        {
            if (!pMP->isBad())
            {
                if (!pMP->IsInKeyFrame(mpCurrentKeyFrame))
                {
                    // 如果地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    // 获得该点的平均观测方向和观测距离范围
                    pMP->UpdateNormalAndDepth();
                    // 更新地图点的最佳描述子
                    pMP->ComputeDistinctiveDescriptors();
                }
                else
                {
                    // 这些地图点可能来自双目或RGBD在创建关键帧中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生
                    // 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }

    // 更新关键帧间的连接关系（共视图）
    mpCurrentKeyFrame->UpdateConnections();

    // 将该关键帧插入到地图中
    mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

剔除不好的地图点

剔除以下两种地图点：

• 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，即Tracking中计算的mnFound和mnVisible
• 该地图点创立时间超过2个关键帧，但是观测到该点的普通帧数目不超过阈值

/**
 * @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点
 * mlpRecentAddedMapPoints：存储新增的地图点，这里是要删除其中不靠谱的
 */
void LocalMapping::MapPointCulling()
{
    ...

    // 遍历检查新添加的地图点
    while (lit != mlpRecentAddedMapPoints.end())
    {
        MapPoint *pMP = *lit;
        if (pMP->isBad())
        {
            // 已经是坏点的地图点仅从队列中删除
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if (pMP->GetFoundRatio() < 0.25f)
        {
            // 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除
            // (mnFound/mnVisible） < 25%
            // mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好
            // mnVisible：地图点应该被看到的次数，在每一个普通帧到来时，计算其局部地图投影到当前帧后是否应该被观测到来计算
            // (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if (((int)nCurrentKFid - (int)pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs)
        {
            // 从该点建立开始，到现在已经过了不小于2个关键帧
            // 但是观测到该点的相机数却不超过阈值cnThObs，从地图中删除
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if (((int)nCurrentKFid - (int)pMP->mnFirstKFid) >= 3)
            // 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
            // 从检测队列中移除
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

三角化创建新的地图点

用当前关键帧与共视关键帧通过三角化产生新的地图点，使得跟踪更稳定，具体而言：

1. 首先找到当前关键帧共视关系最好的前n个共视关键帧
2. 遍历n个共视关键帧，通过计算相机光心距离来判断基线距离，删除掉基线过短的共视关键帧
3. 在剩余共视关键帧中计算基础矩阵，通过Bow快速匹配并用极线抑制离群点来生成匹配点对
4. 三角化并校验重投影误差是否满足卡方检验，同时还会保证深度为正
5. 最后会检查尺度一致性，地图点到两帧的深度的比值应该是对应特征点在两帧中金字塔层级的比值近似一致

/**
 * @brief 用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
 *
 */
void LocalMapping::CreateNewMapPoints()
{
    // nn表示搜索最佳共视关键帧的数目
    // 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图
    int nn = 10;
    if (mbMonocular)
        nn = 20;

    // 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
    const vector<KeyFrame *> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

    ...

    // 记录三角化成功的地图点数目
    int nnew = 0;

    // 遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化
    for (size_t i = 0; i < vpNeighKFs.size(); i++)
    {
        ...

        // 基线向量，两个关键帧间的相机位移
        cv::Mat vBaseline = Ow2 - Ow1;
        // 基线长度
        const float baseline = cv::norm(vBaseline);

        // 判断相机运动的基线是不是足够长
        if (!mbMonocular)
        {
            // 如果是双目相机，关键帧间距小于本身的基线时不生成3D点
            // 因为太短的基线下能够恢复的地图点不稳定
            if (baseline < pKF2->mb)
                continue;
        }
        else
        {
            // 单目相机情况
            // 相邻关键帧的场景深度中值
            const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
            // 基线与景深的比例
            const float ratioBaselineDepth = baseline / medianDepthKF2;
            // 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点
            if (ratioBaselineDepth < 0.01)
                continue;
        }

        // 根据两个关键帧的位姿计算它们之间的基础矩阵
        cv::Mat F12 = ComputeF12(mpCurrentKeyFrame, pKF2);

        // 通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
        vector<pair<size_t, size_t>> vMatchedIndices;
        matcher.SearchForTriangulation(mpCurrentKeyFrame, pKF2, F12, vMatchedIndices, false);

        ...

        // 对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
        const int nmatches = vMatchedIndices.size();
        for (int ikp = 0; ikp < nmatches; ikp++)
        {
            ...

            // 三角化恢复3D点
            cv::Mat x3D;

            if (cosParallaxRays < cosParallaxStereo && cosParallaxRays > 0 && (bStereo1 || bStereo2 || cosParallaxRays < 0.9998))
            {
                cv::Mat A(4, 4, CV_32F);
                A.row(0) = xn1.at<float>(0) * Tcw1.row(2) - Tcw1.row(0);
                A.row(1) = xn1.at<float>(1) * Tcw1.row(2) - Tcw1.row(1);
                A.row(2) = xn2.at<float>(0) * Tcw2.row(2) - Tcw2.row(0);
                A.row(3) = xn2.at<float>(1) * Tcw2.row(2) - Tcw2.row(1);

                cv::Mat w, u, vt;
                cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

                x3D = vt.row(3).t();
                // 归一化之前的检查
                if (x3D.at<float>(3) == 0)
                    continue;
                // 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
                x3D = x3D.rowRange(0, 3) / x3D.at<float>(3);
            }
            // 匹配点对夹角小，用双目恢复3D点
            else if (bStereo1 && cosParallaxStereo1 < cosParallaxStereo2)
            {
                // 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了
                x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
            }
            else if (bStereo2 && cosParallaxStereo2 < cosParallaxStereo1)
            {
                x3D = pKF2->UnprojectStereo(idx2);
            }
            else
                continue; 

            ...

            // 检测生成的3D点是否在相机前方,不在的话就放弃这个点
            float z1 = Rcw1.row(2).dot(x3Dt) + tcw1.at<float>(2);
            if (z1 <= 0)
                continue;
            float z2 = Rcw2.row(2).dot(x3Dt) + tcw2.at<float>(2);
            if (z2 <= 0)
                continue;

            // 计算3D点在当前关键帧下的重投影误差
            const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
            const float x1 = Rcw1.row(0).dot(x3Dt) + tcw1.at<float>(0);
            const float y1 = Rcw1.row(1).dot(x3Dt) + tcw1.at<float>(1);
            const float invz1 = 1.0 / z1;

            if (!bStereo1)
            {
                // 单目情况下
                float u1 = fx1 * x1 * invz1 + cx1;
                float v1 = fy1 * y1 * invz1 + cy1;
                float errX1 = u1 - kp1.pt.x;
                float errY1 = v1 - kp1.pt.y;
                // 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991
                if ((errX1 * errX1 + errY1 * errY1) > 5.991 * sigmaSquare1)
                    continue;
            }
            else
            {
                // 双目情况
                float u1 = fx1 * x1 * invz1 + cx1;
                // 根据视差公式计算假想的右目坐标
                float u1_r = u1 - mpCurrentKeyFrame->mbf * invz1;
                float v1 = fy1 * y1 * invz1 + cy1;
                float errX1 = u1 - kp1.pt.x;
                float errY1 = v1 - kp1.pt.y;
                float errX1_r = u1_r - kp1_ur;
                // 自由度为3，卡方检验阈值是7.8
                if ((errX1 * errX1 + errY1 * errY1 + errX1_r * errX1_r) > 7.8 * sigmaSquare1)
                    continue;
            }

            //  计算3D点在另一个关键帧下的重投影误差，操作同上
            ...

            // 检查尺度连续性
            // 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点
            cv::Mat normal1 = x3D - Ow1;
            float dist1 = cv::norm(normal1);
            cv::Mat normal2 = x3D - Ow2;
            float dist2 = cv::norm(normal2);
            if (dist1 == 0 || dist2 == 0)
                continue;

            // ratioDist是不考虑金字塔尺度下的距离比例
            const float ratioDist = dist2 / dist1;
            // 金字塔尺度因子的比例
            const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave] / pKF2->mvScaleFactors[kp2.octave];

            // 距离的比例和图像金字塔的比例不应该差太多，否则就跳过
            if (ratioDist * ratioFactor < ratioOctave || ratioDist > ratioOctave * ratioFactor)
                continue;

            // 三角化生成3D点成功，构造成MapPoint
            ...

            // 将新产生的点放入检测队列
            // 这些MapPoints都会经过MapPointCulling函数的检验
            mlpRecentAddedMapPoints.push_back(pMP);

            nnew++;
        }
    }
}

融合重复地图点

由于物理上同一个点可能被不同帧观测到从而被认成多个地图点，所以需要不断去融合这些重复的地图点来保证建图本身是准确的，具体而言：

1. 首先找到当前关键帧共视关系最好的前n个一级共视关键帧
2. 再找每个一级共视关键帧中共视最好的前5个二级共视关键帧
3. 记录下这些一级共视和二级共视关键帧，作为待融合的关键帧
4. 将当前关键帧的地图点投影到上述关键帧中，使用前文提到的ORBmatcher::Fuse来进行融合
5. 将上述关键帧的地图点投影到当前关键帧中，使用前文提到的ORBmatcher::Fuse来进行融合
6. 更新地图点的描述子、深度、观测方向等信息

/**
 * @brief 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点
 *
 */
void LocalMapping::SearchInNeighbors()
{
    // 获得当前关键帧在共视图中权重排名前nn的邻接关键帧
    // 以及一级共视关键帧的前五个二级共视关键帧
    ...


    // 将当前帧的地图点分别投影到两级相邻关键帧，寻找匹配点对应的地图点进行融合
    vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for (vector<KeyFrame *>::iterator vit = vpTargetKFs.begin(), vend = vpTargetKFs.end(); vit != vend; vit++)
    {
        KeyFrame *pKFi = *vit;

        // 将地图点投影到关键帧中进行匹配和融合；融合策略如下
        // 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
        // 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
        // 注意这个时候对地图点融合的操作是立即生效的
        matcher.Fuse(pKFi, vpMapPointMatches);
    }

    // 将相邻关键帧地图点分别投影到当前关键帧，寻找匹配点对应的地图点进行融合
    vector<MapPoint *> vpFuseCandidates;
    vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());
    // 遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidates
    for (vector<KeyFrame *>::iterator vitKF = vpTargetKFs.begin(), vendKF = vpTargetKFs.end(); vitKF != vendKF; vitKF++)
    {
        KeyFrame *pKFi = *vitKF;
        vector<MapPoint *> vpMapPointsKFi = pKFi->GetMapPointMatches();

        for (vector<MapPoint *>::iterator vitMP = vpMapPointsKFi.begin(), vendMP = vpMapPointsKFi.end(); vitMP != vendMP; vitMP++)
        {
            MapPoint *pMP = *vitMP;
            if (!pMP)
                continue;
            if (pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;

            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }
    // 当前关键帧和所有邻接关键帧的地图点进行融合
    matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates);

    // 更新当前帧地图点的描述子、深度、平均观测方向等属性
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for (size_t i = 0, iend = vpMapPointMatches.size(); i < iend; i++)
    {
        MapPoint *pMP = vpMapPointMatches[i];
        if (pMP)
        {
            if (!pMP->isBad())
            {
                // 在所有找到pMP的关键帧中，获得最佳的描述子
                pMP->ComputeDistinctiveDescriptors();

                // 更新平均观测方向和观测距离
                pMP->UpdateNormalAndDepth();
            }
        }
    }

    // 更新当前帧与其它帧的共视连接关系
    mpCurrentKeyFrame->UpdateConnections();
}

局部BA

LocalMapping中的局部BA不要跟Tracking中的局部地图优化弄混了，Tracking中的局部地图优化只优化一个普通帧的位姿，不优化地图点；而LocalMapping中的局部BA直接调用Optimizer::LocalBundleAdjustment函数，这个函数会优化多个关键帧以及相应地图点的位姿。

1. 首先找到当前关键帧的一级共视关键帧和对应的地图点
2. 通过地图点找到二级共视关键帧
3. 构建重投影误差优化，其中当前关键帧、一级共视关键帧和地图点是待优化变量；二级共视关键帧的位姿不会被优化
4. 第一阶段优化会使用卡方检验剔除外点
5. 第二阶段优化会使用内点求精
6. 剔除掉误差比较大地图点的观测，并更新关键帧与地图点的各种信息(观测方向等等)

/*
    * @brief Local Bundle Adjustment
    *
    * 1. Vertex:
    *     - g2o::VertexSE3Expmap()，LocalKeyFrames，即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
    *     - g2o::VertexSE3Expmap()，FixedCameras，即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrames）的位姿，在优化中这些关键帧的位姿不变
    *     - g2o::VertexSBAPointXYZ()，LocalMapPoints，即LocalKeyFrames能观测到的所有MapPoints的位置
    * 2. Edge:
    *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
    *         + Vertex：关键帧的Tcw，MapPoint的Pw
    *         + measurement：MapPoint在关键帧中的二维位置(u,v)
    *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    *     - g2o::EdgeStereoSE3ProjectXYZ()，BaseBinaryEdge
    *         + Vertex：关键帧的Tcw，MapPoint的Pw
    *         + measurement：MapPoint在关键帧中的二维位置(ul,v,ur)
    *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
    *
    * @param pKF        KeyFrame
    * @param pbStopFlag 是否停止优化的标志
    * @param pMap       在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate
    * @note 由局部建图线程调用,对局部地图进行优化的函数
    */
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap)
{

    // 局部关键帧 一级共视+二级共视
    ... 

    // 构造g2o优化器
    ...

    // 添加待优化的位姿顶点(一级共视)
    for (list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++)
    {
        KeyFrame *pKFi = *lit;
        g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
        // 设置初始优化位姿
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->mnId);
        // 如果是初始关键帧，要锁住位姿不优化
        vSE3->setFixed(pKFi->mnId == 0);
        optimizer.addVertex(vSE3);
        if (pKFi->mnId > maxKFid)
            maxKFid = pKFi->mnId;
    }

    // 添加不优化的位姿顶点(二级共视)
    for (list<KeyFrame *>::iterator lit = lFixedCameras.begin(), lend = lFixedCameras.end(); lit != lend; lit++)
    {
        KeyFrame *pKFi = *lit;
        g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->mnId);
        // 所有的这些顶点的位姿都不优化，只是为了增加约束项
        vSE3->setFixed(true);
        optimizer.addVertex(vSE3);
        if (pKFi->mnId > maxKFid)
            maxKFid = pKFi->mnId;
    }

    ...

    // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
    const float thHuberMono = sqrt(5.991);

    // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
    const float thHuberStereo = sqrt(7.815);

    // 遍历所有的局部地图点
    for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
    {
        // 添加顶点：MapPoint
        MapPoint *pMP = *lit;
        g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        // 前面记录maxKFid的作用在这里体现
        int id = pMP->mnId + maxKFid + 1;
        vPoint->setId(id);
        // 因为使用了LinearSolverType，所以需要将所有的三维点边缘化掉
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        // 观测到该地图点的KF和该地图点在KF中的索引
        const map<KeyFrame *, size_t> observations = pMP->GetObservations();

        // 在添加完了一个地图点之后, 对每一对关联的地图点和关键帧构建边
        // 遍历所有观测到当前地图点的关键帧
        for (map<KeyFrame *, size_t>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
        {
            KeyFrame *pKFi = mit->first;

            if (!pKFi->isBad())
            {
                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
                // 根据单目/双目两种不同的输入构造不同的误差边
                // Monocular observation
                // 单目模式下
                if (pKFi->mvuRight[mit->second] < 0)
                {
                    Eigen::Matrix<double, 2, 1> obs;
                    obs << kpUn.pt.x, kpUn.pt.y;

                    g2o::EdgeSE3ProjectXYZ *e = new g2o::EdgeSE3ProjectXYZ();
                    // 边的第一个顶点是地图点
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
                    // 边的第一个顶点是观测到该地图点的关键帧
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId)));
                    e->setMeasurement(obs);
                    // 权重为特征点所在图像金字塔的层数的倒数
                    const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                    e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);

                    // 使用鲁棒核函数抑制外点
                    g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuberMono);

                    e->fx = pKFi->fx;
                    e->fy = pKFi->fy;
                    e->cx = pKFi->cx;
                    e->cy = pKFi->cy;
                    // 将边添加到优化器，记录边、边连接的关键帧、边连接的地图点信息
                    optimizer.addEdge(e);
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                else 
                {
                    // 双目或RGB-D模式和单目模式类似
                    ...
                }
            }
        } // 遍历所有观测到当前地图点的关键帧
    } // 遍历所有的局部地图中的地图点

    ...

    // 分成两个阶段开始优化。
    // 第一阶段优化
    optimizer.initializeOptimization();
    // 迭代5次
    optimizer.optimize(5);

    ...

    // 如果有外部请求停止,那么就不在进行第二阶段的优化
    if (bDoMore)
    {
        // 检测outlier，并设置下次不优化
        // 遍历所有的单目误差边
        for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
        {
            g2o::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
            MapPoint *pMP = vpMapPointEdgeMono[i];

            if (pMP->isBad())
                continue;

            // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
            // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
            // 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，不优化了。
            if (e->chi2() > 5.991 || !e->isDepthPositive())
            {
                // 不优化
                e->setLevel(1);
            }
            // 第二阶段优化的时候就属于精求解了,所以就不使用核函数
            e->setRobustKernel(0);
        }

        // 对于所有的双目的误差边也都进行类似的操作
        ...

        // 排除误差较大的outlier后再次优化 -- 第二阶段优化
        optimizer.initializeOptimization(0);
        optimizer.optimize(10);
    }

    vector<pair<KeyFrame *, MapPoint *>> vToErase;
    vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());

    // 在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点
    // 对于单目误差边
    for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    {
        g2o::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
        MapPoint *pMP = vpMapPointEdgeMono[i];

        if (pMP->isBad())
            continue;

        // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
        // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
        // 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，要删掉了
        if (e->chi2() > 5.991 || !e->isDepthPositive())
        {
            // outlier
            KeyFrame *pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi, pMP));
        }
    }

    // 双目误差边
    ...

    // 删除点
    // 连接偏差比较大，在关键帧中剔除对该地图点的观测
    // 连接偏差比较大，在地图点中剔除对该关键帧的观测
    if (!vToErase.empty())
    {
        for (size_t i = 0; i < vToErase.size(); i++)
        {
            KeyFrame *pKFi = vToErase[i].first;
            MapPoint *pMPi = vToErase[i].second;
            pKFi->EraseMapPointMatch(pMPi);
            pMPi->EraseObservation(pKFi);
        }
    }

    // 优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性
    for (list<KeyFrame *>::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++)
    {
        KeyFrame *pKF = *lit;
        g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        pKF->SetPose(Converter::toCvMat(SE3quat));
    }
    for (list<MapPoint *>::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
    {
        MapPoint *pMP = *lit;
        g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + maxKFid + 1));
        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }
}

剔除不好的关键帧

关键帧的剔除也是非常简单的，剔除标准只有一个：当前关键帧的90%以上的地图点能被其他关键帧（至少3个）观测到

/**
 * @brief 检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧
 * 冗余关键帧的判定：90%以上的地图点能被其他关键帧（至少3个）观测到
 */
void LocalMapping::KeyFrameCulling()
{
    // 根据共视图提取当前关键帧的所有共视关键帧
    vector<KeyFrame *> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

    // 对所有的共视关键帧进行遍历
    for (vector<KeyFrame *>::iterator vit = vpLocalKeyFrames.begin(), vend = vpLocalKeyFrames.end(); vit != vend; vit++)
    {
        KeyFrame *pKF = *vit;
        // 第1个关键帧不能删除，跳过
        if (pKF->mnId == 0)
            continue;
        // Step 2：提取每个共视关键帧的地图点
        const vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();

        // 记录某个点被观测次数，后面并未使用
        int nObs = 3;
        // 观测次数阈值，默认为3
        const int thObs = nObs;
        // 记录冗余观测点的数目
        int nRedundantObservations = 0;

        int nMPs = 0;

        // 遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点
        for (size_t i = 0, iend = vpMapPoints.size(); i < iend; i++)
        {
            MapPoint *pMP = vpMapPoints[i];
            if (pMP)
            {
                if (!pMP->isBad())
                {
                    if (!mbMonocular)
                    {
                        // 对于双目或RGB-D，仅考虑近处（不超过基线的40倍 ）的地图点
                        if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
                            continue;
                    }

                    nMPs++;
                    // pMP->Observations() 是观测到该地图点的相机总数目（单目1，双目2）
                    if (pMP->Observations() > thObs)
                    {
                        const int &scaleLevel = pKF->mvKeysUn[i].octave;
                        // Observation存储的是可以看到该地图点的所有关键帧的集合
                        const map<KeyFrame *, size_t> observations = pMP->GetObservations();

                        int nObs = 0;
                        // 遍历观测到该地图点的关键帧
                        for (map<KeyFrame *, size_t>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
                        {
                            KeyFrame *pKFi = mit->first;
                            if (pKFi == pKF)
                                continue;
                            const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
                            
                            if (scaleLeveli <= scaleLevel + 1)
                            {
                                nObs++;
                                // 已经找到3个满足条件的关键帧，就停止不找了
                                if (nObs >= thObs)
                                    break;
                            }
                        }
                        // 地图点至少被3个关键帧观测到，就记录为冗余点，更新冗余点计数数目
                        if (nObs >= thObs)
                        {
                            nRedundantObservations++;
                        }
                    }
                }
            }
        }

        // 如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧
        if (nRedundantObservations > 0.9 * nMPs)
            pKF->SetBadFlag();
    }
}