姿勢推定と回転行列

玉木徹（広島大学）

スマートインフォメディアシステム研究会（SIS）信号処理研究会（SIP）

オーディオビジュアル複合情報処理研究会（IPSJ-AVM）2009年 9月24日(木) 広島大学東千田キャンパス

3次元の剛体変換

• 回転行列R

– 3x3行列

– 直交行列

– 行列式が＋１

– 3次元の回転を表す

– 特殊直交群SO(3)

• 並進ベクトルt

– 3次元ベクトル

– 3次元の並進移動を表す

– Rとtを合わせて特殊ユークリッド群SE(3)

姿勢推定＝Rとtを求める

本日の姿勢推定

• 対応の与えられた点集合のマッチング問題

• 回帰に基づく姿勢推定

大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

大仏の形状スキャン大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

遺跡の電子アーカイブ

http://www.cvl.iis.u-tokyo.ac.jp/research/bayon/

遺跡の電子アーカイブ

http://www.cvl.iis.u-tokyo.ac.jp/research/bayon/

ＩＣＰ：対応が与えられていない点集合同士のマッチング

• Iterative Closest Point (ICP) (Cheng et. al, 1992)

– 点集合マッチング手法の代表的な手法

– その後多数の派生手法が登場

点集合X 点集合Y

？

対応が与えられていない点集合

大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

ＩＣＰ：対応が与えられていない点集合同士のマッチング

• ICPの基本アルゴリズム

– 1. 仮対応を与える• Xの各点にもっとも近いYの点を求める (closest point)

– 2. パラメータを求める• XをYに変換するパラメータの推定

点集合X 点集合Y

？

対応が与えられていない点集合

点集合X 点集合Y

対応が与えられている点集合

対応の与えられた点集合のマッチング問題

点集合X 点集合Y

対応が与えられている点集合

行列形式での定式化

R t

3xn 1xn

3xn

Frobenius norm

3x3 3x1

並進tの計算

点集合X 点集合Y

重心位置の計算

回転が与えられれば並進も求められる

Rの推定問題への変形

点集合X’ 点集合Y’

重心位置を引くと：

3xn 3x3

Lagrange乗数を用いた解法

最小化する項

制約条件

Lagrange乗数6つの異なるLagrange乗数を要素に持つ3x3対称行列

[14] Umeyama, 1991

直交行列 行列式が＋1

SVDを用いた解法

• Frobenius normと行列のトレース(tr)

• トレースの性質

• 目的関数の変形

[10] Arun et. al, 1987[11] Schönemman, 1966

SVDを用いた解法

• トレースの最大値特異値分解(Singular Value Decomposition, SVD)

最大値を与えるのは

直交行列 対角行列

[10] Arun et. al, 1987[11] Schönemman, 1966

Schwarzの不等式

Schwarzの不等式

• Schwarzの不等式

直交行列

回転行列

SVDを用いた解法

• 行列式+1の制約 • Polar decomposition

V,Uは直交行列行列式が±1

[12] Kanatani, 1994[14] Umeyama, 1991

polar部分（行列式が1）

車輪の再発明

• Orthogonal Procrustes Problem

– 心理学分野（Psychometrikaなど）

• Wahba’s Problem

– 天文学分野

• Absolute Orientation

– 写真測量学・光学（Photogrammetria, J. Optical Soc. of America など）

• Fitting corresponding point sets

– コンピュータビジョン（IEEE PAMIなど）

Orthogonal Procrustes Problem

Procrustes. "Now then, you fellows; I mean to fit you all to my little bed!"Chorus. "Oh lor-r!!"

File:The Modern Bed of Procustes - Punch cartoon - Project Gutenberg eText 13961.pnghttp://commons.wikimedia.org/wiki/File:The_Modern_Bed_of_Procustes_-_Punch_cartoon_-_Project_Gutenberg_eText_13961.png

Hurley et. al, 1962[11] Schönemman, 1966[3] Schönemman et. al, 1970[5] Akca, 2003

Orthogonal Procrustes

Extended Orthogonal Procrustes

Generalized Orthogonal Procrustes

Wahba’s Problem

http://www.stat.wisc.edu/~wahba/public/jpg/jsm.05/noether.html

Prof. Grace Wahba(University of Wisconsin-Madison)

Grace Wahba, “Problem 65–1: A Least Squares Estimate of Spacecraft Attitude,” SIAM Review, Vol. 7, No. 3, July 1965, p. 409.

spacecraft座標系での方向

基準座標系での方向

正の重み

Wahba, 1965[6] Shuster, 2006[7] Markley, 1999

Absolute Orientation

E. H. Thompson, "An exact linear solution of the problem of absolute orientation," Photogrammetria 15(4), 163–179 (1958).

[8] Thompson, 1958[16] Horn, 1987[9] Horn et. al, 1988

‘

Fitting corresponding point sets[10] Arun et. al, 1987[14] Umeyama, 1991[12] Kanatani, 1994

大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

本日の姿勢推定

• 対応の与えられた点集合のマッチング問題

• 回帰に基づく姿勢推定

大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

「今年のロボット」大賞2007

M-430iAのビジュアルトラッキングによる高速ハンドリング 「今年のロボット」大賞2007 大賞（経済産業大臣賞）受賞 - 2007年12月のニュース - ファナック:http://www.fanuc.co.jp/ja/news/2007/0712/0712_robotaward.html

FANUC Robot M-430iA は、コンベア上を高速に流れてくる物品を瞬時にピッキングする垂直多関節ロボットです。1台あたり毎分120個の処理能力で24時間連続運転が可能です。iRVisionによるビジュアルトラッキング機能と組み合せて、コンベアでバラバラに供給される物品も素早く整列し、箱詰めします。

ピッキング：ロボットビジョンの応用ロボットアーム カメラ

人間の作業

どうつかむのか？（姿勢推定が必要）

ねじの山から一本取り出す

（3自由度の姿勢推定）

平面上の形状認識（商用化済み）

株式会社リンクス：産業別適用事例http://www.linx.jp/applicate_example/halcon/index.html

？

回帰に基づく姿勢推定手法

学習画像

パラメータ p1 p2 pnp

学習 推定

x1 x2 xn

姿勢推定方法

学習画像

学習画像の姿勢パラメータ

例：1自由度の姿勢パラメータ

最小化問題（一般）

簡単化（線形）

最小ノルム解

一般化逆行列

1自由度の姿勢表現に必要な条件

350°340°330° 10° 20° 30°

画像は学習画像の線形和

0°

350°340°330° 10° 20° 30°180°

sin(0°)‏ sin(350°)‏sin(340°)‏ sin(10°)‏ sin(20°)‏ sin(30°)‏

cos(350°)‏cos(340°)‏ cos(10°)‏ cos(20°)‏ cos(30°)‏cos(0°)‏

姿勢表現に適さない：360°において不連続

姿勢表現に適している：すべてにおいて連続

学習

角度

sin,cos

推定

固定角 オイラー角回転軸回転量

単位四元数 回転行列

連続性

一対一

3自由度の姿勢表現の比較

qと-qが同じ姿勢を表す

回転量を角度で表すと×

(sin,cos)で表すと○ジンバルロックが存在

姿勢推定方法（3自由度）

学習画像

学習画像の姿勢パラメータ

3自由度の姿勢パラメータ

最小ノルム解

一般化逆行列

回転行列Riの要素

推定

学習

問題

回転行列ではない！

Rの推定値の直交化前半の結果解くべき問題

本日の姿勢推定

• 対応の与えられた点集合のマッチング問題

• 回帰に基づく姿勢推定

大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バーチャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

3次元の剛体変換

• 回転行列R

– 3x3行列

– 直交行列

– 行列式が＋１

– 3次元の回転を表す

– 特殊直交群SO(3)

• 並進ベクトルt

– 3次元ベクトル

– 3次元の並進移動を表す

– Rとtを合わせて特殊ユークリッド群SE(3)

姿勢推定＝Rとtを求める

姿勢推定と回転行列

玉木徹（広島大学）

スマートインフォメディアシステム研究会（SIS）信号処理研究会（SIP）

オーディオビジュアル複合情報処理研究会（IPSJ-AVM）2009年 9月24日(木) 広島大学東千田キャンパス