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. 小結

認知提升的方法

螞蟻面試：Springcloud核心組件的底層原理，你知道多少？

採用逆深度參數表達的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以太網驅動總結