g2o学习笔记（二）：Bundle Adjustment 应用

Bundle Adjustment 

预备知识：

g2o学习笔记（一）：曲线拟合

Bundle Adjustment简述

1. 问题定义

已知：一系列图片以及图片内对应的特征点匹配关系

图片：  特征点：

待求：特征点的3d位置以及图片对应的相机位姿

3d点位置：  相机位姿：

误差函数：

其中，是特征点位置是地图点重投影位置，仅当在当前帧能够看到该地图点时。

2. g2o相关API介绍

而由于BA问题为计算机视觉中经典问题，对于其节点和边等关系都已在g2o中实现了，但是为了深入理解还是按照自定义的方式进行介绍。

2.1 节点

在BA问题中不仅需要对相机运动位姿进行优化，还需要优化地图中的三维点。因此节点有两种类型分别为6自由度的位姿和3维坐标点代表的地图点。

位姿：

地图点：

2.1.1 位姿

在重载函数中稍有不同的在于位姿的更新由于其并非欧式空间，因此需要采用特殊的更新方式，这里既可以采用四元数进行更新也可以采用旋转矩阵，其在更新量较小时是等价的。因此函数实现为

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }

 整体实现的其他部分和曲线拟合中的节点并无区别，整体代码如下。

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();

  bool read(std::istream& is);

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

  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

2.1.2 地图点

地图点采用3维坐标表达，处于欧式空间中，因此和曲线拟合中的例子并无差别，不再赘述。整体实现如下：

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map v(update);
      _estimate += v;
    }
};

2.2 边

在BA问题中，边的约束存在于相机位姿和M个地图点之间，由于相机位姿存在于非欧空间中，因此其在计算误差和一阶导数部分需要进行重定义。

重载函数：

• void computeError() 计算当前误差
• virtual void linearizeOplus() 计算线性化一阶导数

误差计算：

这里的误差主要指的是重投影误差，即3D点点投影到相机坐标系下与实际特征点之间的坐标差值。因此误差计算步骤如下：

1. 转换到相机坐标系
2. 利用相机模型转换到像素坐标系（无畸变）
3. 计算特征点位置和重投影位置差值

void computeError()  
{
    // 获得对应相机位姿
    const VertexSE3Expmap* v1 = static_cast(_vertices[1]);
    // 获得对应3D地图点
    const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);
    // 获得相机参数
    const CameraParameters * cam = static_cast(parameter(0));
    // 获得当前测量值
    Vector2 obs(_measurement);
    // 重投影到相机坐标系下，然后相减得到误差
    _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
}

误差函数的一阶导数计算

由于姿态处于非欧空间内，不能采用简单的数值求导的方法进行求解。因此需要自行定义误差函数的导数以便于在迭代中进行求解计算。这里设计到了在BA问题中，误差函数对地图点和对姿态的求导，这部分内容在博客中有较为详细的推导过程，其结果也和代码相互印证。

对地图点求导的雅克比

对相机位姿求导的雅克比

virtual void linearizeOplus()
{
    // 关联帧的姿态
    VertexSE3Expmap * vj = static_cast(_vertices[1]);
    SE3Quat T(vj->estimate());
    // 地图点
    VertexSBAPointXYZ* vi = static_cast(_vertices[0]);
    // 地图点的位置，世界坐标系
    Vector3 xyz = vi->estimate();
    // 地图点位置，相机坐标系
    Vector3 xyz_trans = T.map(xyz);
    // 中间变量提前计算好
    number_t x = xyz_trans[0];
    number_t y = xyz_trans[1];
    number_t z = xyz_trans[2];
    number_t z_2 = z*z;
    // 相机参数
    const CameraParameters * cam = static_cast            (parameter(0));
    // 相机参数，中间变量
    Matrix tmp;
    tmp(0,0) = cam->focal_length;
    tmp(0,1) = 0;
    tmp(0,2) = -x/z*cam->focal_length;

    tmp(1,0) = 0;
    tmp(1,1) = cam->focal_length;
    tmp(1,2) = -y/z*cam->focal_length;
    // 对地图点求导，雅克比矩阵
    _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();
    // 对位姿求导，雅克比矩阵
    _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
    _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
    _jacobianOplusXj(0,2) = y/z *cam->focal_length;
    _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
    _jacobianOplusXj(0,4) = 0;
    _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

    _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
    _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
    _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
    _jacobianOplusXj(1,3) = 0;
    _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
    _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

因此，整体的边实现如下：

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZ2UV();

    bool read(std::istream& is);

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

    void computeError()  {
      const VertexSE3Expmap* v1 = static_cast(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);
      const CameraParameters * cam
        = static_cast(parameter(0));
      Vector2 obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    virtual void linearizeOplus()
    {
      VertexSE3Expmap * vj = static_cast(_vertices[1]);
      SE3Quat T(vj->estimate());
      VertexSBAPointXYZ* vi = static_cast(_vertices[0]);
      Vector3 xyz = vi->estimate();
      Vector3 xyz_trans = T.map(xyz);

      number_t x = xyz_trans[0];
      number_t y = xyz_trans[1];
      number_t z = xyz_trans[2];
      number_t z_2 = z*z;

      const CameraParameters * cam = static_cast(parameter(0));

      Matrix tmp;
      tmp(0,0) = cam->focal_length;
      tmp(0,1) = 0;
      tmp(0,2) = -x/z*cam->focal_length;

      tmp(1,0) = 0;
      tmp(1,1) = cam->focal_length;
      tmp(1,2) = -y/z*cam->focal_length;

      _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

      _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
      _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
      _jacobianOplusXj(0,2) = y/z *cam->focal_length;
      _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
      _jacobianOplusXj(0,4) = 0;
      _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

      _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
      _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
      _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
      _jacobianOplusXj(1,3) = 0;
      _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
      _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
    }

    CameraParameters * _cam;
};

2.3 优化流程

2.3.1 求解器初始化

在g2o内部已经定义好了求解器的类型为 BlockSolver_6_3 ，表示问题中节点维度最大是7而约束维度为3

  // 定义求解器
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);
  // BA线性求解BlockSolver_6_3,含义为节点维度为6 边的维度为3
  std::unique_ptr linearSolver;
  // 选择不同的求解器，如果结构是稀疏的则选择稀疏求解器，稠密结构则选择稠密求求解
  // 合理的利用H矩阵的稀疏性能够极大提高求解速度
  if (DENSE) {
    linearSolver = g2o::make_unique>();
  } else {
    linearSolver = g2o::make_unique>();
  }
  // 选择迭代策略，通常还是L-M算法居多
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique(std::move(linearSolver))
  );
  optimizer.setAlgorithm(solver);

2.3.2 构造图结构

位姿节点

1. 赋予每一个节点id
2. 设置前两帧位姿固定（避免优化过程中尺度发生改变）
3. 设置初始值
4. 添加到图结构中

g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
// 前两帧id的位姿是固定的，在优化过程中不变
if (i<2)
{
    v_se3->setFixed(true);
}
// 将需要优化的位姿信息添加到图结构中
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);

地图点节点

由于矩阵的稀疏性可以将矩阵分解成为姿态相关和地图点相关，由于地图点往往很多，则先求解较小的矩阵姿态相关矩阵，然后再利用已有信息求地图点相关矩阵。因此需要设置地图点节点为边缘，便于在矩阵分解时区分。

1. 赋予每一个节点id
2. 设置初始值
3. 设置为边缘化
4. 添加到图结构中

// 3D点节点
g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
// 设置id
v_p->setId(point_id);
// 设置为边缘化，和姿态节点区分便于稀疏求解
v_p->setMarginalized(true);
// 初始值+噪声
v_p->setEstimate(true_points.at(i)
                 + Vector3d(g2o::Sampler::gaussRand(0., 1),
                            g2o::Sampler::gaussRand(0., 1),
                            g2o::Sampler::gaussRand(0., 1)));

约束边

1. 设置当前边连接的地图点和相机位姿
2. 设置测量值
3. 设置测量值的信息矩阵（权重）
4. 设置鲁棒核函数
5. 添加到图结构中

g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
// 设置关联的3D地图点和当前帧
e->setVertex(0, dynamic_cast(v_p));
e->setVertex(1, dynamic_cast
                       (optimizer.vertices().find(j)->second));
e->setMeasurement(z);
e->information() = Matrix2d::Identity();
// 是否使用鲁棒核，使用huber鲁棒核能有效抑制外点干扰
if (ROBUST_KERNEL) 
{
    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
    e->setRobustKernel(rk);
}
// 添加到优化器中
e->setParameterId(0, 0);
optimizer.addEdge(e);

2.3.3 求解 

  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(10);

3. 代码实现

由于代码实现较长，同样只给出github链接，每一步都有详细的注释。

4. 小结

BA优化问题和之前的曲线优化在结构上并没有太大的区别，这得益于g2o实现中的高度模板化的代码结构，使得不同问题能在同样的框架下进行求解。当然由于问题的特殊性，总是有不同之处，总结主要的不同点如下：

1. 节点状态更新不能只是简单的相加
2. 求导不能采用数值求导，需要重载函数从新定义雅克比矩阵
3. 前两帧的位姿保持固定，避免尺度不断变化
4. 设置地图点节点为边缘点