Bundle Adjustment

预备知识：

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

Bundle Adjustment简述

1. 问题定义

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

图片：特征点：

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

3d点位置：相机位姿：

误差函数：

$\textbf{min}_{R,t,P} = \sum_{i,j}\sigma_{i,j}||u_{i,j}-v_{i,j}||$

其中， $u_{i,j}$ 是特征点位置 $v_{i,j}$ 是地图点重投影位置， $\sigma_{i,j}=1$ 仅当在当前帧能够看到该地图点时。

2. g2o相关API介绍

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

2.1 节点

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

位姿：

地图点：

2.1.1 位姿

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

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> 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<const Vector6> 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<const Vector3> v(update);
      _estimate += v;
    }
};

2.2 边

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

重载函数：

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

误差计算：

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

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

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

误差函数的一阶导数计算

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

对地图点求导的雅克比

$\frac{\partial e}{\partial P} =- \begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2}\\ 0 & \frac{f_y}{Z} & -\frac{f_y Y}{Z^2} \end{bmatrix} R$

对相机位姿求导的雅克比

$\frac{\partial e}{\partial \delta \zeta } = \begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2} & -\frac{f_x XY}{Z^2} & f_x+\frac{f_x X^2}{Z^2} & -\frac{f_x Y}{Z}\\ 0 & \frac{f_y}{Z} & -\frac{f_y Y}{Z^2} & -f_y-\frac{f_y Y^2}{Z^2} & \frac{f_y XY}{Z^2} & \frac{f_y X}{Z} \end{bmatrix}$

virtual void linearizeOplus()
{
    // 关联帧的姿态
    VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
    SE3Quat T(vj->estimate());
    // 地图点
    VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_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<const CameraParameters *>            (parameter(0));
    // 相机参数，中间变量
    Matrix<number_t,2,3,Eigen::ColMajor> 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<const VertexSE3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
      Vector2 obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    virtual void linearizeOplus()
    {
      VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
      SE3Quat T(vj->estimate());
      VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_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<const CameraParameters *>(parameter(0));

      Matrix<number_t,2,3,Eigen::ColMajor> 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<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
  // 选择不同的求解器，如果结构是稀疏的则选择稀疏求解器，稠密结构则选择稠密求求解
  // 合理的利用H矩阵的稀疏性能够极大提高求解速度
  if (DENSE) {
    linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
  } else {
    linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
  }
  // 选择迭代策略，通常还是L-M算法居多
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))
  );
  optimizer.setAlgorithm(solver);

2.3.2 构造图结构

位姿节点

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

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);

地图点节点

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

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

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

约束边

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

g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
// 设置关联的3D地图点和当前帧
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
                       (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实现中的高度模板化的代码结构，使得不同问题能在同样的框架下进行求解。当然由于问题的特殊性，总是有不同之处，总结主要的不同点如下：

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

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

Bundle Adjustment

1. 问题定义

2. g2o相关API介绍

2.1 节点

2.2 边

2.3 优化流程

3. 代码实现

4. 小结

如何使用 JS 判断用户是否处于活跃状态

通过HPA+CronHPA组合应对业务复杂弹性伸缩场景

採用逆深度參數表達的BA問題導數推導

A Robust and Easy to Implement Method for IMU Calibration without

g2o學習筆記（二）：Bundle Adjustment 應用

g2o學習筆記（一）：曲線擬合

Cartographer三部曲（三）：Real-Time Loop Closure in 2D LIDAR SLAM

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結