玉木徹: 「姿勢推定と回転行列」, 電子情報通信学会 スマートインフォメディアシステム研究会（SIS）信号処理研究会（SIP）オーディオビジュアル複合情報処理研究会（IPSJ-AVM）, 電子情報通信学会技術報告 SIP2009-48, SIS2009-23, Vol.109, No.202, pp.59-64, 広島大学, 広島 (2009 09).

1. 姿勢推定と回転行列
玉木徹（広島大学）
スマートインフォメディアシステム研究会（SIS）
信号処理研究会（SIP）
オーディオビジュアル複合情報処理研究会（IPSJ-AVM）
2009年 9月24日(木) 広島大学 東千田キャンパス

2. 3次元の剛体変換
• 回転行列R • 並進ベクトルt
– 3x3行列 – 3次元ベクトル
– 直交行列 – 3次元の並進移動を表
– 行列式が＋１ す
– 3次元の回転を表す – Rとtを合わせて特殊
– 特殊直交群SO(3) ユークリッド群SE(3)
姿勢推定＝Rとtを求める

3. 本日の姿勢推定
• 対応の与えられた点集 • 回帰に基づく姿勢推定
合のマッチング問題
大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及
び大仏殿のデジタル復元,日本バーチャルリアリティ学会論
文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

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

5. 遺跡の電子アーカイブ
http://www.cvl.iis.u-tokyo.ac.jp/research/bayon/

6. 遺跡の電子アーカイブ
http://www.cvl.iis.u-tokyo.ac.jp/research/bayon/

7. ＩＣＰ：対応が与えられていない
点集合同士のマッチング
• Iterative Closest Point
(ICP) (Cheng et. al, 1992)
– 点集合マッチング手法
の代表的な手法
– その後多数の派生手法
が登場
対応が与えられていない点集合
？
点集合X 点集合Y 大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バー
チャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

8. ＩＣＰ：対応が与えられていない
点集合同士のマッチング
• ICPの基本アルゴリズム 対応が与えられていない点集合
– 1. 仮対応を与える ？
• Xの各点にもっとも近いY
の点を求める (closest
point)
– 2. パラメータを求める 点集合X 点集合Y
• XをYに変換するパラメー
タの推定
対応が与えられている点集合
点集合X 点集合Y

9. 対応の与えられた点集合の
マッチング問題
対応が与えられている点集合
点集合X 点集合Y

10. 行列形式での定式化
Frobenius norm
3xn
R t
3x3 3xn 3x1 1xn

11. 並進tの計算
重心位置の計算
点集合X 点集合Y
回転が与えられれば
並進も求められる

12. Rの推定問題への変形
重心位置を引くと：
点集合X’ 点集合Y’
3xn 3x3

13. Lagrange乗数を用いた解法
[14] Umeyama, 1991
制約条件
最小化する項
直交行列 行列式が＋1
6つの異なるLagrange乗数を Lagrange乗数
要素に持つ3x3対称行列

14. SVDを用いた解法 [10] Arun et. al, 1987
[11] Schönemman, 1966
• Frobenius normと行列 • 目的関数の変形
のトレース(tr)
• トレースの性質

15. SVDを用いた解法 [10] Arun et. al, 1987
[11] Schönemman, 1966
• トレースの最大値
特異値分解
(Singular Value Decomposition, SVD)
直交行列 対角行列
Schwarzの不等式
最大値を与えるのは

16. Schwarzの不等式
回転行列
• Schwarzの不等式
直交行列

17. SVDを用いた解法 [12] Kanatani, 1994
[14] Umeyama, 1991
• 行列式+1の制約 • Polar decomposition
V,Uは直交行列
行列式が±1
polar部分（行列式が1）

18. 車輪の再発明
• Orthogonal Procrustes • Absolute Orientation
Problem – 写真測量学・光学
– 心理学分野 （Photogrammetria, J.
（Psychometrikaなど） Optical Soc. of America
など）
• Wahba’s Problem
– 天文学分野
• Fitting corresponding
point sets
– コンピュータビジョン
（IEEE PAMIなど）

19. Orthogonal Procrustes Problem
Hurley et. al, 1962
[11] Schönemman, 1966
Procrustes. "Now then, you fellows; I
[3] Schönemman et. al, 1970
mean to fit you all to my little bed!"
[5] Akca, 2003
Chorus. "Oh lor-r!!"
Orthogonal Procrustes
Extended Orthogonal Procrustes
Generalized Orthogonal Procrustes
File:The Modern Bed of Procustes - Punch cartoon - Project
Gutenberg eText 13961.png
http://commons.wikimedia.org/wiki/File:The_Modern_Bed_of_Proc
ustes_-_Punch_cartoon_-_Project_Gutenberg_eText_13961.png

20. Wahba’s Problem
Wahba, 1965
Prof. Grace Wahba [6] Shuster, 2006
(University of Wisconsin-Madison) [7] Markley, 1999
正の重み
spacecraft座標系 基準座標系
での方向 での方向
http://www.stat.wisc.edu/~wahba/public/jpg/jsm.05/noether.html
Grace Wahba, “Problem 65–1: A Least Squares Estimate of
Spacecraft Attitude,” SIAM Review, Vol. 7, No. 3, July 1965, p. 409.

21. Absolute Orientation
[8] Thompson, 1958
[16] Horn, 1987
[9] Horn et. al, 1988
‘
E. H. Thompson, "An exact linear solution of
the problem of absolute orientation,"
Photogrammetria 15(4), 163–179 (1958).

22. Fitting corresponding point sets
[10] Arun et. al, 1987
[14] Umeyama, 1991
[12] Kanatani, 1994
大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及び大仏殿のデジタル復元,日本バー
チャルリアリティ学会論文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

23. 本日の姿勢推定
• 対応の与えられた点集 • 回帰に基づく姿勢推定
合のマッチング問題
大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及
び大仏殿のデジタル復元,日本バーチャルリアリティ学会論
文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

24. 「今年のロボット」大賞2007
FANUC Robot M-430iA は、コンベア上を高速に流れてくる物品を
瞬時にピッキングする垂直多関節ロボットです。1台あたり毎分120
個の処理能力で24時間連続運転が可能です。iRVisionによるビ
ジュアルトラッキング機能と組み合せて、コンベアでバラバラに供
給される物品も素早く整列し、箱詰めします。
M-430iAのビジュアルトラッキングによる高速ハンドリング 「今年のロボット」大賞2007 大賞（経済産業大臣賞）受賞 - 2007年12月のニュース - ファナック:
http://www.fanuc.co.jp/ja/news/2007/0712/0712_robotaward.html

25. ピッキング：ロボットビジョンの応用
ロボットアーム カメラ 平面上の
形状認識
（商用化済み）
株式会社リンクス：産業別適用事例
http://www.linx.jp/applicate_example/halcon/index.html
どうつかむのか？
（姿勢推定が必要）
？
ねじの山から
一本取り出す
人間の作業 （3自由度の姿勢推定）

26. 回帰に基づく姿勢推定手法
学習 推定
x1 x2 xn
学習画像
p
パラメータ p1 p2  pn

27. 姿勢推定方法
学習画像 最小化問題（一般）
学習画像の姿勢パラメータ
簡単化（線形）
例：1自由度の姿勢パラメータ
最小ノルム解
一般化逆行列

28. 1自由度の姿勢表現に必要な条件
画像は学習画像の線形和
0° 330° 340° 350° 10° 20° 30°
学習 推定
角度
180° 330° 340° 350° 10° 20° 30°
姿勢表現に適さない：360°において不連続
sin,cos
sin(0°)‫‏‬ sin(340°)‫‏‬ sin(350°)‫‏‬ sin(10°)‫‏‬ sin(20°)‫‏‬ sin(30°)‫‏‬
cos(0°)‫‏‬ cos(340°)‫‏‬ cos(350°)‫‏‬ cos(10°)‫‏‬ cos(20°)‫‏‬ cos(30°)‫‏‬
姿勢表現に適している：すべてにおいて連続

29. 3自由度の姿勢表現の比較
回転軸
固定角 オイラー角 単位四元数 回転行列
回転量
連続性
一対一
回転量を qと-qが同じ
ジンバルロック 角度で表すと× 姿勢を表す
が存在 (sin,cos)で表すと○

30. 姿勢推定方法（3自由度）
学習画像 最小ノルム解
学習画像の姿勢パラメータ
学習
一般化逆行列
3自由度の姿勢パラメータ 推定
回転行列ではない！
回転行列Riの要素
問題

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

32. 本日の姿勢推定
• 対応の与えられた点集 • 回帰に基づく姿勢推定
合のマッチング問題
大石岳史,増田智仁,倉爪亮,池内克史,創建期奈良大仏及
び大仏殿のデジタル復元,日本バーチャルリアリティ学会論
文誌, Vol. 10, No. 3, pp.429-436, 2005.10.

33. 3次元の剛体変換
• 回転行列R • 並進ベクトルt
– 3x3行列 – 3次元ベクトル
– 直交行列 – 3次元の並進移動を表
– 行列式が＋１ す
– 3次元の回転を表す – Rとtを合わせて特殊
– 特殊直交群SO(3) ユークリッド群SE(3)
姿勢推定＝Rとtを求める

34. 姿勢推定と回転行列
玉木徹（広島大学）
スマートインフォメディアシステム研究会（SIS）
信号処理研究会（SIP）
オーディオビジュアル複合情報処理研究会（IPSJ-AVM）
2009年 9月24日(木) 広島大学 東千田キャンパス