Skip to main content

Bundle Adjustment

Bundle Adjustment（BA）は、camera pose、camera intrinsics、3D point を同時に最適化し、画像上の観測点との reprojection error を最小化する処理です。SfM と SLAM の精度を支える中心的な optimization です。名前の「bundle」は、各 camera center から 3D point へ伸びる viewing ray の束を意味します。BA は、その ray の束が全体として最も自然に交わるように調整する処理だと考えられます。

Reprojection error​

3D point Xj\mathbf{X}_j が camera ii によって image point xij\mathbf{x}_{ij} として観測されたとします。Projection function を Πi(⋅)\Pi_i(\cdot) とすると、reprojection error は次のように書けます。

eij=xij−Πi(Xj)=xij−π(Ki(RiXj+ti))\mathbf{e}_{ij} = \mathbf{x}_{ij} - \Pi_i(\mathbf{X}_j) = \mathbf{x}_{ij} - \pi\left(\mathbf{K}_i(\mathbf{R}_i\mathbf{X}_j + \mathbf{t}_i)\right)

ここで、xij\mathbf{x}_{ij} は実際に画像上で検出された 2D point です。Πi(Xj)\Pi_i(\mathbf{X}_j) は、現在の camera parameter と 3D point を使って同じ点を画像に投影した位置です。したがって、eij\mathbf{e}_{ij} は「観測された点と、今の推定値から見えるはずの点とのずれ」を表します。

Bundle Adjustment は、すべての観測について次の objective を最小化します。

min⁡{Ki,Ri,ti},{Xj}∑(i,j)∈Oρ(∥eij∥Σij−12)\min_{\{\mathbf{K}_i,\mathbf{R}_i,\mathbf{t}_i\},\{\mathbf{X}_j\}} \sum_{(i,j)\in\mathcal{O}} \rho\left(\left\|\mathbf{e}_{ij}\right\|_{\mathbf{\Sigma}_{ij}^{-1}}^2\right)

各項の意味は次の通りです。

  • O\mathcal{O} は、観測 edge の集合です。Camera ii が point jj を見ているときだけ、その pair が入ります。
  • Ki\mathbf{K}_i は camera intrinsics です。焦点距離や principal point などを含みます。
  • Ri,ti\mathbf{R}_i, \mathbf{t}_i は camera extrinsics です。World coordinate から camera coordinate への変換を表します。
  • Xj\mathbf{X}_j は最適化される 3D point です。
  • Σij\mathbf{\Sigma}_{ij} は観測 noise の covariance です。信頼度の低い観測には大きい分散を与えることがあります。
  • ρ\rho は robust loss です。Huber loss や Cauchy loss のように、大きな外れ値の影響を弱める関数を使います。

この式の気持ちは、「すべての camera で、すべての 3D point が観測された pixel にできるだけ戻ってくるように、camera と point を同時に少しずつ動かす」というものです。

Robust loss の意味​

単純な二乗誤差では、少数の大きな outlier が最適化を大きく歪めます。SfM では、誤対応、動的物体、反射、blur などによって outlier が残るため、robust loss が重要です。

Huber loss は、閾値 δ\delta を使って次のように書けます。

ρ(s)={s,s≤δ22δs−δ2,s>δ2\rho(s) = \begin{cases} s, & s \leq \delta^2 \\ 2\delta\sqrt{s} - \delta^2, & s > \delta^2 \end{cases}

s=∥eij∥2s = \|\mathbf{e}_{ij}\|^2 とすると、小さな誤差には通常の二乗誤差として振る舞い、大きな誤差には線形に近い penalty を与えます。この式の気持ちは、「良い観測は精密に合わせたいが、明らかに怪しい観測に最適化全体を支配させたくない」というものです。

Local parameterization​

Rotation matrix R\mathbf{R} は、単なる 9 個の自由な数ではありません。直交性 R⊤R=I\mathbf{R}^\top\mathbf{R}=\mathbf{I} と determinant det⁡(R)=1\det(\mathbf{R})=1 を満たす必要があります。そのため、BA では rotation を Lie algebra の小さな更新で表すことが多いです。

現在の rotation を R\mathbf{R}、小さな更新量を ω∈R3\boldsymbol{\omega}\in\mathbb{R}^3 とすると、更新は次のように書けます。

R←exp⁡([ω]×)R\mathbf{R} \leftarrow \exp([\boldsymbol{\omega}]_\times)\mathbf{R}

ここで、[ω]×[\boldsymbol{\omega}]_\times は skew-symmetric matrix、exp⁡\exp は matrix exponential です。この式の気持ちは、「rotation を直接 9 個の値としていじるのではなく、三次元の小さな回転として安全に更新する」ということです。これにより、更新後の R\mathbf{R} も rotation として自然な形を保ちます。

Gauss-Newton / Levenberg-Marquardt​

BA の objective は nonlinear least squares です。現在の parameter を θ\boldsymbol{\theta}、更新量を Δθ\Delta\boldsymbol{\theta}、residual vector を r(θ)\mathbf{r}(\boldsymbol{\theta}) とすると、一次近似は次のようになります。

r(θ+Δθ)≈r(θ)+JΔθ\mathbf{r}(\boldsymbol{\theta} + \Delta\boldsymbol{\theta}) \approx \mathbf{r}(\boldsymbol{\theta}) + \mathbf{J}\Delta\boldsymbol{\theta}

ここで、J\mathbf{J} は Jacobian matrix です。Gauss-Newton 法では、次の normal equation を解いて更新量を求めます。

(J⊤WJ)Δθ=−J⊤Wr(\mathbf{J}^\top\mathbf{W}\mathbf{J})\Delta\boldsymbol{\theta} = -\mathbf{J}^\top\mathbf{W}\mathbf{r}

W\mathbf{W} は観測重みや robust loss から来る weight matrix です。Levenberg-Marquardt 法では、数値的に安定させるために damping term を加えます。

(J⊤WJ+λI)Δθ=−J⊤Wr(\mathbf{J}^\top\mathbf{W}\mathbf{J} + \lambda\mathbf{I})\Delta\boldsymbol{\theta} = -\mathbf{J}^\top\mathbf{W}\mathbf{r}

この式の気持ちは、「現在の非線形問題を一旦近くの二次関数として見なし、その近似問題で誤差が下がる方向へ parameter を動かす」というものです。λ\lambda が大きいと慎重な更新になり、小さいと Gauss-Newton に近い大胆な更新になります。

Sparse structure と Schur complement​

BA の重要な性質は、Jacobian が非常に sparse であることです。一つの reprojection residual は、一つの camera と一つの 3D point にしか依存しません。つまり、camera 同士や point 同士が直接すべて結びついているわけではありません。

Parameter を camera 側の更新 Δθc\Delta\boldsymbol{\theta}_c と point 側の更新 Δθp\Delta\boldsymbol{\theta}_p に分けると、normal equation は概念的に次の block matrix で書けます。ここでは右辺の符号を b=−J⊤Wr\mathbf{b}=-\mathbf{J}^\top\mathbf{W}\mathbf{r} に含めています。

[UWcpWcp⊤V][ΔθcΔθp]=[bcbp]\begin{bmatrix} \mathbf{U} & \mathbf{W}_{cp} \\ \mathbf{W}_{cp}^\top & \mathbf{V} \end{bmatrix} \begin{bmatrix} \Delta\boldsymbol{\theta}_c \\ \Delta\boldsymbol{\theta}_p \end{bmatrix} = \begin{bmatrix} \mathbf{b}_c \\ \mathbf{b}_p \end{bmatrix}

ここで、U\mathbf{U} は camera-camera block、V\mathbf{V} は point-point block、Wcp\mathbf{W}_{cp} は camera と point の coupling block です。各 3D point は互いに直接依存しないため、V\mathbf{V} は block diagonal になります。

この構造を使うと、point の更新を消去して、camera parameter だけの小さな system を作れます。これが Schur complement です。

(U−WcpV−1Wcp⊤)Δθc=bc−WcpV−1bp(\mathbf{U} - \mathbf{W}_{cp}\mathbf{V}^{-1}\mathbf{W}_{cp}^\top) \Delta\boldsymbol{\theta}_c = \mathbf{b}_c - \mathbf{W}_{cp}\mathbf{V}^{-1}\mathbf{b}_p

この式の気持ちは、「大量の 3D point を直接すべて同時に解くのではなく、点の影響を camera 側の制約へ畳み込んで、まず camera を効率的に解く」というものです。大規模 SfM で BA が実用的に動くのは、この sparse structure を利用しているからです。

Local BA と global BA​

種類説明
Local BA最近追加された camera と近傍 point だけを最適化します。SLAM など real-time system で重要です。
Global BA全 camera と全 point を最適化します。高精度ですが計算コストが大きいです。

Incremental SfM では、新しい image を追加するたびに local BA を行い、一定のタイミングで global BA を行うことが多いです。Local BA は誤差の局所的な増加を抑え、global BA は reconstruction 全体の歪みを取り除きます。

Gauge freedom​

単眼 SfM の BA には、scale、global rotation、global translation の自由度があります。つまり、reconstruction 全体を回転・平行移動・拡大縮小しても、image 上の reprojection は変わりません。この自由度を放置すると、Hessian が rank deficient になり、最適化が不安定になります。

実装では、最初の camera pose を固定したり、特定の camera や point に prior を与えたりして gauge を固定します。GPS、IMU、known baseline、ground control point がある場合は、それらを factor として入れることで metric scale を安定させられます。

なぜ重要か​

Pose estimation や triangulation は noise を含みます。Bundle Adjustment は、それらを全体として整合するように再最適化します。SfM の最終品質は BA の品質に大きく依存します。

もう少し具体的に言うと、pairwise relative pose、PnP、triangulation は、それぞれ局所的な推定です。局所的には良く見えても、全体として camera と 3D point が矛盾することがあります。BA は、その矛盾を reprojection error という一つの尺度にまとめ、全体の整合性を取り戻します。

Graph optimization と SLAM​

SLAM では、BA は graph optimization の一種として扱われます。Node は camera pose や landmark、edge は observation constraint です。Loop closure が検出されると、pose graph optimization や BA によって drift を修正します。

Pose graph optimization は camera pose 同士の相対 pose constraint を主に使います。一方で、BA は画像上の landmark observation まで戻って最適化します。そのため、BA はより直接的で精密ですが、計算量は大きくなります。

関連ページ​

主なソース​