ORB-SLAM2源码逐行解析系列（二十一）：ORB-SLAM2局部建图线程中的优化方法

1. LocalBundleAdjustment

（1）顶点与边

顶点

​ 局部建图线程的局部优化函数LocalBundleAdjustment主要用于优化局部建图线程中的局部关键帧和局部地图点。其中，局部关键帧是指当前关键帧的一级共视关键帧，局部地图点是指局部关键帧观测到的所有地图点。

​ 由于局部关键帧所观测到的地图点也会被当前关键帧的二级共视关键帧观测到，即二级共视关键帧对这些地图点也会产生约束，因此顶点包括需要优化的当前帧的局部关键帧和对应的局部地图点，以及不参与优化的当前帧的二级共视关键帧（在优化时被固定住）。

​ 顶点中关键帧的类型为g2o::VertexSE3Expmap，地图点的类型为g2o::VertexSBAPointXYZ。

边

​ 边是指由局部地图点和观测到它的关键帧之间的约束关系，由于局部地图点和关键帧都参与优化，因此是二元边。对于单目相机，边的类型为g2o::EdgeSE3ProjectXYZ，对于双目/RGB-D相机，边的类型为g2o::EdgeStereoSE3ProjectXYZ。

（2）误差

​ 误差定义为3D-2D的重投影误差：
$$e=(u,v)-project(Tcw\ast Pw)$$

// 以单目为例
class  EdgeSE3ProjectXYZ: public  BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{
   
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZ();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  void computeError()  {
   
    // 顶点1: 关键帧的SE3变换
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); 
    // 顶点0: 地图点
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);  
    Vector2d obs(_measurement);  // 二维观测
    // 误差 = 观测 - 重投影坐标
    // 与一元边不同的是：一元边中地图点是直接传入的，而二元边是作为优化变量estimate出来的
    _error = obs-cam_project(v1->estimate().map(v2->estimate()));
  }

  bool isDepthPositive() {
   
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    return (v1->estimate().map(v2->estimate()))(2)>0.0;
  }
    

  virtual void linearizeOplus();
  // 将相机坐标系下的3D点 转换为 像素坐标系下的2D点
  Vector2d cam_project(const Vector3d & trans_xyz) const;
  double fx, fy, cx, cy;
};

 局部建图优化中的边是二元边，它连接的需要优化的两个顶点分别是：_vertices[0]，表示待优化的局部关键帧的地图点；_vertices[1]，表示观测到该地图点的关键帧，包括一级共视关键帧和二级共视关键帧。但二级共视关键帧不参与优化，在后续设置中需要将其固定住。

​ v1->estimate()表示优化的关键帧位姿，v2->estimate()表示优化的地图点，在优化过程中不断更新，参与到重投影误差的计算当中。v1->estimate().map(v2->estimate())表示用优化的关键帧位姿把优化的地图点坐标投影到当前关键帧上，得到当前帧相机坐标系下的3D点。

（3）LocalBundleAdjustment整体流程

• 将当前关键帧及其共视关键帧添加到局部关键帧中。

• 遍历局部关键帧中的一级共视关键帧，将它们观测到的地图点添加到局部地图点中。

• 获取当前关键帧的二级共视关键帧，也称为固定关键帧，在局部BA优化时仅作为约束条件，不参与优化。

• 构造g2o优化器。

• 添加待优化的位姿顶点——局部关键帧的位姿。

• 添加不优化的位姿顶点——固定关键帧的位姿。

• 添加待优化的局部地图点作为顶点。

• 每添加完一个地图点之后，对每一对关联的地图点和观测到它的关键帧构建边。

• 分为两个阶段开始优化。第一阶段迭代5次，排除误差较大的边，第二阶段迭代10次。

• 在所有优化结束后重新计算误差，剔除边连接误差比较大的关键帧和地图点

• 更新优化后的关键帧位姿和地图点的位置、平均观测方向等属性。

（4）LocalBundleAdjustment代码实现

/*
 * @brief Local Bundle Adjustment
 *
 * 1. Vertex:
 *     - g2o::VertexSE3Expmap()，局部关键帧，即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
 *     - g2o::VertexSE3Expmap()，FixedCameras，即能观测到局部地图点的关键帧(并且不属于局部关键帧)
 *  											的位姿，在优化中这些关键帧的位姿不变
 *     - g2o::VertexSBAPointXYZ()，局部地图点，即局部关键帧能观测到的所有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        当前关键帧
 * @param pbStopFlag 是否停止优化的标志
 * @param pMap       局部地图
 * @note 由局部建图线程调用,对局部地图进行优化的函数
 */
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
   
    // 该优化函数用于LocalMapping线程的局部BA优化
    
    // lLocalKeyFrames：用于存储局部关键帧
    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);
    }
    
    // 用于存储局部地图点
    list<MapPoint*> lLocalMapPoints;
    // Step 2: 将局部关键帧观测到的地图点添加到局部地图点中
    // 遍历局部关键帧
    for(list<KeyFrame*>