剛体の位置と姿勢 - Tohoku University Official ...koichi/l5.pdf · 剛体の位置と姿勢...

ベクトルの表現

• ΣAとΣBの原点は一致しているとする

• あるベクトルr

– ΣAから見たとき (ΣAでの表現): Ar = [ArxAry

Arz]T

– ΣBでの表現: Br = [BrxBry

Brz]T

Ar = AxBBrx + AyB

Bry + AzBBrz

Ar = ARBBr

同様に，(ΣAとΣBの立場を逆にすれば)

Br = BRAAr

• 上の関係は任意のrについて成り立つから

(ARB)(BRA) = I3


座標系の関係

• ΣA，ΣBと原点が一致している第3の座標系ΣC

• rのΣCにおける表現CrBr = BRC

Cr, Ar = ARCCr

ARCCr = Ar = ARB

Br = ARBBRC

CrARC = ARB

BRC, ARB = (CRA)T CRB

ARB =

(CxA)T CxB (CxA)T CyB (CxA)T CzB

(CyA)T CxB (CyA)T CyB (CyA)T CzB

(CzA)T CxB (CzA)T CyB (CzA)T CzB


同次変換行列

• 2つの座標系ΣA, ΣB(原点が一致していない)

• ΣAに対するΣBの位置: ApB

• ΣAに対するΣBの姿勢: ARB

• ΣBに関してBrで表示された点をΣAで表示

Ar = ARBBr + ApB

Ar

1

=

ARBApB

0 0 0 1

Br

1

• 同次変換行列AT B =

ARBApB

0 0 0 1

• [注意！]:

Ar と

Ar

1

，Br と

Br

1

を同一視することがある．


同次変換

• 同次変換Ar = AT B

Br

• クイズ– ΣBはΣAと原点が一致していて，ZAまわりにα回転した位置にある．このときAT Bを求めよ．

– ΣBはΣAに対してYA方向に2，ZA方向に1だけ並進した位置にある．このときAT Bを求めよ．


同次変換の分解

1 0 1 2 3 4 5 61

0

1

2

3

4

5

ΣA

Σ B

p

B

Ar

r

A

B

• 同次変換行列

ARBApB

0 1

=

I3ApB

0 1

ARB 0

0 1

• 同次変換Ar = AT B

Br

• ベクトルの回転・並進としての解釈– ΣAにおいてBrで表現される点をΣA に関してARB回転し，それをΣAに関して ApB並進するとArを得る．

– ΣBにおける表現がBrである点の ΣAにおける表現は Arである．


座標系の回転・並進としての解釈

−1 0 1 2 3 4 5−1

0

1

2

3

4

5

• ΣAをΣAに関してARB回転し，それを ΣAに関して ApB 並進するとΣBを得る．

• ΣAをΣAに関して ApB並進し，その場でARB回転するとΣBを得る．


座標系の回転・並進としての解釈 [注意 1]

−1 0 1 2 3 4 5−1

0

1

2

3

4

5

• ΣAをΣAに関してARB回転し，回転後の座標系Σ′Aに関して

A′pB並進してもΣBにはならない！


座標系の回転・並進としての解釈 [注意 2]

−1 0 1 2 3 4 5 6−1

0

1

2

3

4

5

• ΣAをΣAに関してApB並進し，ΣA

に関して ARB回転してもΣBにはならない！


同次変換の積

• 3つの座標系ΣA, ΣB, ΣCがあり，ΣAとΣBの関係がAT B，ΣBとΣCの関係がBT Cで与えられるとき，ΣAとΣCの関係は？

AT C = AT BBT C

• ここでAT B =

ARBApB

0 1

=

I3ApB

0 1

ARB 0

0 1

つまりAT C =

I3ApB

0 1

ARB 0

0 1

I3BpC

0 1

BRC 0

0 1

に注意する．


同次変換の積の解釈

1. 左から解釈する方法

(a) ΣAをΣAに関してApB並進

(b) それをその場でARB回転−→ ΣBを得る

(c) ΣBをΣBに関してBpC並進

(d) それをその場でBRC回転−→ ΣCを得る

2. 右から解釈する方法

(a) ΣAをΣAに関してBRC回転

(b) それをΣAに関してBpC並進−→ Σ′Bを得る

(c) Σ′BをΣAに関してARB回転

(d) それをΣAに関してApB並進−→ ΣCを得る


同次変換の積の例題，逆変換

• クイズ

AT B =

√3/2 −1/2 0 2

1/2√

3/2 0 1

0 0 1 0

0 0 0 1

, BT C =

1/√

2 1/√

2 0 1

−1/√

2 1/√

2 0 1

0 0 1 0

0 0 0 1

左から解釈する場合と右から解釈する場合の図を書き，同じ結果を得ることを確認せよ．

• 逆変換BT A = (AT B)−1 =

(ARB)T −(ARB)T ApB

0 1


線形代数の応用

• ロボットのキネマティクス• ステレオ• キャリブレーション


PUMA ロボットのリンク構造


PUMA ロボットの同次変換行列 (0 → 1)

• Σ0に対しΣ1は並進なしでΣ0の z軸まわりの回転．

0T 1 =

C1 −S1 0 0

S1 C1 0 0

0 0 1 0

0 0 0 1

ただしC1 = cos(θ1), S1 = sin(θ1)である．



• Σ1に対しΣ2は...

– Σ1をx軸まわりに−π/2回転．

– 回転後のz方向に lb− ld並進し，その場で z軸まわりに θ2回転．

1T 2 =

1 0 0 0

0 0 1 0

0 −1 0 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1 lb − ld0 0 0 1

C2 −S2 0 0

S2 C2 0 0

0 0 1 0

0 0 0 1

=

C2 −S2 0 0

0 0 1 lb − ld−S2 −C2 0 0

0 0 0 1

• 次のように考えても同じ– Σ1をy方向に lb− ld並進し，その場でx

軸まわりに−π/2回転．

– 回転後の z方向に θ2回転．




– Σ2をx方向に lc並進．

– その場で z軸まわりに θ3回転．

2T 3 =

1 0 0 lc0 1 0 0

0 0 1 0

0 0 0 1

C3 −S3 0 0

S3 C3 0 0

0 0 1 0

0 0 0 1

=

C3 −S3 0 lcS3 C3 0 0

0 0 1 0

0 0 0 1




– Σ3を x方向に le並進し，その場で x軸まわりに−π/2回転．

– z軸方向に lf並進し，その場でz軸まわりに θ4回転．

3T 4 =

1 0 0 le0 1 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 0 1 0

0 −1 0 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1 lf0 0 0 1

C4 −S4 0 0

S4 C4 0 0

0 0 1 0

0 0 0 1

=

C4 −S4 0 le0 0 1 lf−S4 −C4 0 0

0 0 0 1


PUMA ロボットの同次変換行列

• 同様にして

4T 5 =

C5 −S5 0 0

0 0 −1 0

S5 C5 0 0

0 0 0 1

5T 6 =

C6 −S6 0 0

0 0 1 0

−S6 −C6 0 0

0 0 0 1


ロボットキネマティクス(機構学)

• 関節角度θから手先(または手先に固定されたツール)の位置0r と姿勢 0R6 を求めること

• 一般的にはAr = ARB

Br + ApB

をA = 5, B = 6から順に基準座標系に向かって解いていく．

• 同次変換行列の積で書くとコンパクト0T 6 = 0T 1

1T 2 · · · 5T 6


インバースキネマティクス(逆機構学)

• 手先の位置姿勢 0T 6を実現する関節角度 θ

を求めること

• 非線形であり，解は一意ではない• 例

– 右利き・左利き

– エルボーアップ・エルボーダウン


透視射影変換

Image Plane

Y�

X�

Z�

y

x

ObjectCoordinateSystemCamera

CoordinateSystem

Cameraf

�

cprel

• 幾何光学 (レンズのモデル)

– 深さZ，焦点距離f

x = fX

Z, y = f

Y

Z

• 透視投影変換

s

x

y

1

=

f 0 0 0

0 f 0 0

0 0 1 0

X

Y

Z

1

• 透視投影変換行列

A =

f 0 0 0

0 f 0 0

0 0 1 0

– CCD の縦横の解像度の違い，CCD 面のゆがみなどを考慮すると A はさらに複雑になる．


ビジョンシステム

World�

CoordinateSystem


CoordinateSystem

Image Plane

Camera

y

x

f�

Z�

X�

Y�

• 対象物とカメラの関係 (CT O)

– 対象物の座標系において表現Oriをもつ点は，カメラ座標系では

Cri

1

= CT O

Ori

1

, Cri =

Xi

Yi

Zi

– 撮像面上の位置

si

xi

yi

1

= ACT O

Ori

1

= A(WT C)−1WT O

Ori

1


キャリブレーション

World�

CoordinateSystem


CoordinateSystem

Image Plane

Camera

y

x

f�

Z�

X�

Y�

• 正確に測定された対象位置(WT O)と物体形状 (Ori)に基づいて，カメラで物体を計測し(xi, yi)，カメラの内部パラメータ (A)および外部パラメータ(WT C) を同定する問題．

si

xi

yi

1

= A(WT C)−1WT O

Ori

1

• あとで詳しく説明する．


透視射影変換

Image Plane

Y�

X�

Z�

y

x


CoordinateSystem

Cameraf

�

cprel

• 幾何光学 (レンズのモデル)

– 深さZ，焦点距離f

x = fX

Z, y = f

Y

Z


s

x

y

1

=

f 0 0 0

0 f 0 0

0 0 1 0

X

Y

Z

1

• 透視投影変換行列

A =

f 0 0 0

0 f 0 0

0 0 1 0

– CCD の縦横の解像度の違い，CCD 面のゆがみなどを考慮すると A はさらに複雑になる．


ビジョンシステム

World�

CoordinateSystem


CoordinateSystem

Image Plane

Camera

y

x

f�

Z�

X�

Y�


s

x

y

1

= A

XY

Z

1

• 対象物とカメラの関係 (CT O)

– 対象物の座標系において表現Oriをもつ点のカメラ座標系での位置

Cri

1

= CT O

Ori

1

, Cri =

Xi

Yi

Zi

– 撮像面上の位置

si

xi

yi

1

= ACT O

Ori

1

= A(WT C)−1WT O

Ori

1


キャリブレーション

World�

CoordinateSystem


CoordinateSystem

Image Plane

Camera

y

x

f�

Z�

X�

Y�

si

xi

yi

1

= A(WT C)−1WT O

Ori

1

• 正確に測定された– 対象位置: WT O

– 物体形状: Ori

– 物体の画像: xi, yi

ただし (i = 1, . . . , N)

に基づいて，カメラの

– 内部パラメータ: A

– 外部パラメータ: WT C

を同定する問題．


ビジョンシステムのキャリブレーション

World�

CoordinateSystem


CoordinateSystem

Image Plane

Camera

y

x

f�

Z�

X�

Y�

si

xi

yi

1

= A(WT C)−1WT O

Ori

1

= PWT O

Ori

1

• Pのサイズは3×4であり，自由度は12．ただし，P ′ = αP に対しても上式は成立するため，本質的には11自由度．

• 1点の観測につき2自由度の情報が得られるため，6点を観測すればPは決定可能．

• しかしノイズの影響などにより，精度よく推定するためには数十程度の点を計測する必要があり，現在もまだアクティブな研究対象である．


キャリブレーションアルゴリズム

透視変換のモデル:

si

xi

yi

1

= PWT O

Ori

1

問題: Find P ∈ R3×4 s.t.

∑

i

∥∥∥∥∥∥∥∥∥∥∥∥∥∥

si

xi

yi

1

− P

Xi

Yi

Zi

1

∥∥∥∥∥∥∥∥∥∥∥∥∥∥

2

→ min

アルゴリズム:

点 iに関して

• P = [Pjk]とおくと，最小化すべきベクトルの要素は

sixi − (P11Xi + P12Yi + P13Zi + P14)

siyi − (P21Xi + P22Yi + P23Zi + P24)

si − (P31Xi + P32Yi + P33Zi + P34)

となる．


キャリブレーションアルゴリズム

• P の要素を縦に並べたベクトルをP = [P11, P12, . . . , P34]T とし，パラメータ si を消去する

と，上式は[Xi Yi Zi 1 0 0 0 0 −xiXi −xiYi −xiZi −xi

]P

[0 0 0 0 Xi Yi Zi 1 −yiXi −yiYi −yiZi −yi

]P

となる．

• P の自由度を用いて P34 = 1 となるように規格化する．P = [P11, P12, . . . , P33]T とおき，点

i = 1, · · · , Nに関して前の式を連立すると最小化すべき式として

x1

y1

x2

y2...

yN

−

X1 Y1 Z1 1 0 0 0 0 −x1X1 −x1Y1 −x1Z1

0 0 0 0 X1 Y1 Z1 1 −y1X1 −y1Y1 −y1Z1

X2 Y2 Z2 1 0 0 0 0 −x2X2 −x2Y2 −x2Z2

0 0 0 0 X2 Y2 Z2 1 −y2X2 −y2Y2 −y2Z2...

0 0 0 0 XN YN ZN 1 −yNXN −yNYN −yNZN

P

を得る．

• したがって，キャリブレーション問題は下記の最小2乗問題となる

minx‖y −Mx‖2


キャリブレーションの例

• 透視投影モデル:

si

xi

yi

1

= PWT O

Ori

1

, P = A(WT C)−1

• カメラ内部パラメータ

A =

fx α cx 0

0 fy cy 0

0 0 1 0

= [B 0]

• カメラ外部パラメータ

(WT C)−1 = CT W =

CRWCtW

0 0 0 1

• カメラモデルP = A(WT C)−1 =

[BCRW BCtW

]


数値例

• 観察対象: 一辺 80cm 平面，観察点数 64

• 観察条件: 画素整数，分散 1.5 pixel のガウス分布ノイズ

• カメラパラメータ

B = tc = 1.0e+03 * Rc =

-300 0 1 0.0510 0.9820 -0.0600 0.1790

0 -300 -2 -0.1571 0.0046 0.9555 0.2950

0 0 1 -1.0693 -0.1888 -0.2889 0.9386

• カメラモデル

P = Phat =

0.2757 -0.0166 0.0493 15.2973 0.2762 -0.0170 0.0000 15.2083

0.0009 0.2675 0.0845 -46.0803 0.0010 0.2674 0.0000 -46.7265

0.0002 0.0003 -0.0009 1.0000 0.0002 0.0003 0 1.0000


数値例

• 観察対象: 一辺 40cm 立方体，観察点数 64

• 観察条件: 画素整数，分散 1.5 pixel のガウス分布ノイズ

• カメラパラメータ

B = tc = Rc =

-300 0 1 28.7676 0.8410 0.1218 -0.5272

0 -300 -2 -114.6471 -0.1221 0.9919 0.0343

0 0 1 -880.9085 0.5271 0.0355 0.8491

• カメラモデル

P = Phat =

0.2858 0.0414 -0.1805 10.7970 0.2839 0.0400 -0.1798 10.7011

-0.0404 0.3379 0.0136 -41.0439 -0.0411 0.3404 0.0117 -41.0608

-0.0006 -0.0000 -0.0010 1.0000 -0.0006 -0.0000 -0.0009 1.0000


キャリブレーションまとめ

• P = A(WT C)−1と分解するにはさらに作業が必要

• オブジェクト位置を精度良く設定することは困難• ノイズおよび数値的安定性のために線形解法では精度が悪い


ビジュアルサーボシステムの感度

Camera

Image Plane

Object

D

B

B

x

x

x

1

2X

Z∆

∆

f

対象物上の2点を観測

• 2点の像

ξ =

x1

x2

, x1 = −f

B

D, x2 = f

B

D

• カメラの自由度p =

∆X

∆Z


ビジュアルサーボシステムの感度

Camera

Image Plane

Object

D

B

B

x

x

x

1

2X

Z∆

∆

f

• カメラが動くことによる像の変位∂ξ

∂p=

− f

DfBD2

− fD −fB

D2

• ヤコビ行列J =

∂ξ

∂p


感度解析の例

• たとえば D = 1000, B = 100, f = 100 のとき

J =

−0.1 0.01

−0.1 −0.01

• 特異値分解 (J = UΣV T)

U =

−0.707 −0.707

−0.707 0.707

, Σ =

0.141 0.0

0.0 0.014

, V T =

1 0

0 −1

• かたい方向・柔らかい方向JV = UD, Jv1 = σ1u1, Jv2 = σ2u2


カメラの動きやすさ

カメラの運動 −→ 特徴量の運動

J

1

0

= 0.141

−0.707

−0.707

, J

0

−1

= 0.014

−0.707

0.707

Camera

Image Plane

Object

D

B

B

x

x

x

1

2X

Z∆

∆

f

X

Z

x

x

1

2

Camera Motion Feature Motion

Image Jacobian

u1

u2

v1

v2


感度の解釈

Camera

Image Plane

Object

D

B

B

x

x

x

1

2X

Z∆

∆

f

• カメラのu1 (= X)方向の運動は大きな画像の変化 (v1方向)

Image Plane

x1

x2


感度の解釈

• u2 (= −Z)方向の運動は小さな画像の変化 (v2方向)

x1

x2

Image Plane

• ビジュアルサーボは画像偏差に基づいてアクチュエータを動かすので，X方向にはかたく，Z方向には柔らかくなる．

• ヤコビ行列の特異値分解を用いることで制御システムの持つ特性を評価できる．


可操作度

• n自由度をもつマニピュレータ

– 関節角度ベクトル: q ∈ Rn，手先位置ベクトル: r ∈ Rm

– キネマティクス: r = f (q)

– ヤコビ行列: J =∂r

∂q

– 速度関係式: r = J q

– 可操作楕円体: ‖q‖ ≤ 1をみたす関節速度 qを用いて実現しうる手先速度 rの集合

∗ 主軸半径の長い方向: 大きな速度を出しやすい方向∗ 主軸半径の短い方向: 小さな速度しか出せない方向

– 可操作楕円体

1 ≥ ‖q‖2 = ‖J+v + (I − J+J)k‖2

≥ vT (J+)TJ+v, v ∈ ImJ

– 可操作度: 操作のしやすさの指標，可操作楕円体の体積

w =√

det JJT (m = nのときw = | det J |)


可操作度の例

θ

θ 1

2

l 1

l2

X

Y

• 手先位置: r，ヤコビ行列: J

r =

l1C1 + l2C12

l1S1 + l2S12

, J =

∂r

∂θ=

−l1S1 − l2S12 −l2S12

l1C1 + l2C12 l2C12

• 可操作度: w

w =√

det JJT = l1l2|S2|• l1, l2, θ1の値とは無関係に θ2 = ±π/2のときに最適姿勢


可操作楕円体

l1 = l2 = 1, θ2 = −2θ1のとき

-1.5

-1

-0.5

0

0.5

1

1.5

-0.5 0 0.5 1 1.5 2 2.5


ステレオ

��

�� 1 �� 2

��

R TR T

C

A A

C

1 21 2

1 2

12

R t

• 左右に少し離れた位置に 2台のカメラをおくと，それらのカメラの像は少し異る．その違いはカメラの相対的な位置と対象表面までの距離によって決まる．

• 左右のカメラの相対的な位置がわかり，左右のカメラで見ている対象の対応がつくならば，画像の違いを用いて距離が逆算できる．この原理を三角法と呼ぶ．


透視射影

��

�� 1 �� 2

��

R TR T

C

A A

C

1 21 2

1 2

12

R t

• 注目する点のワールド座標での位置: (X, Y, Z)

• この点の画像座標: (u1, v1), (u2, v2)

• カメラの透視投影行列: P1, P2

• このとき，

si

ui

vi

1

= Pi

X

Y

Z

1

が成り立つ．


位置の復元

• カメラ 1, 2 において

si

ui

vi

1

= Pi

X

Y

Z

1

i = 1, 2

• したがって，P1, P2, s1, s2が既知であれば，対応する画像の位置座標の組 (u1, v1), (u2, v2)からX,Y, Z を求めることができる．

X

Y

Z

1

= P+y ただし y =

s1u1

s1v1

s1

s2u2

s2v2

s2

, P =

P1

P2

• siが未知の場合はどうすれば良いか考えてみよ．


2台のカメラで見える像

C

Ce

e

m m

M

11

2

1

2

2

R1 T1

R2 T2

R t

• 内部パラメータ行列: A1, A2

• ワールド座標に対するカメラの相対的な位置・姿勢: (R1, T1), (R2, T2)

• カメラ 1を基準としたカメラ 2への回転と並進:

R = RT1 R2, t = RT

1 (T2 − T1)

• 注目点の位置: M = (X,Y, Z, 1)T

• カメラの画像面上における注目点の位置:

m1 = (u1, v1, 1)T , m2 = (u2, v2, 1)T

つまり

s1m1 = A1 [ RT1 | −RT

1 T1 ] M

s2m2 = A2 [ RT2 | −RT

2 T2 ] M


エピポーラ幾何

C

Ce

e

m m

M

11

2

1

2

2

R1 T1

R2 T2

R t

Σ

L

L

1

2

l 1

l 2

• C1, C2,Mはひとつの平面Σを定義する．この平面をエピポーラ平面と呼ぶ．

– 点m1,m2はエピポーラ平面上にある．

– エピポーラ面と画像面の交線 l1, l2をエピポーラ線という．

– 視点C1, C2を結ぶ直線と画像平面の交点 e1, e2をエピポールという．

• 空間上の点のひとつの点に対し，ひとつのエピポーラ平面が規定され，それぞれの画像平面との交線 (エピポーラ線)が定まる．エピポーラ線はエピポールを中心とした扇形の構造をなす．つまり，すべてのエピポーラ線はエピポールを通る．

• 空間上の点を複数のカメラに投影した場合には複数の画像間に特有の幾何が現れる．このような幾何のことをエピポーラ幾何という．


エピポーラ拘束

C

Ce

e

m m

M

11

2

1

2

2

R1 T1

R2 T2

R t

• カメラの画像面上における注目点の位置:

s1m1 = A1 [ RT1 | −RT

1 T1 ] M

s2m2 = A2 [ RT2 | −RT

2 T2 ] M

• M = [X, Y, Z]T , M = [MT 1]T に注意して書き直すと

s1(A1RT1 )−1m1 = M − T1

s2(A2RT2 )−1m2 = M − T2

• 両式からMを消去すると

s1R1A−11 m1 − s2R2A

−12 m2 = T2 − T1

• R = RT1 R2, t = RT

1 (T2 − T1)であるから

s1A−11 m1 − s2RA−1

2 m2 = t

• 3本のベクトルA−11 m1, RA−1

2 m2, tが同一平面上に存在する (エピポーラ拘束)


基礎方程式と基礎行列

C

Ce

e

m m

M

11

2

1

2

2

R1 T1

R2 T2

R t

• 画像(m1,m2)，カメラ内部パラメータ(A1, A2)

とカメラ間の幾何学 (R, t)の関係 (エピポーラ拘束)

s1A−11 m1 − s2RA−1

2 m2 = t

• A−11 m1と t∧ (RA−1

2 m2)は直交(∧はベクトルの外積)

• ベクトル t = (tx, ty, tz)に対して t ∧ x = Txとなる歪対称行列

T =

0 −tz tytz 0 −tx−ty tx 0

• 基礎方程式 (fundamental equation):

mT1 (A−1

1 )TTRA−12 m2 = 0

• Fを基礎行列 (fundamental matrix)という

mT1 Fm2 = 0, F = (A−1

1 )TTRA−12


基本方程式

x1

x1Rx2

m 1

m2

ひとつのカメラが移動しながら画像を撮る場合や，カメラは固定して対象が移動する場合は，まったく同じ内部パラメータのカメラ2台によるステレオとみなすことができ，そのときは基本行列 (essential matrix)

E = TR

が本質的な役割を果たす．

• エピポーラ拘束 (Rm1, m2, tは同一平面)

mT2 (t ∧Rm1) = 0

• 基本方程式mT

2 Em1 = 0


基本行列

x1

x1Rx2

m 1

m2

• カメラ座標系における対応点の3次元位置

x2i = Rx1i + t for i = 1, . . . N

xki =

kXikYikZi

for k = 1, 2, i = 1, . . . N

• 画像平面における点

mkj =

ukj

vkj

wkj

=

fXkj/Zkj

fYkj/Zkj

f

• エピポーラ拘束mT

2iEm1i = 0 for i = 1, . . . N

• 基本行列

E =

e11 e12 e13

e21 e22 e23

e31 e32 e33


基本行列の性質

• 基本行列の集合E = TR, T = t∧

• 任意の 3× 3行列Qが基本行列であるための必用十分条件

Q = UΣV T , Q ∈ E ⇐⇒ Σ = diag{λ, λ, 0}– (=⇒) : t = ‖t‖zとおく

QQT = TRRTT T = TT T = −T 2

T 2[x y z] = t ∧ t ∧ [x y z] = ‖t‖2[−x − y 0] = [x y z]diag{−‖t‖2,−‖t‖2, 0}QQT = −T 2 = [x y z]diag{‖t‖2, ‖t‖2, 0}[x y z]T

Q = [x y z]diag{‖t‖, ‖t‖, 0}V T

– (⇐=) : Q = UΣ0VTとおく．Rz = Rot(z, π/2)

Q = UΣ0RTz UTURzV

T = T0R0, T0 = UΣ0RTz UT , R0 = URzV

T

RT0 R0 = V RT

z UTURzVT = I

T T0 = URzΣ0U

T = UΣ0RzUT = −T0 (RT

z = −Rz)


基本行列の推定(8点アルゴリズム)

x1

x1Rx2

m 1

m 2

• エピポーラ拘束mT

2iEm1i = 0 for i = 1, . . . N

E =

e11 e12 e13

e21 e22 e23

e31 e32 e33

• 1点に対しひとつのエピポーラ条件が成立する．一方，基本行列 (3 × 3行列)はスカラの自由度を持つ．したがって8点が観測できれば基本行列を推定できる．

Be = 0, B =

b1...

bN

, e =

e11

e12...

e33

,

bi = [ u2ju1j u2jv1j u2js1j

v2ju1j v2jv1j v2js1j

s2ju1j s2jv1j s2js1j ]


基本行列の推定(8点アルゴリズム)

• 基本行列 (3× 3行列)はスカラの自由度を持つから‖e‖ = 1とする．

• N ≥ 8に対しエピポーラ拘束が成り立つ

Be = 0, B =

b1...

bN

, e =

e11

e12...

e33

,

bi = [ u2ju1j u2jv1j u2js1j v2ju1j v2jv1j v2js1j s2ju1j s2jv1j s2js1j ]

• したがって，基本行列の推定は下記の線形問題となるFind e ∈ R9 s.t. Be → min with ‖e‖ = 1

• この問題の解はBの最小特異値に対応する特異ベクトルとして求まる

• eからEを構成し，Eの特異値がλ, λ, 0となるように修正


運動パラメタの復元

• E = UΣV Tと特異値分解

• t = σ1u3 (u3 は U の第3列)

• R = URzVT


8点アルゴリズムI

1. {(m1i,m2i), i = 1, . . . , N}よりBを構成する．

B=zeros(N,9);

for i=1:N

u1=m1(1,i); v1=m1(2,i); w1=m1(3,i);

u2=m2(1,i); v2=m2(2,i); w2=m2(3,i);

B(i,:)=[u2*u1, u2*v1, u2*w1,

v2*u1, v2*v1, v2*w1,

w2*u1, w2*v1, w2*w1];

end

2. |Be| → minとなる e (ただし |e| = 1) を求める．

[U D V]=svd(B); e=V(:,9);

3. eからEをつくる．ただし traceETE = 2である．

E=sqrt(2)*[q(1:3)’; q(4:6)’; q(7:9)’];

# residual=trace(m2’*E*m1);


8点アルゴリズムII

4. 歪対称行列Tと回転行列Rを用いて E = TRをみたす Eのうち |E − E|を最小とする Eを求める．

[U D V]=svd(E);

D=diag((D(1)+D(2))/2, (D(1)+D(2))/2, 0);

hatE=U*D*V’;

5. EをRとTに分解する．

[U, D, V]=svd(hatE);

t=U(:,3);

Rz=[0 1 0; -1 0 0;0 0 1];

R1=U*Rz*V’;

R2=U*Rz’*V’;


8点アルゴリズムシミュレーション

0

20

40

60

80

100

120

-80 -60 -40 -20 0 20 40

• 15点観測

• f = −3; noizelevel = 0.3;

• rpy = [10 30 5]; t = [50 10 3];

• D = 1000; objsize = 100

• 画面離散化• ノイズ• 正規化


離散化なしノイズなし正規化なし

0

20

40

60

80

100

120

-80 -60 -40 -20 0 20 40

Essential Matrix:

-0.11 -0.04 0.17

0.54 -0.08 -0.81

-0.02 0.99 -0.10

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

rpy:

0.17 0.52 0.09

rpye:

0.17 0.52 0.09

t:

500 100 30

normlized t:

0.98 0.20 0.06

te:

0.98 0.20 0.06


離散化ありノイズなし正規化なし

0

20

40

60

80

100

120

-80 -60 -40 -20 0 20 40

Essential Matrix:

-0.17 -0.27 0.15

0.71 -0.00 -0.72

0.01 0.92 -0.09

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.86 -0.16 0.48

0.15 0.99 0.06

-0.48 0.02 0.88

rpy:

0.17 0.52 0.09

rpye:

0.17 0.50 0.03

t:

500 100 30

normlized t:

0.98 0.20 0.06

te:

0.94 0.19 0.28


離散化ありノイズあり正規化なし

0

20

40

60

80

100

120

-80 -60 -40 -20 0 20 40

Essential Matrix:

-0.01 0.15 0.18

0.44 -0.12 -0.85

-0.04 1.01 -0.10

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.83 -0.10 0.56

0.14 0.99 -0.03

-0.55 0.10 0.83

rpy:

0.17 0.52 0.09

rpye:

0.17 0.58 0.12

t:

500 100 30

normlized t:

0.98 0.20 0.06

te:

0.98 0.18 -0.12


離散化ありノイズあり正規化あり

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

-2 -1.5 -1 -0.5 0 0.5 1 1.5

Essential Matrix:

0.22 0.16 -0.20

-0.53 0.05 0.79

0.05 -0.98 0.10

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.87 -0.21 0.44

0.20 0.98 0.06

-0.44 0.04 0.90

rpy:

0.17 0.52 0.09

rpye:

0.23 0.46 0.04

t:

500 100 30

normlized t:

0.98 0.20 0.06

te:

0.95 0.28 0.16


離散化なしノイズなし正規化なし

0

50

100

150

200

250

300

-250 -200 -150 -100 -50 0

Essential Matrix:

-0.11 -0.04 0.17

0.54 -0.08 -0.81

-0.02 0.99 -0.10

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

rpy:

0.17 0.52 0.09

rpye:

0.17 0.52 0.09

t:

50 10 3

normlized t:

0.98 0.20 0.06

te:

0.98 0.20 0.06


離散化ありノイズなし正規化なし

0

50

100

150

200

250

300

-250 -200 -150 -100 -50 0

Essential Matrix:

0.07 0.80 0.06

-0.98 0.14 0.13

-0.15 -0.56 -0.03

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.87 -0.12 0.48

0.14 0.99 -0.01

-0.47 0.08 0.88

rpy:

0.17 0.52 0.09

rpye:

0.17 0.50 0.03

t:

50 10 3

normlized t:

0.98 0.20 0.06

te:

0.58 -0.08 0.81


離散化ありノイズあり正規化なし

0

50

100

150

200

250

300

-250 -200 -150 -100 -50 0

Essential Matrix:

0.12 0.91 0.06

-0.98 0.15 -0.06

-0.13 -0.40 -0.03

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.93 -0.09 -0.36

0.15 0.98 0.14

-0.34 0.18 -0.92

rpy:

0.17 0.52 0.09

rpye:

0.16 0.34 -0.19

t:

50 10 3

normlized t:

0.98 0.20 0.06

te:

-0.41 0.07 -0.91


離散化ありノイズあり正規化あり

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

-2 -1.5 -1 -0.5 0 0.5 1 1.5

Essential Matrix:

0.13 0.30 -0.06

-0.74 0.14 0.66

-0.11 -0.93 0.04

R:

0.85 -0.13 0.51

0.15 0.99 0.00

-0.50 0.08 0.86

Re:

0.85 -0.19 0.50

0.21 0.98 0.02

-0.49 0.09 0.87

rpy:

0.17 0.52 0.09

rpye:

0.24 0.51 0.10

t:

50 10 3

normlized t:

0.98 0.20 0.06

te:

0.94 0.09 0.32


剛体の位置と姿勢 - Tohoku University Official ...koichi/l5.pdf · 剛体の位置と姿勢...

Documents

Transcript of 剛体の位置と姿勢 - Tohoku University Official ...koichi/l5.pdf · 剛体の位置と姿勢...