【SLAM学习笔记】12-ORB

【SLAM学习笔记】12-ORB

2021SC@SDUSC

目录

• 1.前言
• 2.代码分析

1.前言

这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析

1. 单帧优化
2. 局部地图优化
3. 全局优化
4. 尺度与重力优化
5. sim3优化
6. 地图回环优化
7. 地图融合优化

下面给出逐步注释分析

2.代码分析

Local Bundle Adjustment LoopClosing::MergeLocal() 融合地图时使用，纯视觉
优化目标：

1. vpAdjustKF
2. 2.vpAdjustKF与vpFixedKF对应的MP点

优化所有的当前关键帧共视窗口里的关键帧和地图点, 固定所有融合帧共视窗口里的帧

在这里插入代码片void Optimizer::LocalBundleAdjustment(Keyframe *pMainKF, vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag)
{
    bool bShowImages = false;

    // 1. 构建g2o优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType *linearSolver;

    linearSolver = new g2o::LinearSolverEigen();

    g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    optimizer.setVerbose(false);

    if (pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    long unsigned int maxKFid = 0;
    set spKeyframeBA; // 存放关键帧，包含固定的与不固定的
    vector vpMPs;

    Map *pCurrentMap = pMainKF->GetMap();

    // Set fixed Keyframe vertices
    // 2. 构建固定关键帧的节点，并储存对应的MP
    for (Keyframe *pKFi : vpFixedKF)
    {
        if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
        {
            Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map", Verbose::VERBOSITY_NORMAL);
            continue;
        }

        pKFi->mnBALocalForMerge = pMainKF->mnId; // 防止重复添加

        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;

        set spViewMPs = pKFi->GetMapPoints();
        for (MapPoint *pMPi : spViewMPs)
        {
            if (pMPi)
                if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap)

                    if (pMPi->mnBALocalForMerge != pMainKF->mnId)
                    {
                        vpMPs.push_back(pMPi);
                        pMPi->mnBALocalForMerge = pMainKF->mnId;
                    }
        }

        spKeyframeBA.insert(pKFi);
    }

    // Set non fixed Keyframe vertices
    // 3. 构建不固定关键帧的节点，并储存对应的MP
    set spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());
    for (Keyframe *pKFi : vpAdjustKF)
    {
        if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
            continue;

        pKFi->mnBALocalForKF = pMainKF->mnId; // 防止重复添加

        g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->mnId);
        optimizer.addVertex(vSE3);
        if (pKFi->mnId > maxKFid)
            maxKFid = pKFi->mnId;

        set spViewMPs = pKFi->GetMapPoints();
        for (MapPoint *pMPi : spViewMPs)
        {
            if (pMPi)
            {
                if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap)
                {
                    if (pMPi->mnBALocalForMerge != pMainKF->mnId)
                    {
                        vpMPs.push_back(pMPi);
                        pMPi->mnBALocalForMerge = pMainKF->mnId;
                    }
                }
            }
        }

        spKeyframeBA.insert(pKFi);
    }

    // 准备存放边的vector
    const int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size();

    vector vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    vector vpEdgesStereo;
    vpEdgesStereo.reserve(nExpectedSize);

    vector vpEdgeKFStereo;
    vpEdgeKFStereo.reserve(nExpectedSize);

    vector vpMapPointEdgeStereo;
    vpMapPointEdgeStereo.reserve(nExpectedSize);

    const float thHuber2D = sqrt(5.99);
    const float thHuber3D = sqrt(7.815);

    // Set MapPoint vertices
    map mpObsKFs;      // 统计每个关键帧对应的MP点数，调试输出用
    map mpObsFinalKFs; // 统计每个MP对应的关键帧数，调试输出用
    map mpObsMPs;      // 统计每个MP被观测的图片数，双目就是两个，调试输出用

    // 4. 确定MP节点与边的连接
    for (unsigned int i = 0; i < vpMPs.size(); ++i)
    {
        MapPoint *pMPi = vpMPs[i];
        if (pMPi->isBad())
            continue;

        g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMPi->GetWorldPos()));
        const int id = pMPi->mnId + maxKFid + 1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        const map> observations = pMPi->GetObservations();
        int nEdges = 0;
        // SET EDGES
        for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
        {

            Keyframe *pKF = mit->first;
            // 跳过的条件
            // 1. 帧坏了
            // 2. 帧靠后
            // 3. 不在参与优化帧的里面
            // 4. 在左相机上不存在这个三维点
            if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))
                continue;

            nEdges++;

            const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];

            // 注意！！！这里没有考虑相机2
            if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular
            {
                mpObsMPs[pMPi]++;
                Eigen::Matrix obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);

                g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuber2D);

                e->pCamera = pKF->mpCamera;

                optimizer.addEdge(e);

                vpEdgesMono.push_back(e);
                vpEdgeKFMono.push_back(pKF);
                vpMapPointEdgeMono.push_back(pMPi);

                mpObsKFs[pKF]++;
            }
            else // RGBD or Stereo
            {
                mpObsMPs[pMPi] += 2;
                Eigen::Matrix obs;
                const float kp_ur = pKF->mvuRight[get<0>(mit->second)];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;
                e->setInformation(Info);

                g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuber3D);

                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;
                e->bf = pKF->mbf;

                optimizer.addEdge(e);

                vpEdgesStereo.push_back(e);
                vpEdgeKFStereo.push_back(pKF);
                vpMapPointEdgeStereo.push_back(pMPi);

                mpObsKFs[pKF]++;
            }
        }
    }

    // 这段没啥用，调试输出的，暂时不看
    map mStatsObs;
    for (map::iterator it = mpObsMPs.begin(); it != mpObsMPs.end(); ++it)
    {
        MapPoint *pMPi = it->first;
        int numObs = it->second;

        mStatsObs[numObs]++;
    }

    if (pbStopFlag)
        if (*pbStopFlag)
            return;
    // 5. 优化
    optimizer.initializeOptimization();
    optimizer.optimize(5);

    bool bDoMore = true;

    if (pbStopFlag)
        if (*pbStopFlag)
            bDoMore = false;
    // 6. 剔除误差大的边
    map mWrongObsKF;
    if (bDoMore)
    {

        // Check inlier observations
        int badMonoMP = 0, badStereoMP = 0;
        for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
        {
            ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
            MapPoint *pMP = vpMapPointEdgeMono[i];

            if (pMP->isBad())
                continue;

            if (e->chi2() > 5.991 || !e->isDepthPositive())
            {
                e->setLevel(1);
                badMonoMP++;
            }

            e->setRobustKernel(0);
        }

        for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
        {
            g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
            MapPoint *pMP = vpMapPointEdgeStereo[i];

            if (pMP->isBad())
                continue;

            if (e->chi2() > 7.815 || !e->isDepthPositive())
            {
                e->setLevel(1);
                badStereoMP++;
            }

            e->setRobustKernel(0);
        }
        Verbose::PrintMess("LBA: First optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);

        // Optimize again without the outliers

        optimizer.initializeOptimization(0);
        optimizer.optimize(10);
    }

    // 7. 删除误差大的点与帧的连接关系
    vector> vToErase;
    vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
    set spErasedMPs;
    set spErasedKFs;

    // Check inlier observations
    int badMonoMP = 0, badStereoMP = 0;
    for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    {
        ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
        MapPoint *pMP = vpMapPointEdgeMono[i];

        if (pMP->isBad())
            continue;

        if (e->chi2() > 5.991 || !e->isDepthPositive())
        {
            Keyframe *pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi, pMP));
            mWrongObsKF[pKFi->mnId]++;
            badMonoMP++;

            spErasedMPs.insert(pMP);
            spErasedKFs.insert(pKFi);
        }
    }

    for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    {
        g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
        MapPoint *pMP = vpMapPointEdgeStereo[i];

        if (pMP->isBad())
            continue;

        if (e->chi2() > 7.815 || !e->isDepthPositive())
        {
            Keyframe *pKFi = vpEdgeKFStereo[i];
            vToErase.push_back(make_pair(pKFi, pMP));
            mWrongObsKF[pKFi->mnId]++;
            badStereoMP++;

            spErasedMPs.insert(pMP);
            spErasedKFs.insert(pKFi);
        }
    }
    Verbose::PrintMess("LBA: Second optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG);

    // Get Map Mutex
    unique_lock lock(pMainKF->GetMap()->mMutexMapUpdate);

    if (!vToErase.empty())
    {
        map mpMPs_in_KF;
        for (Keyframe *pKFi : spErasedKFs)
        {
            int num_MPs = pKFi->GetMapPoints().size();
            mpMPs_in_KF[pKFi] = num_MPs;
        }
        // 剔除链接关系，其他均为调试
        Verbose::PrintMess("LBA: There are " + to_string(vToErase.size()) + " observations whose will be deleted from the map", Verbose::VERBOSITY_DEBUG);
        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);
        }

        Verbose::PrintMess("LBA: " + to_string(spErasedMPs.size()) + " MPs had deleted observations", Verbose::VERBOSITY_DEBUG);
        Verbose::PrintMess("LBA: Current map is " + to_string(pMainKF->GetMap()->GetId()), Verbose::VERBOSITY_DEBUG);
        int numErasedMP = 0;
        for (MapPoint *pMPi : spErasedMPs)
        {
            if (pMPi->isBad())
            {
                Verbose::PrintMess("LBA: MP " + to_string(pMPi->mnId) + " has lost almost all the observations, its origin map is " + to_string(pMPi->mnOriginMapId), Verbose::VERBOSITY_DEBUG);
                numErasedMP++;
            }
        }
        Verbose::PrintMess("LBA: " + to_string(numErasedMP) + " MPs had deleted from the map", Verbose::VERBOSITY_DEBUG);

        for (Keyframe *pKFi : spErasedKFs)
        {
            int num_MPs = pKFi->GetMapPoints().size();
            int num_init_MPs = mpMPs_in_KF[pKFi];
            Verbose::PrintMess("LBA: Initially KF " + to_string(pKFi->mnId) + " had " + to_string(num_init_MPs) + ", at the end has " + to_string(num_MPs), Verbose::VERBOSITY_DEBUG);
        }
    }
    // 没用，调试的
    for (unsigned int i = 0; i < vpMPs.size(); ++i)
    {
        MapPoint *pMPi = vpMPs[i];
        if (pMPi->isBad())
            continue;

        const map> observations = pMPi->GetObservations();
        for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
        {

            Keyframe *pKF = mit->first;
            if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second)))
                continue;

            const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)];

            if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular
            {
                mpObsFinalKFs[pKF]++;
            }
            else // RGBD or Stereo
            {

                mpObsFinalKFs[pKF]++;
            }
        }
    }

    // Recover optimized data
    // 8. 取出结果
    // Keyframes
    for (Keyframe *pKFi : vpAdjustKF)
    {
        if (pKFi->isBad())
            continue;
        // 8.1 取出对应位姿
        g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKFi->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        cv::Mat Tiw = Converter::toCvMat(SE3quat);

        // 统计调试用
        int numMonoBadPoints = 0, numMonoOptPoints = 0;
        int numStereoBadPoints = 0, numStereoOptPoints = 0;
        vector vpMonoMPsOpt, vpStereoMPsOpt; // 存放mp内点
        vector vpMonoMPsBad, vpStereoMPsBad; // 存放mp外点
        // 8.2 卡方检验，调试用的貌似没别的作用
        for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
        {
            ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
            MapPoint *pMP = vpMapPointEdgeMono[i];
            Keyframe *pKFedge = vpEdgeKFMono[i];

            if (pKFi != pKFedge)
            {
                continue;
            }

            if (pMP->isBad())
                continue;

            if (e->chi2() > 5.991 || !e->isDepthPositive())
            {
                numMonoBadPoints++;
                vpMonoMPsBad.push_back(pMP);
            }
            else
            {
                numMonoOptPoints++;
                vpMonoMPsOpt.push_back(pMP);
            }
        }

        for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
        {
            g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
            MapPoint *pMP = vpMapPointEdgeStereo[i];
            Keyframe *pKFedge = vpEdgeKFMono[i];

            if (pKFi != pKFedge)
            {
                continue;
            }

            if (pMP->isBad())
                continue;

            if (e->chi2() > 7.815 || !e->isDepthPositive())
            {
                numStereoBadPoints++;
                vpStereoMPsBad.push_back(pMP);
            }
            else
            {
                numStereoOptPoints++;
                vpStereoMPsOpt.push_back(pMP);
            }
        }

        if (numMonoOptPoints + numStereoOptPoints < 50)
        {
            Verbose::PrintMess("LBA ERROR: KF " + to_string(pKFi->mnId) + " has only " + to_string(numMonoOptPoints) + " monocular and " + to_string(numStereoOptPoints) + " stereo points", Verbose::VERBOSITY_DEBUG);
        }
        // 设置位姿
        pKFi->SetPose(Tiw);
    }

    // 更新点的坐标
    for (MapPoint *pMPi : vpMPs)
    {
        if (pMPi->isBad())
            continue;

        g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMPi->mnId + maxKFid + 1));
        pMPi->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMPi->UpdateNormalAndDepth();
    }
}

LoopClosing::MergeLocal() 融合地图时使用，优化当前帧没有参与融合的元素，本质图优化
优化目标：

1. vpNonFixedKFs
2. 2.vpNonCorrectedMPs

void Optimizer::OptimizeEssentialGraph(Keyframe *pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs,
                                        vector &vpNonFixedKFs, vector &vpNonCorrectedMPs)
{
    // 1. 优化器构建
    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(false);
    g2o::BlockSolver_7_3::LinearSolverType *linearSolver =
        new g2o::LinearSolverEigen();
    g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    solver->setUserLambdaInit(1e-16);
    optimizer.setAlgorithm(solver);

    Map *pMap = pCurKF->GetMap();
    const unsigned int nMaxKFid = pMap->GetMaxKFid();

    vector> vScw(nMaxKFid + 1);          // 存放每一帧优化前的sim3
    vector> vCorrectedSwc(nMaxKFid + 1); // 存放每一帧优化后的sim3，调试输出用，没有实际作用
    vector vpVertices(nMaxKFid + 1);                           // 存放节点，没用，还占地方

    const int minFeat = 100; // pKFi->GetCovisiblesByWeight(minFeat);  essentialgraph 阈值就是共视大于100

    // 2. 确定固定关键帧的节点
    // vpMergeConnectedKFs 融合地图中的关键帧
    for (Keyframe *pKFi : vpFixedKFs)
    {
        if (pKFi->isBad())
            continue;

        g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();

        const int nIDi = pKFi->mnId;

        Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation());
        Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation());
        g2o::Sim3 Siw(Rcw, tcw, 1.0);
        vScw[nIDi] = Siw;
        vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用
        VSim3->setEstimate(Siw);

        VSim3->setFixed(true);

        VSim3->setId(nIDi);
        VSim3->setMarginalized(false);
        VSim3->_fix_scale = true;

        optimizer.addVertex(VSim3);

        vpVertices[nIDi] = VSim3;
    }
    Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG);

    set sIdKF;
    // vpLocalCurrentWindowKFs 当前地图中经过矫正的关键帧
    for (Keyframe *pKFi : vpFixedCorrectedKFs)
    {
        if (pKFi->isBad())
            continue;

        g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();

        const int nIDi = pKFi->mnId;

        Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation());
        Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation());
        g2o::Sim3 Siw(Rcw, tcw, 1.0);
        vCorrectedSwc[nIDi] = Siw.inverse(); // This KFs mustn't be corrected 无实际作用
        VSim3->setEstimate(Siw);

        cv::Mat Tcw_bef = pKFi->mTcwBefMerge;
        Eigen::Matrix Rcw_bef = Converter::toMatrix3d(Tcw_bef.rowRange(0, 3).colRange(0, 3));
        Eigen::Matrix tcw_bef = Converter::toVector3d(Tcw_bef.rowRange(0, 3).col(3));
        vScw[nIDi] = g2o::Sim3(Rcw_bef, tcw_bef, 1.0);

        VSim3->setFixed(true);

        VSim3->setId(nIDi);
        VSim3->setMarginalized(false);

        optimizer.addVertex(VSim3);

        vpVertices[nIDi] = VSim3;

        sIdKF.insert(nIDi);
    }
    Verbose::PrintMess("Opt_Essential: vpFixedCorrectedKFs loaded", Verbose::VERBOSITY_DEBUG);

    // 3. 确定待优化的关键帧节点
    for (Keyframe *pKFi : vpNonFixedKFs)
    {
        if (pKFi->isBad())
            continue;

        const int nIDi = pKFi->mnId;

        if (sIdKF.count(nIDi)) // It has already added in the corrected merge KFs
            continue;

        g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap();

        Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation());
        Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation());
        g2o::Sim3 Siw(Rcw, tcw, 1.0);
        vScw[nIDi] = Siw;
        VSim3->setEstimate(Siw);

        VSim3->setFixed(false);

        VSim3->setId(nIDi);
        VSim3->setMarginalized(false);

        optimizer.addVertex(VSim3);

        vpVertices[nIDi] = VSim3;

        sIdKF.insert(nIDi);
    }
    Verbose::PrintMess("Opt_Essential: vpNonFixedKFs loaded", Verbose::VERBOSITY_DEBUG);

    vector vpKFs;
    vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size());
    vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end());
    vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(), vpFixedCorrectedKFs.end());
    vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end());
    set spKFs(vpKFs.begin(), vpKFs.end());

    Verbose::PrintMess("Opt_Essential: List of KF loaded", Verbose::VERBOSITY_DEBUG);

    const Eigen::Matrix matLambda = Eigen::Matrix::Identity();

    // 4. 遍历所有帧
    for (Keyframe *pKFi : vpKFs)
    {
        int num_connections = 0; // 统计与pKFi连接的数量
        const int nIDi = pKFi->mnId;

        g2o::Sim3 Swi = vScw[nIDi].inverse();

        Keyframe *pParentKFi = pKFi->GetParent();

        // Spanning tree edge
        // 4.1 找到pKFi的父帧且在这批关键帧里面，添加与其关联的sim3边，相对约束
        if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end())
        {
            int nIDj = pParentKFi->mnId;

            g2o::Sim3 Sjw = vScw[nIDj];

            g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3 *e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi)));
            e->setMeasurement(Sji);

            e->information() = matLambda;
            optimizer.addEdge(e);
            num_connections++;
        }

        // Loop edges
        // 4.2 添加回环的边
        const set sLoopEdges = pKFi->GetLoopEdges();
        for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++)
        {
            Keyframe *pLKF = *sit;
            if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId)
            {
                g2o::Sim3 Slw = vScw[pLKF->mnId];

                g2o::Sim3 Sli = Slw * Swi;
                g2o::EdgeSim3 *el = new g2o::EdgeSim3();
                el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId)));
                el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi)));
                el->setMeasurement(Sli);
                el->information() = matLambda;
                optimizer.addEdge(el);
                num_connections++;
            }
        }

        // Covisibility graph edges
        // 4.3 建立essentialgraph（共视边）
        const vector vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat);
        for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++)
        {
            Keyframe *pKFn = *vit;
            // 1.这个帧存在且不是pKFi的父帧，防止重复添加
            // 2.pKFn不为pKFi的子帧
            // 3.pKFn不为回环帧
            // 4.pKFn要在这批关键帧里面
            // 防止重复添加
            if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end())
            {
                if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId)
                {

                    g2o::Sim3 Snw = vScw[pKFn->mnId];

                    g2o::Sim3 Sni = Snw * Swi;

                    g2o::EdgeSim3 *en = new g2o::EdgeSim3();
                    en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId)));
                    en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi)));
                    en->setMeasurement(Sni);
                    en->information() = matLambda;
                    optimizer.addEdge(en);
                    num_connections++;
                }
            }
        }
    }

    // Optimize!
    // 5. 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(20);

    unique_lock lock(pMap->mMutexMapUpdate);

    // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
    // 6. 取出结果
    for (Keyframe *pKFi : vpNonFixedKFs)
    {
        if (pKFi->isBad())
            continue;

        const int nIDi = pKFi->mnId;

        g2o::VertexSim3Expmap *VSim3 = static_cast(optimizer.vertex(nIDi));
        g2o::Sim3 CorrectedSiw = VSim3->estimate();
        vCorrectedSwc[nIDi] = CorrectedSiw.inverse();
        Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
        Eigen::Vector3d eigt = CorrectedSiw.translation();
        double s = CorrectedSiw.scale();

        eigt *= (1. / s); //[R t/s;0 1]

        cv::Mat Tiw = Converter::toCvSE3(eigR, eigt);

        pKFi->mTcwBefMerge = pKFi->GetPose();
        pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
        pKFi->SetPose(Tiw);
    }

    // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
    for (MapPoint *pMPi : vpNonCorrectedMPs)
    {
        if (pMPi->isBad())
            continue;

        Keyframe *pRefKF = pMPi->GetReferenceKeyframe();
        g2o::Sim3 Srw;
        g2o::Sim3 correctedSwr;
        while (pRefKF->isBad())
        {
            if (!pRefKF)
            {
                Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG);
                break;
            }

            pMPi->EraseObservation(pRefKF);
            pRefKF = pMPi->GetReferenceKeyframe();
        }

        // 流程就是转到相机坐标系，在用新的rt转到世界
        cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge;
        Eigen::Matrix RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0, 3).colRange(0, 3));
        Eigen::Matrix tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0, 3).col(3));
        Srw = g2o::Sim3(RNonCorrectedwr, tNonCorrectedwr, 1.0).inverse();

        cv::Mat Twr = pRefKF->GetPoseInverse();
        Eigen::Matrix Rwr = Converter::toMatrix3d(Twr.rowRange(0, 3).colRange(0, 3));
        Eigen::Matrix twr = Converter::toVector3d(Twr.rowRange(0, 3).col(3));
        correctedSwr = g2o::Sim3(Rwr, twr, 1.0);

        cv::Mat P3Dw = pMPi->GetWorldPos();
        Eigen::Matrix eigP3Dw = Converter::toVector3d(P3Dw);
        Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));

        cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
        pMPi->SetWorldPos(cvCorrectedP3Dw);

        pMPi->UpdateNormalAndDepth();
    }
}

这里面进行visual inertial ba
LoopClosing::MergeLocal2 中用到
优化目标：

• 相关帧的位姿，速度，偏置，还有涉及点的坐标，可以理解为跨地图的局部窗口优化

void Optimizer::MergeInertialBA(Keyframe *pCurrKF, Keyframe *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyframeAndPose &corrPoses)
{
    const int Nd = 6;
    const unsigned long maxKFid = pCurrKF->mnId;
    // 1. 准备所有被优化的关键帧, 完全固定的帧
    // 被优化的帧, 当前帧和融合匹配帧加起来一共12个
    vector vpOptimizableKFs;
    vpOptimizableKFs.reserve(2 * Nd);

    // For cov KFS, inertial parameters are not optimized
    // 共视帧, 只优化位姿，不优化速度与偏置
    const int maxCovKF = 15;
    vector vpOptimizableCovKFs;
    vpOptimizableCovKFs.reserve(2 * maxCovKF);

    // Add sliding window for current KF
    // 当前关键帧先加到容器中
    vpOptimizableKFs.push_back(pCurrKF);
    // 后面用这个变量避免重复
    pCurrKF->mnBALocalForKF = pCurrKF->mnId;
    // 添加5个与当前关键帧相连的时序上相连的关键帧进容器
    // 1.1 一直放一直放，一直放到没有上一个关键帧，这里面包含了pCurrKF最近的6帧，从晚到早排列，不过有可能存不满
    for (int i = 1; i < Nd; i++)
    {
        if (vpOptimizableKFs.back()->mPrevKF)
        {
            // 用mPrevKF访问时序上的上一个关键帧
            vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
            vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
        }
        else
            break;
    }

    // 1.2 如果 vpOptimizableKFs 中最早的一帧前面还有，往不更新惯导参数的序列中添加
    // 否则把最后一个取出来放到不更新惯导参数的序列中
    if (vpOptimizableKFs.back()->mPrevKF)
    {
        vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
        vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId;
    }
    else
    {
        // 如果没找到时序相连的帧,把被优化的窗口缩减一个给到固定的变量
        vpOptimizableCovKFs.push_back(vpOptimizableKFs.back());
        vpOptimizableKFs.pop_back();
    }

    // 没用到
    Keyframe *pKF0 = vpOptimizableCovKFs.back();
    cv::Mat Twc0 = pKF0->GetPoseInverse();

    // Add temporal neighbours to merge KF (previous and next KFs)
    // 2.  同样, 对于融合帧也把它和时序上的几个邻居加到可优化的容器里
    // 2.1 把匹配的融合关键帧也放进来准备一起优化
    vpOptimizableKFs.push_back(pMergeKF);
    pMergeKF->mnBALocalForKF = pCurrKF->mnId;

    // Previous KFs
    // 把融合帧时序上的邻居添加到可优化的容器里, 因为融合帧左右都有时序上的邻居,所以这里先取一半 Nd/2
    // 2.2 再放进来3个pMergeKF之前的关键帧，有可能放不满
    for (int i = 1; i < (Nd / 2); i++)
    {
        if (vpOptimizableKFs.back()->mPrevKF)
        {
            vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
            vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
        }
        else
            break;
    }

    // We fix just once the old map
    // 记录与融合帧窗口时序上紧邻的帧为完全固定的帧
    // 2.3 类似于上面的做法如果有前一个关键帧放入lFixedKeyframes，否则从vpOptimizableKFs取出，注意这里防止重复添加的标识又多了一个变量
    // 记录完全固定的帧
    list lFixedKeyframes;
    if (vpOptimizableKFs.back()->mPrevKF)
    {
        lFixedKeyframes.push_back(vpOptimizableKFs.back()->mPrevKF);
        vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pCurrKF->mnId;
    }
    // 如果没找到,则从窗口内拿出一帧给到完全固定的帧
    else
    {
        vpOptimizableKFs.back()->mnBALocalForKF = 0;
        vpOptimizableKFs.back()->mnBAFixedForKF = pCurrKF->mnId;
        lFixedKeyframes.push_back(vpOptimizableKFs.back());
        vpOptimizableKFs.pop_back();
    }

    // Next KFs
    // 2.4 再添加一个pMergeKF的下一个关键帧
    if (pMergeKF->mNextKF)
    {
        vpOptimizableKFs.push_back(pMergeKF->mNextKF);
        vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
    }
    // 2.5 数量不够时，添加最后一个的下一帧。继续添加直到达到2Nd个可优化关键帧,或没有新的可以添加了
    while (vpOptimizableKFs.size() < (2 * Nd))
    {
        if (vpOptimizableKFs.back()->mNextKF)
        {
            vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mNextKF);
            vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId;
        }
        else
            break;
    }

    int N = vpOptimizableKFs.size();

    // Optimizable points seen by optimizable keyframes
    // 3. 帧弄完了该弄点了，将优化的帧的点存入lLocalMapPoints (所有被可优化关键帧看到的点)

    // 添加用来优化的地图点
    list lLocalMapPoints;

    // 统计了在这些帧中点被观测的次数
    map mLocalObs;
    // 遍历所有可优化的关键帧
    for (int i = 0; i < N; i++)
    {
        vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches();
        // 遍历每个关键帧所有的地图点
        for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++)
        {
            // Using mnBALocalForKF we avoid redundance here, one MP can not be added several times to lLocalMapPoints
            MapPoint *pMP = *vit;
            if (pMP)
                if (!pMP->isBad())
                    // 用这个变量记录是否重复添加
                    if (pMP->mnBALocalForKF != pCurrKF->mnId)
                    {
                        mLocalObs[pMP] = 1;
                        lLocalMapPoints.push_back(pMP);
                        pMP->mnBALocalForKF = pCurrKF->mnId; // 防止重复添加
                    }
                    else
                        mLocalObs[pMP]++;
        }
    }

    // 4. 把pCurrKF的共视帧加进来 只优化位姿，不优化速度与偏置
    // 固定所有观察到地图点,但没有被加到优化变量中的关键帧
    int i = 0;
    const int min_obs = 10;
    vector vNeighCurr = pCurrKF->GetCovisiblesByWeight(min_obs);
    // 遍历所有的pair<地图点指针,观测次数>
    for (vector::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++)
    {
        // 拿到所有的观测
        // 上限，添加15个
        if (i >= maxCovKF)
            break;
        Keyframe *pKFi = *lit;

        if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...
        {
            pKFi->mnBALocalForKF = pCurrKF->mnId;
            if (!pKFi->isBad())
            {
                i++;
                vpOptimizableCovKFs.push_back(pKFi);
            }
        }
    }

    i = 0;
    // 把pMergeKF的共视帧加进来 只优化位姿，不优化速度与偏置
    // BUG ！！！！！这里不知道是bug还是啥vNeighCurr是不是应该改成vNeighMerge
    // vpOptimizableCovKFs 改成 lFixedKeyframes
    vector vNeighMerge = pMergeKF->GetCovisiblesByWeight(min_obs);
    for (vector::iterator lit = vNeighCurr.begin(), lend = vNeighCurr.end(); lit != lend; lit++, i++)
    {
        // 上限，添加15个
        if (i >= maxCovKF)
            break;
        Keyframe *pKFi = *lit;

        // 如果还没有被添加到共视帧容器里
        if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included...
        {
            pKFi->mnBALocalForKF = pCurrKF->mnId;
            if (!pKFi->isBad())
            {
                i++;
                vpOptimizableCovKFs.push_back(pKFi);
            }
        }
    }

    // 5. 构建优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolverX::LinearSolverType *linearSolver;
    linearSolver = new g2o::LinearSolverEigen();

    g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);

    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    solver->setUserLambdaInit(1e3);

    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(false);

    // Set Local Keyframe vertices
    N = vpOptimizableKFs.size();
    // 5.1 设置所有的可优化关键帧顶点
    for (int i = 0; i < N; i++)
    {
        Keyframe *pKFi = vpOptimizableKFs[i];

        VertexPose *VP = new VertexPose(pKFi);
        VP->setId(pKFi->mnId);
        VP->setFixed(false);
        // 位姿
        optimizer.addVertex(VP);

        if (pKFi->bImu)
        {
            VertexVelocity *VV = new VertexVelocity(pKFi);
            VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
            VV->setFixed(false);
            optimizer.addVertex(VV);
            VertexGyroBias *VG = new VertexGyroBias(pKFi);
            VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
            VG->setFixed(false);
            optimizer.addVertex(VG);
            VertexAccBias *VA = new VertexAccBias(pKFi);
            VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
            VA->setFixed(false);
            optimizer.addVertex(VA);
        }
    }

    // Set Local cov keyframes vertices
    // 5.2 只优化位姿，不优化速度与偏置
    int Ncov = vpOptimizableCovKFs.size();
    for (int i = 0; i < Ncov; i++)
    {
        Keyframe *pKFi = vpOptimizableCovKFs[i];

        VertexPose *VP = new VertexPose(pKFi);
        VP->setId(pKFi->mnId);
        VP->setFixed(false);
        optimizer.addVertex(VP);

        if (pKFi->bImu)
        {
            VertexVelocity *VV = new VertexVelocity(pKFi);
            VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
            VV->setFixed(true);
            optimizer.addVertex(VV);
            VertexGyroBias *VG = new VertexGyroBias(pKFi);
            VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
            VG->setFixed(true);
            optimizer.addVertex(VG);
            VertexAccBias *VA = new VertexAccBias(pKFi);
            VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
            VA->setFixed(true);
            optimizer.addVertex(VA);
        }
    }

    // Set Fixed Keyframe vertices
    // 5.3 设置所有完全固定的关键帧顶点
    for (list::iterator lit = lFixedKeyframes.begin(), lend = lFixedKeyframes.end(); lit != lend; lit++)
    {
        Keyframe *pKFi = *lit;
        VertexPose *VP = new VertexPose(pKFi);
        VP->setId(pKFi->mnId);
        VP->setFixed(true);
        optimizer.addVertex(VP);

        if (pKFi->bImu)
        {
            VertexVelocity *VV = new VertexVelocity(pKFi);
            VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
            VV->setFixed(true);
            optimizer.addVertex(VV);
            VertexGyroBias *VG = new VertexGyroBias(pKFi);
            VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
            VG->setFixed(true);
            optimizer.addVertex(VG);
            VertexAccBias *VA = new VertexAccBias(pKFi);
            VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
            VA->setFixed(true);
            optimizer.addVertex(VA);
        }
    }

    // 6 设置所有的inertial的边
    // Create intertial constraints
    // 预积分边
    vector vei(N, (EdgeInertial *)NULL);
    // 角速度bias边
    vector vegr(N, (EdgeGyroRW *)NULL);
    // 加速地bias边
    vector vear(N, (EdgeAccRW *)NULL);

    // 6.1 第一阶段优化vpOptimizableKFs里面的帧，遍历所有可优化的关键帧
    for (int i = 0; i < N; i++)
    {
        // cout << "inserting inertial edge " << i << endl;
        Keyframe *pKFi = vpOptimizableKFs[i];

        if (!pKFi->mPrevKF)
        {
            Verbose::PrintMess("NOT INERTIAL link TO PREVIOUS frame!!!!", Verbose::VERBOSITY_NORMAL);
            continue;
        }
        if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated)
        {
            pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
            // 位姿
            g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
            // 速度
            g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1);
            // 角速度bias
            g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);
            // 加速度bias
            g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3);
            // 同上
            g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
            g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1);
            g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2);
            g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3);

            if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
            {
                cerr << "Error " << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;
                continue;
            }

            vei[i] = new EdgeInertial(pKFi->mpImuPreintegrated);
            // 设置顶点 2*Pose + 2*Velocity + 1 角速度bias + 1 加速度bias
            vei[i]->setVertex(0, dynamic_cast(VP1));
            vei[i]->setVertex(1, dynamic_cast(VV1));
            vei[i]->setVertex(2, dynamic_cast(VG1));
            vei[i]->setVertex(3, dynamic_cast(VA1));
            vei[i]->setVertex(4, dynamic_cast(VP2));
            vei[i]->setVertex(5, dynamic_cast(VV2));

            // TODO Uncomment
            // 设置优化参数
            g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
            vei[i]->setRobustKernel(rki);
            rki->setDelta(sqrt(16.92));
            // 添加边
            optimizer.addEdge(vei[i]);

            // 角速度bias的边
            vegr[i] = new EdgeGyroRW();
            // 设置顶点两个角速度bias
            vegr[i]->setVertex(0, VG1);
            vegr[i]->setVertex(1, VG2);
            // 设置infomation matrix
            cv::Mat cvInfoG = pKFi->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);
            Eigen::Matrix3d InfoG;

            for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++)
                    InfoG(r, c) = cvInfoG.at(r, c);
            vegr[i]->setInformation(InfoG);
            optimizer.addEdge(vegr[i]);
            // 设置加速度的边
            vear[i] = new EdgeAccRW();
            vear[i]->setVertex(0, VA1);
            vear[i]->setVertex(1, VA2);
            // 设置information matrix
            cv::Mat cvInfoA = pKFi->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);
            Eigen::Matrix3d InfoA;
            for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++)
                    InfoA(r, c) = cvInfoA.at(r, c);
            vear[i]->setInformation(InfoA);
            optimizer.addEdge(vear[i]);
        }
        else
            Verbose::PrintMess("ERROR building inertial edge", Verbose::VERBOSITY_NORMAL);
    }

    Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_DEBUG);

    // Set MapPoint vertices
    // 6.2 设置所有地图的顶点
    // 设置地图点顶点
    const int nExpectedSize = (N + Ncov + lFixedKeyframes.size()) * lLocalMapPoints.size();

    // 对于双目单目设置不同
    // Mono
    vector vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    // Stereo
    vector vpEdgesStereo;
    vpEdgesStereo.reserve(nExpectedSize);

    vector vpEdgeKFStereo;
    vpEdgeKFStereo.reserve(nExpectedSize);

    vector vpMapPointEdgeStereo;
    vpMapPointEdgeStereo.reserve(nExpectedSize);

    const float thHuberMono = sqrt(5.991);
    const float chi2Mono2 = 5.991;
    const float thHuberStereo = sqrt(7.815);
    const float chi2Stereo2 = 7.815;

    const unsigned long iniMPid = maxKFid * 5;

    // 遍历所有被可优化关键帧观测到的的地图点
    // 7. 添加重投影的边
    for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
    {
        MapPoint *pMP = *lit;
        if (!pMP)
            continue;

        g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));

        unsigned long id = pMP->mnId + iniMPid + 1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        // 添加顶点
        optimizer.addVertex(vPoint);

        const map> observations = pMP->GetObservations();

        // Create visual constraints
        // 添加重投影边
        for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
        {
            Keyframe *pKFi = mit->first;

            if (!pKFi)
                continue;

            if ((pKFi->mnBALocalForKF != pCurrKF->mnId) && (pKFi->mnBAFixedForKF != pCurrKF->mnId))
                continue;

            if (pKFi->mnId > maxKFid)
            {
                Verbose::PrintMess("ID greater than current KF is", Verbose::VERBOSITY_NORMAL);
                continue;
            }

            if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL)
                continue;

            if (!pKFi->isBad())
            {
                // 3D点的观测
                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)];
                // 如果是单目观测
                if (pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation
                {
                    // 投影
                    Eigen::Matrix obs;
                    obs << kpUn.pt.x, kpUn.pt.y;

                    EdgeMono *e = new EdgeMono();
                    // 设置边的顶点
                    // 3D点
                    e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
                    // 关键帧位姿
                    e->setVertex(1, dynamic_cast(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);
                    // 添加边
                    optimizer.addEdge(e);
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                //双目
                else // stereo observation
                {
                    const float kp_ur = pKFi->mvuRight[get<0>(mit->second)];
                    Eigen::Matrix obs;
                    obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                    EdgeStereo *e = new EdgeStereo();

                    e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                    e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);

                    g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuberStereo);

                    optimizer.addEdge(e);
                    vpEdgesStereo.push_back(e);
                    vpEdgeKFStereo.push_back(pKFi);
                    vpMapPointEdgeStereo.push_back(pMP);
                }
            }
        }
    }
    // 如果要停止就直接返回
    if (pbStopFlag)
        if (*pbStopFlag)
            return;
    // 8. 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(3);
    if (pbStopFlag)
        if (!*pbStopFlag)
            optimizer.optimize(5);

    optimizer.setForceStopFlag(pbStopFlag);

    // 更具优化结果挑选删除外点
    vector> vToErase;
    vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());

    // Check inlier observations
    // Mono

    // 9. 处理外点
    // 更具卡方检测来记录要删除的外点
    for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    {
        EdgeMono *e = vpEdgesMono[i];
        MapPoint *pMP = vpMapPointEdgeMono[i];

        if (pMP->isBad())
            continue;

        if (e->chi2() > chi2Mono2)
        {
            Keyframe *pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi, pMP));
        }
    }

    // Stereo
    for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
    {
        EdgeStereo *e = vpEdgesStereo[i];
        MapPoint *pMP = vpMapPointEdgeStereo[i];

        if (pMP->isBad())
            continue;

        if (e->chi2() > chi2Stereo2)
        {
            Keyframe *pKFi = vpEdgeKFStereo[i];
            vToErase.push_back(make_pair(pKFi, pMP));
        }
    }

    // Get Map Mutex and erase outliers
    // 移除外点
    unique_lock lock(pMap->mMutexMapUpdate);
    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);
        }
    }

    // Recover optimized data
    // 10. 取数
    // 根据优化的结果,修改每个被优化的变量
    // Keyframes
    // 对于每个可优化关键帧
    for (int i = 0; i < N; i++)
    {
        Keyframe *pKFi = vpOptimizableKFs[i];

        // 修改位姿
        VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId));
        cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
        pKFi->SetPose(Tcw);

        // 在corrPoses记录每个关键帧融合后的位姿
        cv::Mat Tiw = pKFi->GetPose();
        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);
        corrPoses[pKFi] = g2oSiw;

        if (pKFi->bImu)
        {
            // 速度
            VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));
            // 修改速度
            pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
            // 角速度bias
            VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));
            // 加速度bias
            VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));
            Vector6d b;
            // 修改bias
            b << VG->estimate(), VA->estimate();
            pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));
        }
    }
    // 遍历所有的共视帧
    for (int i = 0; i < Ncov; i++)
    {
        Keyframe *pKFi = vpOptimizableCovKFs[i];

        VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId));
        cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
        // 修改位姿
        pKFi->SetPose(Tcw);

        // 记录融合后的位姿
        cv::Mat Tiw = pKFi->GetPose();
        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);
        corrPoses[pKFi] = g2oSiw;

        // 这部分固定了，跟没做一样~
        if (pKFi->bImu)
        {
            VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));
            pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
            VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));
            VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));
            Vector6d b;
            b << VG->estimate(), VA->estimate();
            pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]));
        }
    }

    // Points
    // 对于所有的地图点
    for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++)
    {
        // 跟新位置和normal等信息
        MapPoint *pMP = *lit;
        g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1));
        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }

    pMap->IncreaseChangeIndex();
}