ORB-SLAM2源码逐行解析系列（二十二）：ORB-SLAM2闭环线程中的Sim3优化

1. OptimizeSim3

（1）顶点与边

顶点

​ Sim(3)位姿优化发生在闭环检测过程中，仅对形成闭环的闭环候选关键帧到当前帧的Sim(3)位姿进行优化，不优化地图点。但由于地图点与二者产生约束，因此顶点包括待优化的闭环候选关键帧到当前帧的Sim(3)位姿和地图点（固定住不优化）。

​ 顶点中Sim(3)位姿的类型为g2o::VertexSim3Expmap，地图点的类型为g2o::VertexSBAPointXYZ。

边

​ 边在Sim(3)优化中是指地图点和Sim(3)位姿的投影关系，由于地图点和Sim(3)位姿都参与优化，因此是二元边。另外，根据投影关系可分为两种类型边。第一种为正向投影，即从闭环候选关键帧的地图点用g2oS12投影到当前关键帧上的边，类型是g2o::EdgeSim3ProjectXYZ；第二种是反向投影，即从当前关键帧的地图点用g2oS21投影到闭环候选关键帧的边上，类型是g2o::EdgeInverseSim3ProjectXYZ。

（2）误差

正向投影

// 正向投影
class EdgeSim3ProjectXYZ : public  BaseBinaryEdge<2, Vector2d,  VertexSBAPointXYZ, VertexSim3Expmap>
{
   
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeSim3ProjectXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    void computeError()
    {
   
      // 顶点1: 关键帧的Sim(3)变换  g2oS12
      const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
      // 顶点0: 地图点
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);

      Vector2d obs(_measurement);  // 二维观测
      // 误差 = 观测 - 重投影坐标  候选关键帧投影到当前关键帧
      _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
    }

   // virtual void linearizeOplus();
};

// 反向投影
class EdgeInverseSim3ProjectXYZ : public  BaseBinaryEdge<2, Vector2d,  VertexSBAPointXYZ, VertexSim3Expmap>
{
   
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeInverseSim3ProjectXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    void computeError()
    {
   
      // 顶点1: 关键帧的Sim(3)变换  g2oS21
      const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
      // 顶点0: 地图点
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);

      Vector2d obs(_measurement);  // 二维观测
      // 误差 = 观测 - 重投影坐标  当前关键帧投影到候选关键帧
      _error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())));
    }

   // virtual void linearizeOplus();
};

v2->estimate()表示估计的相机2坐标系下的三维点坐标，
v1->estimate()表示估计的位姿g2oS12。

​ v1->estimate().inverse()表示估计的位姿g2oS21。在优化过程中不断更新，参与到重投影误差的计算当中。

​ 正向投影：cam_map1(project(v1->estimate().map(v2->estimate())))表示用v1估计的位姿g2oS12把v2估计的相机2坐标系下的三维点转换到相机1坐标系下，然后通过project函数归一化三维点，再通过cam_map1函数用内参将其转化为像素坐标。

（3）OptimizeSim3整体流程

• 初始化g2o优化器。
• 设置待优化的Sim(3)位姿作为顶点。如果是单目相机模式，则不固定尺度；如果是双目/RGB-D相机模式，则在优化时固定尺度。
• 设置匹配的地图点作为顶点，并且设置地图点不优化。
• 设置地图点投影关系作为边。包括正向投影：从闭环候选关键帧的地图点投影到当前关键帧；反向投影：从当前关键帧投影到闭环候选关键帧。
• 用g2o开始优化，迭代5次。
• 用卡方检验剔除优化后误差大的边。
• 再次用g2o优化剩下的边。如果在上一步中有误差较大的边被剔除，说明闭环质量不是很好，则本次迭代10次；否则，本次迭代5次。
• 用优化后的结果更新Sim(3)位姿。

（4）OptimizeSim3代码实现

/**
 * @brief 形成闭环时固定（不优化）地图点进行Sim3位姿优化
 * 1. Vertex:
 *     - g2o::VertexSim3Expmap()，两个关键帧的位姿
 *     - g2o::VertexSBAPointXYZ()，两个关键帧共有的MapPoints
 * 2. Edge:
 *     - g2o::EdgeSim3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Sim3，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *     - g2o::EdgeInverseSim3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Sim3，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 * 
 * @param[in] pKF1              当前帧
 * @param[in] pKF2              闭环候选帧
 * @param[in] vpMatches1        两个关键帧之间的匹配关系
 * @param[in] g2oS12            两个关键帧间的Sim3变换，方向是从2到1，即闭环候选关键帧到当前帧    
 * @param[in] th2               卡方检验是否为误差边用到的阈值
 * @param[in] bFixScale         是否优化尺度，单目进行尺度优化，双目/RGB-D不进行尺度优化
 * @return int                  优化之后匹配点中内点的个数
 */
int Optimizer::OptimizeSim3(KeyFra