【画像処理】測定誤差が推定カメラパラメータに与える影響について【誤差伝播】

はじめに
推定カメラパラメータと誤差伝播
実際にやってみた

はじめに

　外部・内部パラメータの推定は様々なサイトでチェッカーボードを使用した方法が紹介されるのを見かけるが、日本語記事で誤差に関する考察が含まれるものは少ない。考察されていたとしても再投影誤差で確認するにとどまっている。再投影誤差はパラメータ誤差を直接的に表現しているわけではない。場合によっては推定パラメータ達にどの程度誤差が含まれているのか知りたくなると思う。
　誤差伝播に関する文献をいろいろ探していると以下の文献を見つけた。
www.jstage.jst.go.jp
　最小二乗法の解説がなされていた。さらに探すと以下の書籍のp.144で詳細に説明がなされていた。

Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

　本ページでは上記二つの文献の内容を自分なりに解釈して実装したプログラムを紹介する。

推定カメラパラメータと誤差伝播

　誤差伝播を検討するため、再投影誤差を最小化するパラメータを推定する最小二乗法を考える。
　まず測定値として、画像上に投影された特徴点位置を設定する。画像上での位置を表すので単位はpixel。そして測定誤差が乗るため分散を有する。この測定値は、特徴点一点につきx軸方向とy軸方向の2つの値を持つ。
　次に観測パラメータとして、以下のパラメータを設定する。

$fx$ ：x軸方向焦点距離[pixel]
$fy$ ：y軸方向焦点距離[pixel]
$cx$ ：x軸方向光学中心[pixel]
$cy$ ：y軸方向光学中心[pixel]
$s$ ：せん断ひずみ[pixel]
$tx$ ：x軸方向カメラ位置[m]
$ty$ ：y軸方向カメラ位置[m]
$tz$ ：z軸方向カメラ位置[m]
$\gamma$ ：z軸方向回転角度[deg.]
$\beta$ ：y軸方向回転角度[deg.]
$\alpha$ ：x軸方向回転角度[deg.]

　回転角度はZYX オイラー角であり、 $(roll,pitch,yaw) = (\alpha,\beta,\gamma)$ である。
　以降、これら11個の観測パラメータは $q_{i,1},q_{i,2}...q_{i,11}$ と表現する。
　次に観測モデル $f$ は以下のように設定する。

$x$ は測定位置ベクトルで、測定値である特徴点の三次元位置を表しており、以下のように設定する。

$X$ ：x軸方向特徴点位置[m]
$Y$ ：y軸方向特徴点位置[m]
$Z$ ：z軸方向特徴点位置[m]

　 $A$ はヤコビ行列であり、観測モデル $f$ をm個の観測パラメータでそれぞれ偏微分することで得られる。
　観測パラメータが多いとヤコビ行列は非常に長大になり手計算は現実的ではなくなる。そこでsympyのシンボリック計算で観測パラメータを変数としたヤコビ行列の式を得る。

import sympy as sp
import numpy as np

X = sp.Symbol('tx')
Y = sp.Symbol('ty')
Z = sp.Symbol('tz')

x = sp.Symbol('x')
y = sp.Symbol('y')

fx = sp.Symbol('fx')
fy = sp.Symbol('fy')
cx = sp.Symbol('cx')
cy = sp.Symbol('cy')
s = sp.Symbol('s')

tx = sp.Symbol('tx')
ty = sp.Symbol('ty')
tz = sp.Symbol('tz')

roll = sp.Symbol('roll')
pitch = sp.Symbol('pitch')
yaw = sp.Symbol('yaw')

#回転行列
Rx = np.array([[1,0,0],[0,sp.cos(roll),-sp.sin(roll)],[0,sp.sin(roll),sp.cos(roll)]])
Ry = np.array([[sp.cos(pitch),0,sp.sin(pitch)],[0,1,0],[-sp.sin(pitch),0,sp.cos(pitch)]])
Rz = np.array([[sp.cos(yaw),-sp.sin(yaw),0],[sp.sin(yaw),sp.cos(yaw),0],[0,0,1]])

#Z→Y→Xの順で回転
Rzyx = Rz@Ry@Rx

#内部パラメータ行列
K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]])

#外部パラメータ行列
RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])

#特徴点の三次元位置
POS = np.array([X,Y,Z,1])

#投影点の計算
PPOS = K@RT@POS.T
xe = x - PPOS[0]/PPOS[2]
ye = y - PPOS[1]/PPOS[2]

#ヤコビ行列の計算
J = np.array([[sp.diff(xe,tx),sp.diff(xe,ty),sp.diff(xe,tz),sp.diff(xe,roll),sp.diff(xe,pitch),sp.diff(xe,yaw)],
              [sp.diff(ye,tx),sp.diff(ye,ty),sp.diff(ye,tz),sp.diff(ye,roll),sp.diff(ye,pitch),sp.diff(ye,yaw)]])

#ヤコビ行列の表示
print(J)

　これを表示してみると以下のようになる。長い。

[[-(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + fx + s*sin(yaw)*cos(pitch))/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz) - (cx*tz + fx*tx + s*ty + tx*(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + s*sin(yaw)*cos(pitch)) + ty*(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw))) + tz*(cx*cos(pitch)*cos(roll) + fx*(sin(pitch)*cos(roll)*cos(yaw) + sin(roll)*sin(yaw)) + s*(sin(pitch)*sin(yaw)*cos(roll) - sin(roll)*cos(yaw))))*sin(pitch)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz)**2
  -(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw)) + s)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz) + (cx*tz + fx*tx + s*ty + tx*(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + s*sin(yaw)*cos(pitch)) + ty*(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw))) + tz*(cx*cos(pitch)*cos(roll) + fx*(sin(pitch)*cos(roll)*cos(yaw) + sin(roll)*sin(yaw)) + s*(sin(pitch)*sin(yaw)*cos(roll) - sin(roll)*cos(yaw))))*sin(roll)*cos(pitch)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz)**2
以下略

　次に観測パラメータの誤差を計算していく。以下のような計算によって求まる。

　 $S$ は誤差行列で、n個の測定値に対応する分散共分散行列である。これが観測パラメータに誤差伝播する。分散が既知でないとき、平均値が真値であると仮定して有限個数の測定値から分散を計算することもできる。

実際にやってみた

　以上の内容を使用して、以下ページでカメラパラメータとその誤差範囲を計算した。
jonajiro.hatenablog.com
　