FANUC KARELで3D姿勢推定 その3
TRIAD・Gram-Schmidt・SVD・Torque法をノイズ入り点群で比較する
前回までの記事では、SVDや固有値分解を使わずに、対応点の外積から回転誤差を求め、Quaternionを少しずつ更新するTorque法を試しました。
今回は理屈の話をいったん横に置きます。
やりたいことは単純です。
同じ点群、同じ姿勢変化、同じランダムノイズを与えたとき、それぞれの方法でどの程度正しく姿勢を復元できるのか。
比較するのは次の3系統4手法です。
| 系統 | 方法 | 特徴 |
|---|---|---|
| 直接基底生成 | TRIAD / Gram-Schmidt | 少数の方向ベクトルから直接座標系を作る |
| 最小二乗 | SVDによる点群合わせ | 全対応点を使った基準解 |
| 反復法 | Torque法 | 外積和を回転誤差としてQuaternionを更新する |
TRIADとGram-Schmidtは少し性格が違いますが、どちらも「ベクトルから直交基底を直接作る方法」として同じグループに置きます。
1. 比較条件
元となる3次元点群 $A$ を用意します。
今回は、平面上または立体的に配置した多角形の頂点を使用します。
各点を $ A_i$ とします。
これに既知の回転 $R_{true}$ と並進 $t_{true}$ を与え、
B_i =
R_{true} A_i + t_{true}
として、正解となる点群 (B) を作ります。
さらに各点へランダムノイズを加えます。
B_i' =
B_i + \epsilon_i
ここで、
\epsilon_i \sim N(0,\sigma^2)
とします。
各アルゴリズムには、同じ $A_i$ と $B_i'$ を入力します。
つまり乱数による条件差が出ないよう、1回の試行ではすべての方法に同じノイズを与えます。
2. 評価項目
今回は主に次の値を比較します。
回転誤差
推定回転を $$R_{est}$$ とし、真値との差
R_{err}
=
R_{est}R_{true}^{T}
から回転角誤差を求めます。
\theta_{err}
=
\cos^{-1}
\left(
\frac{\operatorname{tr}(R_{err})-1}{2}
\right)
単位はdegreeとします。
並進誤差
e_t
=
\|t_{est}-t_{true}\|
で評価します。
点群RMSE
推定した姿勢で点群 (A) を変換し、観測点群との残差を求めます。
RMSE =
\sqrt{
\frac{1}{N}
\sum_i
\|
R_{est}A_i+t_{est}-B_i'
\|^2
}
これは最終的に、
点群同士がどれだけ重なったか
を見る値です。
計算時間
Python上で各アルゴリズムを同じ回数実行し、平均計算時間を測定します。
ただし、これはFANUCコントローラ上での実行時間そのものではありません。
あくまで、
アルゴリズム同士の相対的な計算量を見るための参考値
として扱います。
反復回数
Torque法については、収束までの反復回数も記録します。
また、最大反復回数まで収束しなかったケースがあれば、未収束率も確認します。
3. TRIAD法
TRIAD法では、2本の方向ベクトルから直交基底を作ります。
例えば点群から
v_1=P_2-P_1
v_2=P_3-P_1
を作り、正規化します。
1本目を、
e_1=
\frac{v_1}{\|v_1\|}
とします。
さらに、
e_2=
\frac{v_1\times v_2}
{\|v_1\times v_2\|}
e_3=e_1\times e_2
として直交基底を作ります。
基準点群と観測点群でそれぞれ基底を作れば、その関係から回転行列を求めることができます。
特徴は非常に軽いことです。
一方で、姿勢推定に使う情報は基本的に選択したベクトルに依存します。
点が32個あっても、32個すべてを最小二乗的に利用するわけではありません。
したがって今回の比較では、
少数点だけから直接姿勢を作る軽量な方法
として扱います。
4. Gram-Schmidt直交化
Gram-Schmidtでも、同様に2本のベクトルから直交基底を作れます。
まず、
e_1=
\frac{v_1}{\|v_1\|}
とします。
次に、(v_2) から (e_1) 方向の成分を取り除きます。
u_2
=
v_2-(v_2\cdot e_1)e_1
そして、
e_2=
\frac{u_2}{\|u_2\|}
最後の軸を
e_3=e_1\times e_2
として座標系を作ります。
3次元ではTRIADと非常に近い結果になります。
今回ここを分けて比較する理由は、
外積中心で直交基底を作る方法と、射影によって直交化する方法で、ノイズや単精度計算時に差が出るか
を確認するためです。
実際にはほぼ同じ結果になる可能性もあります。
それも含めて確認します。
5. SVDによる点群位置合わせ
比較の基準として、全対応点を使った最小二乗解を求めます。
まず重心を計算します。
\bar A =
\frac{1}{N}\sum_i A_i
\bar B =
\frac{1}{N}\sum_i B_i
重心を取り除きます。
\hat A_i=A_i-\bar A
\hat B_i=B_i-\bar B
相関行列
H
=
\sum_i
\hat A_i\hat B_i^T
を作り、
H=U\Sigma V^T
とSVDします。
回転行列は、
R=VU^T
から求めます。
反転が発生した場合は、$\det(R)=+1$ となるよう補正します。
並進は、
t=\bar B-R\bar A
です。
これは今回の比較における基準解です。
FANUC KAREL上でそのまま実装するのは不可能ですが、
SVDを普通に使える環境なら、どの程度の性能が得られるのか
を見るために使用します。
6. Torque法
今回試している方法です。
異方性ありにするため、Z軸方向の誤差を潰す処理をしています。
Python上でシミュレーションするために計算精度はわざとロボットコントローラ上の制限である単精度小数点に限定します。
7. 実験条件
例えば次の条件でMonte Carlo試験を行います。
板状の点群に対して
- Z軸周りの回転を10°の範囲内で与える
- XY平面上で移動する
点群の数は
N=4,8,16,32
とします。
ランダムノイズは、
\sigma=
0.0,\,
0.1,\,
0.3,\,
0.5,\,
1.0\ {\rm mm}
程度を試します。
各条件について1000回程度試行し、
- 回転誤差平均
- 回転誤差標準偏差
- 並進誤差平均
- RMSE
- 最大誤差
- 計算時間
- 反復回数
を集計します。
8. 直接基底生成法と全点法の違い
この比較では注意点があります。
TRIADとGram-Schmidtは、SVD、Torque法とは少し立場が違います。
後者2つは基本的に全対応点を利用します。
一方TRIADとGram-Schmidtでは、姿勢生成に使用する点を選択します。
そのため、
点数が増えればTRIADの精度も自動的に向上する
わけではありません。
むしろ今回確認したいのは、
少数点で姿勢を直接決める軽量法に対して、全点を利用する方法がノイズ下でどの程度有利になるか
です。
したがってTRIAD / Gram-Schmidtで使用する点は、全試行で同じルールによって選択します。
例えば、
- 原点から最も遠い点
- そこから角度が十分離れた点
などに固定し、都合のよい点だけを選ばないようにします。
9. 予想
実験前の予想としては、
TRIAD / Gram-Schmidt
非常に軽い。
ノイズが小さければ十分使える。
ただし少数ベクトルへの依存が大きいため、ノイズが増えるとばらつきが増えそうです。
SVD
最も安定するはずです。
全対応点を使った最小二乗解なので、今回の基準になります。
ただしKAREL上への実装はほぼ不可能。
Torque法
SVDほど厳密ではありません。
しかし全点を使いながら、
- SVDなし
- 固有値分解なし
- 逆行列なし
でどこまで近づけるかが今回の本題です。
理想は、
SVDより少し精度は落ちるが、TRIADよりノイズに強く、ロボット上で十分実装可能
くらいの位置に収まることです。
もちろん実験して普通に負ける可能性もあります。
その場合はそのまま載せます。
11. 結果
計算速度
Python上ではSVDが圧倒的に速いです。
N=32で、
- SVD：約0.035 ms
- Gram-Schmidt：約0.095 ms
- TRIAD：約0.108 ms
- Torque：約11.7 ms 本条件では反復回数は10回程度で全て収束しました
TorqueはSVDの300倍くらい遅い（笑）。SVDはNumpyを使用、TorqueはPython上での反復計算のため大きな差が出ます。これは比較にならないですね。
Python上でほぼ同じ条件にて計算しているGram-Schmidt、TRIADと比較すると100倍遅いです。
推定精度
今回の比較では、TRIAD / Gram-Schmidt、SVD、Torque法のそれぞれに異なる特徴が確認できました。
回転量の比較グラフ
4点の点群
8点の点群
16点の点群
32点の点群
並行移動量の比較グラフ
12. 考察
回転量の推定性能差について
TRIADおよびGram-Schmidtは、少数の方向ベクトルから直接姿勢を求める非常に軽量な方法です。使用するベクトルの選択や幾何条件が適切であれば、ノイズ下でも十分高い精度を得ることができます。今回の条件では両者の結果はほぼ同一であり、大きな差は確認できないです。
SVDによる点群位置合わせは、全対応点を用いた最小二乗解として最も安定した結果を示しました。平均誤差だけでなく試行ごとのばらつきも小さいです。
Torque法は、SVDほど安定してはいないものの、全対応点を利用することで、点数が増えた場合にはTRIAD / Gram-Schmidtより小さな誤差を示す傾向が確認できました。一方で、試行ごとのばらつきはSVDより大きく、点群形状やノイズ分布、反復条件の影響を受けていると思われます。
また、Torque法はノイズ0の条件でも完全に誤差0まで収束させることができません。
FANUC KAREL上での実装を想定し、単精度浮動小数点で計算しています。収束判定も単精度計算で数値誤差の影響が大きくなる手前に設定している影響と考えられます。
実際の計測環境ではノイズ0の条件はほぼ存在しなません。したがってTorque法は、SVDの代替となる厳密解というよりも、SVDや固有値分解を利用できない環境で、点群形状やノイズ特性を考慮しながら複数点の情報を利用するための実用的な選択肢と考えるのが適切です。
並行移動量の推定性能差について
並進成分は各手法でほぼ同一となりました。これは重心から並進を求めており、基準点群を重心原点としているため、並進推定が回転推定法にほぼ依存しないためです。
今回の結果をまとめると、
- 保証された少数点から軽量に姿勢を求めるならTRIAD / Gram-Schmidt
- SVDを利用できる環境ならSVD
- SVDを使えない環境で全点を利用したい場合はTorque法
という、それぞれの立ち位置が確認できました。