第 6 章 統合に向けて
A.4 非線形最小化
非線形の関数を用いて、多数の変数の組み合わせから最適な解を求める手法である。本研 究では 、２枚もし くは多数枚の画像を用いて、カメラの位置・姿勢や、点の３次元位置を補 正する際に用いる。
条件として、別の視点から取得した２枚もし くは多数枚の画像があり、画像間で４つ以上 の点が対応づいているとする。また、初期値が必要なため、真値に近いカメラの位置・姿勢 と特徴点の３次元位置があらかじめわかっているものとする。本研究では 、これらの初期値 を、モーションステレオや、因子分解法を用いて取得しておく。
評価関数の算出
透視投影モデルのもとで、カメラの位置・姿勢、特徴点の画像上の位置と空間上の位置 に関する関係式を導出する。基準座標系は、ロボットが走行する床面上のワールド 座標 系（ ローカル座標系）とする。ある点Pをワールド 座標系から見た場合と、カメラ座 標系から見た場合の関係式は、以下の式で表される。
R1Tx1 =X−T1
R2Tx2 =X−T2 (A.18)
ここで、Ri(i= 1,2)はワールド 座標系から見たカメラ姿勢の回転行列、Tiはワールド 座標系から見たカメラ姿勢への平行移動ベクトル、Xはワールド 座標系から見た点P の３次元位置、xiはカメラ座標系から見た点Pの３次元位置を表す。
一方、透視投影の式では、点の画像上の位置(u, v)と空間上の位置(X, Y, Z)は 、以下 の式で結びつく。
w
u v 1
=A(R| −RT)
X Y Z 1
ここで、wはスカラーの係数、Aはカメラの内部行列である。カメラの内部行列はあら かじめ求めておく。
まず、カメラ座標から見た点Pの座標を(xc, yc, zc)と置き、透視投影の式を表す。この 時、基準座標系はカメラ座標系となるので、そのカメラの位置・姿勢は、回転行列が単 位行列、平行移動ベクトルがゼロベクトルとなる。
w
u v 1
=
f1 0 0 0 f2 0 0 0 1
1 0 0 0 0 1 0 0 0 0 1 0
xc yc zc 1
これよりwを消去すると、以下の関係式が導かれる。
xc yc zc
=
zc f1u
zc f2v
zc
(A.19)
この式（A.19）を、式（A.18）にそれぞれ代入しzcを消去すると、以下の評価関数が 導かれる。
C =2
j=0
P i=0
rTxjmij
rTzjmij −Xi+txj Zi+tzj
2
+2
j=0
P i=0
rTyjmij
rTzjmij − Yi+tyj Zi+tzj
2
(A.20)
具体的には 、以下のような４つの式から成る。
C1=
v1sinθ1−f2cosθ1
v1cosθ1+f2sinθ1 − X Z−h
2
C2 =
f2u1
v1cosθ1+f2sinθ1 −f1 Y Z−h
2
C3 =
−f2u2sinφ+f1v2sinθ2sinφ−f1f2cosθ2cosφ
v2cosθ2+f2sinθ2 −f1X−dx Z−h
2
C4=
f2u2cosφ+f1v2sinθ2cosφ−f1f2cosθ2sinφ
v2cosθ2+f2sinθ2 −f1Y −dy Z−h
2
f1、f2はカメラの焦点距離に関する係数であり、既知とする。また、hはカメラの床面 からの高さであり、全体を通して一定である。
この式より、カメラの角度ψ、θ、ロボットの並進成分dx、dy、点の空間上での位置座標 X、Y、Zのうち、未知のパラメータを変数とし 、勾配法による最小化計算をおこなう。
勾配法による最適化
求めた評価関数にパラメータを入力し 、最適化計算をおこなう。本研究では、ニュート ン法と呼ばれる関数の二次微分を利用した最適化手法を用いる。ニュートン法の更新式 は 、以下のようになる。
ni+1 =ni−H−1dC(ni)
dn (A.21)
ここで、nは変数を並べたベクトルであり、Hはヘッセ行列と呼ばれる、関数の二次微 分を対象に並べた行列である。
H=
∂2C
∂n12
∂2C
∂n1∂n2 · · ·
∂2C
∂n1∂n2
∂2C
∂n22 · · · ... ... . ..
ニュートン法は、初期値が真値に近い場合、収束が早いという性質を持つ。本研究にお いては、線形解法などの他の手法から求める初期値が相応に正確であると判断し 、この 手法を採用した。
非線形最小化は、周囲に極値が存在しなければ 、入力されたパラメータの初期値に対し 、反 復計算をおこない最適なパラメータを求めることができる。本研究においてカメラの位置・
姿勢を補正する際には 、少なくとも４つの特徴点の対応が与えられていれば 、その３次元位 置が既知でなくても、解を精度よく求めることができる。問題点としては 、最小化をおこな うパラメータの数に比例し 、処理時間が増加することがあげられる。これは主に 、ヘッセ行 列が大きくなることにより、逆行列算出の計算コストがかかることに起因する。カメラの位 置・姿勢と特徴点の３次元位置を最適化する問題では、ヘッセ行列が特殊な形をしていること を利用し 、高速化する手法が提案されている[16]が 、本研究では実装していない。また、多 くの特徴点をまとめて処理するため、一部の特徴点に追跡ミスがあった場合、最小化処理が 収束せず、精度の良い解が求まらないおそれがある。本研究では、RANSACの枠組みを利用 し 、これに対処している。