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
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值