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

一、前言

在上一篇博客中，为大家讲解了 Optimizer::PoseOptimization→仅位姿优化，同时还分享了几篇g2o(图优化)博客，这篇博客讲 Optimizer::LocalBundleAdjustment，其在局部建图线程中被调，即 src/LocalMapping.cc 文件中 LocalMapping::Run() 函数。

首先需要明白，优化的目标是→局部关键帧的位姿与地图点。如下图所示：

需要优化的的目标就是红色椭圆中的当前KF位姿、KF的地图点、能够观测到KF地图点的关键帧(称为一级相邻关键帧)。Optimizer::LocalBundleAdjustment 实现于于 src/Optimizer.cc 文件。核心要点介绍如下：

/*
 * @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 由局部建图线程调用,对局部地图进行优化的函数
 */

这里可以看到，这里定义使用的是 g2o::EdgeSE3ProjectXYZ() 、 EdgeStereoSE3ProjectXYZ()，其为二元边，上一篇博客 g2o::EdgeSE3ProjectXYZOnlyPose() 、g2o::EdgeStereoSE3ProjectXYZOnlyPose 为一元边，是不一样的。因为这里每条边(上图中的红线)都需要连接两个顶点，其顶点类型分别为 VertexSBAPointXYZ，VertexSE3Expmap。
 

二、顶点

上面已经提到，Optimizer::LocalBundleAdjustment 是有两种类型顶点需要优化，即 VertexSE3Expmap 与 VertexSBAPointXYZ，分别对应于 pKF(当前关键帧) 的一级相邻关键帧位姿，一级相邻关键帧能观测到的所有关键点。

VertexSE3Expmap

该顶点上一篇博客有详细的讲解，其继承于 BaseVertex，类定义如下：

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
    }
	......

参数6→SE3Quat类型为六维，三维旋转，三维平移。
参数SE3Quat →该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数。
 

VertexSBAPointXYZ

首先在 Thirdparty\g2o\g2o\types\types_sba.h 文件中，可以找到类定义代码如下：

 class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>{
    }
 	......

参数3→地图点为3三维坐标
参数Vector3d→空间点位置(地图点节点)为 Vector3d 类型

在添加 VertexSBAPointXYZ 类型顶点时，会使用到其中的 setMarginalized(true) 函数， 需要将所有的三维点边缘化掉，以便稀疏化求解。简单的说G2O 中对路标点设置边缘化(Point->setMarginalized(true))是为了 在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机位姿，反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化掉。
 

三、二元边

与之前一样，边的定义与用法相比于顶点更加复杂一些，前面提到 Optimizer::LocalBundleAdjustment 中需要使用到二元边 g2o::EdgeSE3ProjectXYZ() 、 EdgeStereoSE3ProjectXYZ()。他们的原理基本与用法基本是一致的，一个为单目，一个为双目，实现原理比较相近，这里只对 g2o::EdgeSE3ProjectXYZ() 进行讲解，代码实现于 Thirdparty\g2o\g2o\types\types_six_dof_expmap.cpp 文件中，先来看其继承关系：

EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {
    }
	......

参数D(2)：测量值(ORB特征点)的维度，有x,y轴像素坐标，所以为2
参数E(Vector2d)：测量值的数据类型。
参数VertexXi(VertexSBAPointXYZ)：边连接第一个顶点的类型。
参数VertexXj(VertexSE3Expmap)：边连接第二个顶点的类型。
注意：VertexX、VertexXj 是继承自BaseVertex等基础类的类！不是顶点的数据类。

通过一系列博客的学习，已经只要边中有两个比较重要的函数，分别为 virtual void computeError()、
virtual void linearizeOplus()。先来看看 virtual void computeError()，其代码实现如下：

  void computeError()  {
    
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(v2->estimate()));
  }

这里我粘贴一下上一篇博客中 EdgeSE3ProjectXYZOnlyPose 的 computeError() 函数如下：

  void computeError()  {
    
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(Xw));
  }

可以看到其是十分相近的，v1 都表示顶点(关键帧)。v2 与 Xw 都是地图点，只是 v2 以顶点的形式进行表示了。总而言之：

v1->estimate().map(v2->estimate());
v1->estimate().map(Xw);

实现的功能是一样的，就是把世界坐标系下的地图点，变换 v1(关键帧) 的相机坐标系下。然后再通过 cam_project 投影到像素坐标系上，最后在计算误差，即误差 = 关键点像素坐标(观测)- 地图点投影到关键点附近像素坐标(投影)。

那么接下来就是 virtual void linearizeOplus() 函数了，其同样实现于 Thirdparty\g2o\g2o\types\types_six_dof_expmap.cpp 文件中，搜索 void EdgeSE3ProjectXYZ::linearizeOplus() 即可。该内容已经在上篇博客中进行了详细分析，所以就不再重复了。

 

四、源码逻辑

在讲解了顶点与边的相关内容之后，再来看 Optimizer::LocalBundleAdjustment 实现流程，就十分简单了：

$(01)：$ 首先获得局部关键帧lLocalKeyFrames，主要包含：当前关键帧 KF、能够观测到 KF 地图点的所有关键帧，也就是一级相邻关键帧。然后获得局部关键帧所有地图点 lLocalMapPoints，不重复。

$(02)：$ 构造g2o优化器，选择列文伯格-马夸尔特算法(LM)算法，BlockSolver_6_3表示：位姿 _PoseDim 为6维，路标点 _LandmarkDim 是3维，设置外界优化停止标志 optimizer.setForceStopFlag(pbStopFlag);

$(03)：$ 添加待优化的位姿顶点→局部关键帧的位姿。另外设置不需要优化的位姿顶点，对于能够看到局部关键帧地图点，但是其又不是局部关键帧的位姿，设置为 setFixed(pKFi->mnId==0)，不参与优化。那么不优化为啥也要添加？回答：为了增加约束信息

$(04)：$ 遍历所有的局部地图点 ，为每个地图点 pMP 创建一个顶点 g2o::VertexSBAPointXYZ* vPoint，同时设置边 id = pMP->mnId+maxKFid+1。

$(05)：$ 获得能够观测到地图地图点 pMP 的所有局部关键帧，为地图点 pMP 与 每个局部关键帧创建一条二元边 g2o::EdgeSE3ProjectXYZ* e。

$(06)：$ 为二元边设置好连接的两个顶点， 第零个顶点是地图点，第二个顶点是观测到该地图点的关键帧，只是信息矩阵，也就是这条边的权重，权重为特征点所在图像金字塔的层数的倒数。另外还需要使用鲁棒核函数抑制外点，并且设置好相机内参。

$(07)：$ 开始BA前再次确认是否有外部请求停止优化，分成两个阶段开始优化。第一阶段优化迭代5次，如果有外部请求停止,那么就不在进行第二阶段的优化。

$(08)：$ 进行二阶段优化，根据卡方检验，如果当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，不优化了。第二阶段优化的时候就属于精求解了,所以就不使用核函数。排除误差较大的outlier后再次优化 ，即第二阶段优化，迭代10次。

$(09)：$ 在二阶段优化后重新计算误差，剔除连接误差比较大的关键帧和地图点。更新关键帧位姿以及地图点的位置、平均观测方向等属性。

 

五、源码注释

源码位于 src/Optimizer.cc 文件中

/*
 * @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)
{
    
    // 该优化函数用于LocalMapping线程的局部BA优化

    // Local KeyFrames: First Breadth Search from Current Keyframe
    // 局部关键帧
    list<KeyFrame*> lLocalKeyFrames;

    // Step 1 将当前关键帧及其共视关键帧加入局部关键帧
    lLocalKeyFrames.push_back(pKF);
    pKF->mnBALocalForKF = pKF->mnId;

    // 找到关键帧连接的共视关键帧（一级相连），加入局部关键帧中
    const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
    for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
    {
    
        KeyFrame* pKFi = vNeighKFs[i];

        // 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId，防止重复添加
        pKFi->mnBALocalForKF = pKF->mnId;

        // 保证该关键帧有效才能加入
        if(!pKFi->isBad())      
            lLocalKeyFrames.push_back(pKFi);
    }

    // Local MapPoints seen in Local KeyFrames
    // Step 2 遍历局部关键帧中(一级相连)关键帧，将它们观测的地图点加入到局部地图点
    list<MapPoint*> lLocalMapPoints;
    // 遍历局部关键帧中的每一个关键帧
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
    
        // 取出该关键帧对应的地图点
        vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
        // 遍历这个关键帧观测到的每一个地图点，加入到局部地图点
        for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
        {
    
            MapPoint* pMP = *vit;
            if(pMP)
            {
    
                if(!pMP->isBad())   //保证地图点有效
                    // 把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId
                    // mnBALocalForKF 是为了防止重复添加
                    if(pMP->mnBALocalForKF!=pKF->mnId)
                    {
    
                        lLocalMapPoints.push_back(pMP);
                        pMP->mnBALocalForKF=pKF->mnId;
                    }
            }   // 判断这个地图点是否靠谱
        } // 遍历这个关键帧观测到的每一个地图点
    } // 遍历 lLocalKeyFrames 中的每一个关键帧

    // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
    // Step 3 得到能被局部地图点观测到，但不属于局部关键帧的关键帧(二级相连)，这些二级相连关键帧在局部BA优化时不优化
    list<KeyFrame*> lFixedCameras;
    // 遍历局部地图中的每个地图点
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
    
        // 观测到该地图点的KF和该地图点在KF中的索引
        map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
        // 遍历所有观测到该地图点的关键帧
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
    
            KeyFrame* pKFi = mit->first;

            // pKFi->mnBALocalForKF!=pKF->mnId 表示不属于局部关键帧，
            // pKFi->mnBAFixedForKF!=pKF->mnId 表示还未标记为fixed（固定的）关键帧
            if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
            {
                    
                // 将局部地图点能观测到的、但是不属于局部BA范围的关键帧的mnBAFixedForKF标记为pKF（触发局部BA的当前关键帧）的mnId
                pKFi->mnBAFixedForKF=pKF->mnId;
                if(!pKFi->isBad())
                    lFixedCameras.push_back(pKFi);
            }
        }
    }

    // Setup optimizer
    // Step 4 构造g2o优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    // LM大法好
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // 外界设置的停止优化标志
    // 可能在 Tracking::NeedNewKeyFrame() 里置位
    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    // 记录参与局部BA的最大关键帧mnId
    unsigned long maxKFid = 0;

    // Set Local KeyFrame vertices
    // Step 5 添加待优化的位姿顶点：局部关键帧的位姿
    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;
    }

    // Set Fixed KeyFrame vertices
    // Step  6 添加不优化的位姿顶点：固定关键帧的位姿，注意这里调用了vSE3->setFixed(true)
    // 不优化为啥也要添加？回答：为了增加约束信息
    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;
    }

    // Set MapPoint vertices
    // Step  7 添加待优化的局部地图点顶点
    // 边的最大数目 = 位姿数目 * 地图点数目
    const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();

    vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
    vpEdgesStereo.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFStereo;
    vpEdgeKFStereo.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeStereo;
    vpMapPointEdgeStereo.reserve(nExpectedSize);

    // 自由度为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();

        // Set edges
        // Step 8 在添加完了一个地图点之后, 对每一对关联的地图点和关键帧构建边
        // 遍历所有观测到当前地图点的关键帧
        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 // Stereo observation
                {
    
                    // 双目或RGB-D模式和单目模式类似
                    Eigen::Matrix<double,3,1> obs;
                    const float kp_ur = pKFi->mvuRight[mit->second];
                    obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

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

                    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];
                    Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                    e->setInformation(Info);

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

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

                    optimizer.addEdge(e);
                    vpEdgesStereo.push_back(e);
                    vpEdgeKFStereo.push_back(pKFi);
                    vpMapPointEdgeStereo.push_back(pMP);
                } 
            } 
        } // 遍历所有观测到当前地图点的关键帧
    } // 遍历所有的局部地图中的地图点

    // 开始BA前再次确认是否有外部请求停止优化，因为这个变量是引用传递，会随外部变化
    // 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位
    if(pbStopFlag)
        if(*pbStopFlag)
            return;

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

    bool bDoMore= true;

    // 检查是否外部请求停止
    if(pbStopFlag)
        if(*pbStopFlag)
            bDoMore = false;

    // 如果有外部请求停止,那么就不在进行第二阶段的优化
    if(bDoMore)
    {
    
        // Check inlier observations
        // Step 10 检测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);
        }

        // 对于所有的双目的误差边也都进行类似的操作
        for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
        {
    
            g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
            MapPoint* pMP = vpMapPointEdgeStereo[i];

            if(pMP->isBad())
                continue;
            // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
            if(e->chi2()>7.815 || !e->isDepthPositive())
            {
    
                e->setLevel(1);
            }

            e->setRobustKernel(0);
        }

        // Optimize again without the outliers
        // Step 11：排除误差较大的outlier后再次优化 -- 第二阶段优化
        optimizer.initializeOptimization(0);
        optimizer.optimize(10);

    }

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

    // Check inlier observations
    // Step 12：在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点
    // 对于单目误差边
    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));
        }
    }

    // 双目误差边
    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));
        }
    }

    // Get Map Mutex
    unique_lock<mutex> 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
    // Step 13：优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性

    //Keyframes
    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));
    }

    //Points
    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();
    }
}

 

六、结语

通过该篇博客的讲解，了解了 g2o::VertexSE3Expmap()、g2o::VertexSE3Expmap()、g2o::VertexSBAPointXYZ()、g2o::EdgeSE3ProjectXYZ()、g2o::EdgeStereoSE3ProjectXYZ()。知道了 Optimizer::LocalBundleAdjustment→局部建图位姿与地图点优化 的具体详细过程。下篇博客要讲解的就是Optimizer::OptimizeSim3() //闭环线程Sim3变换优化。

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