Reprojection Error

**再投影誤差(reprojection error)**は視覚SLAMの基本的な幾何学的残差であり、仮説上の3D点とカメラ姿勢が、実際の2D特徴観測をどれだけよく説明できるかを計測する。3D点 Xj\mathbf{X}_j (ワールドフレーム)、カメラ姿勢 Ti∈SE(3)T_i \in SE(3) (ワールドからカメラへの変換)、そしてその点が画像 ii の中で観測されたピクセル位置 zij\mathbf{z}_{ij} を考える。

eij=zij−π ⁣(TiXj)\mathbf{e}_{ij} = \mathbf{z}_{ij} - \pi\!\left(T_i \mathbf{X}_j\right)

ここで π:R3→R2\pi : \mathbb{R}^3 \to \mathbb{R}^2 はカメラの投影関数である。内部パラメータ (fx,fy,cx,cy)(f_x, f_y, c_x, c_y) を持つピンホールカメラと、カメラフレームの点 Xc=(X,Y,Z)T=TiXj\mathbf{X}^c = (X, Y, Z)^T = T_i \mathbf{X}_j に対して、

π(Xc)=[fx X/Z+cxfy Y/Z+cy]\pi(\mathbf{X}^c) = \begin{bmatrix} f_x \, X / Z + c_x \\ f_y \, Y / Z + c_y \end{bmatrix}

この誤差はピクセル単位で存在し、特徴検出器の位置決め誤差(通常は1ピクセル前後)と直接比較できる。これにより、閾値や共分散を設定しやすくなる。

残差からコスト関数へ

観測雑音がガウス分布 zij∼N(π(TiXj),Σij)\mathbf{z}_{ij} \sim \mathcal{N}\left(\pi(T_i\mathbf{X}_j), \Sigma_{ij}\right) に従うという仮定の下では、姿勢と点の最大尤度推定は、まさに重み付き非線形最小二乗問題

C=∑(i,j)∈OeijT Ωij eijC = \sum_{(i,j) \in \mathcal{O}} \mathbf{e}_{ij}^T \, \Omega_{ij} \, \mathbf{e}_{ij}

となる。ここで Ωij=Σij−1\Omega_{ij} = \Sigma_{ij}^{-1} は情報行列であり、O\mathcal{O} は(姿勢、点)の観測対の集合である。各項は二乗マハラノビス距離であり、実際には Σij\Sigma_{ij} はしばしば等方的であり、その特徴が検出された画像ピラミッドの階層でスケールされる(粗い階層 = 雑音が多い = 重みが低い)。

誤ったマッチは二次コストを支配してしまうほど巨大な残差を生じるため、実際のシステムでは各項をロバストカーネル ρ\rho(Huber、Cauchy)でラップし、∑ρ(eijTΩijeij)\sum \rho\left(\mathbf{e}_{ij}^T \Omega_{ij} \mathbf{e}_{ij}\right) とし、カイ二乗値が閾値を超える観測を枝刈りする。

最適化

このコストは姿勢に関して非線形である(SE(3)SE(3)の作用と透視除算を通じて)。現在の推定値の周りで e(x+Δx)≈e(x)+JΔx\mathbf{e}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}(\mathbf{x}) + J \Delta\mathbf{x} を線形化し、Gauss–Newton正規方程式 (JTΩJ)Δx=−JTΩ e\left(J^T \Omega J\right)\Delta\mathbf{x} = -J^T \Omega\, \mathbf{e} を反復的に解くのが標準的な手法である。連鎖律により、ヤコビアンは以下のように因数分解される。

∂e∂(⋅)=−∂π∂Xc⋅∂Xc∂(⋅)\frac{\partial \mathbf{e}}{\partial (\cdot)} = -\frac{\partial \pi}{\partial \mathbf{X}^c} \cdot \frac{\partial \mathbf{X}^c}{\partial (\cdot)}

∂π/∂Xc\partial \pi / \partial \mathbf{X}^c は 1/Z1/Z と −X/Z2-X/Z^2、−Y/Z2-Y/Z^2 の項を含む 2×32 \times 3 行列であり、2番目の因子は、姿勢に関して微分する(リー代数の摂動を経由、2×62 \times 6)か、点に関して微分する(2×32 \times 3)かによって異なる。

どの問題がこれを最小化するか

比較対象となる代替手法: 測光誤差(photometric error)(direct methodはピクセルの位置ではなく画素強度を比較する)と3D点対点/点対平面誤差(ICP)。信頼できる特徴の対応関係が存在する場合には、ピクセル空間の雑音モデルが計測が実際に行われた方法と一致するため、再投影誤差が好まれる。

SLAMにおける意義

関連ノート