剛体の位置と姿勢 - tohoku university official ...koichi/l5.pdf · 剛体の位置と姿勢...
TRANSCRIPT
剛体の位置と姿勢
• 3次元空間内の剛体の位置・姿勢の表現方法
– 基準座標系: ΣA = OA–{XA, YA, ZA}– 剛体座標系: ΣB = OB–{XB, YB, ZB}– OBの位置ベクトル: ApB
– XB, YB, ZBの方向を向く単位ベクトルをΣAで表したもの: AxB, AyB, AzB
– ΣAから見た剛体の位置: ApB
– ΣAから見た剛体の姿勢: {AxB, AyB, AzB}
• 回転行列– ARB = [AxB
AyBAzB]
– (ARB)T (ARB) = I3
– (ARB)−1 = (ARB)T
ステレオ,8点アルゴリズム 1
ベクトルの表現
• Σ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
ステレオ,8点アルゴリズム 2
座標系の関係
• Σ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
ステレオ,8点アルゴリズム 3
同次変換行列
• 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
を同一視することがある.
ステレオ,8点アルゴリズム 4
同次変換
• 同次変換Ar = AT B
Br
• クイズ– ΣBはΣAと原点が一致していて,ZAまわりにα回転した位置にある.このときAT Bを求めよ.
– ΣBはΣAに対してYA方向に2,ZA方向に1だけ並進した位置にある.このときAT Bを求めよ.
ステレオ,8点アルゴリズム 5
同次変換の分解
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である.
ステレオ,8点アルゴリズム 6
座標系の回転・並進としての解釈
−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を得る.
ステレオ,8点アルゴリズム 7
座標系の回転・並進としての解釈 [注意 1]
−1 0 1 2 3 4 5−1
0
1
2
3
4
5
• ΣAをΣAに関してARB回転し,回転後の座標系Σ′Aに関して
A′pB並進してもΣBにはならない!
ステレオ,8点アルゴリズム 8
座標系の回転・並進としての解釈 [注意 2]
−1 0 1 2 3 4 5 6−1
0
1
2
3
4
5
• ΣAをΣAに関してApB並進し,ΣA
に関して ARB回転してもΣBにはならない!
ステレオ,8点アルゴリズム 9
同次変換の積
• 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
に注意する.
ステレオ,8点アルゴリズム 10
同次変換の積の解釈
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を得る
ステレオ,8点アルゴリズム 11
同次変換の積の例題,逆変換
• クイズ
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
ステレオ,8点アルゴリズム 12
線形代数の応用
• ロボットのキネマティクス• ステレオ• キャリブレーション
ステレオ,8点アルゴリズム 13
PUMA ロボットのリンク構造
ステレオ,8点アルゴリズム 14
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)である.
ステレオ,8点アルゴリズム 15
PUMA ロボットの同次変換行列 (1 → 2)
• Σ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回転.
ステレオ,8点アルゴリズム 16
PUMA ロボットの同次変換行列 (2 → 3)
• Σ2に対しΣ3は...
– Σ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
ステレオ,8点アルゴリズム 17
PUMA ロボットの同次変換行列 (3 → 4)
• Σ3に対しΣ4は...
– Σ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
ステレオ,8点アルゴリズム 18
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
ステレオ,8点アルゴリズム 19
ロボットキネマティクス(機構学)
• 関節角度θから手先(または手先に固定されたツール)の位置0r と姿勢 0R6 を求めること
• 一般的にはAr = ARB
Br + ApB
をA = 5, B = 6から順に基準座標系に向かって解いていく.
• 同次変換行列の積で書くとコンパクト0T 6 = 0T 1
1T 2 · · · 5T 6
ステレオ,8点アルゴリズム 20
インバースキネマティクス(逆機構学)
• 手先の位置姿勢 0T 6を実現する関節角度 θ
を求めること
• 非線形であり,解は一意ではない• 例
– 右利き・左利き
– エルボーアップ・エルボーダウン
ステレオ,8点アルゴリズム 21
透視射影変換
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 はさらに複雑になる.
ステレオ,8点アルゴリズム 22
ビジョンシステム
World�
CoordinateSystem
ObjectCoordinateSystemCamera
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
ステレオ,8点アルゴリズム 23
キャリブレーション
World�
CoordinateSystem
ObjectCoordinateSystemCamera
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
• あとで詳しく説明する.
ステレオ,8点アルゴリズム 24
透視射影変換
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 はさらに複雑になる.
ステレオ,8点アルゴリズム 25
ビジョンシステム
World�
CoordinateSystem
ObjectCoordinateSystemCamera
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
ステレオ,8点アルゴリズム 26
キャリブレーション
World�
CoordinateSystem
ObjectCoordinateSystemCamera
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
を同定する問題.
ステレオ,8点アルゴリズム 27
ビジョンシステムのキャリブレーション
World�
CoordinateSystem
ObjectCoordinateSystemCamera
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は決定可能.
• しかしノイズの影響などにより,精度よく推定するためには数十程度の点を計測する必要があり,現在もまだアクティブな研究対象である.
ステレオ,8点アルゴリズム 28
キャリブレーションアルゴリズム
透視変換のモデル:
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)
となる.
ステレオ,8点アルゴリズム 29
キャリブレーションアルゴリズム
• 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
ステレオ,8点アルゴリズム 30
キャリブレーションの例
• 透視投影モデル:
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
]
ステレオ,8点アルゴリズム 31
数値例
• 観察対象: 一辺 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
ステレオ,8点アルゴリズム 32
数値例
• 観察対象: 一辺 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
ステレオ,8点アルゴリズム 33
キャリブレーションまとめ
• P = A(WT C)−1と分解するにはさらに作業が必要
• オブジェクト位置を精度良く設定することは困難• ノイズおよび数値的安定性のために線形解法では精度が悪い
ステレオ,8点アルゴリズム 34
ビジュアルサーボシステムの感度
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
ステレオ,8点アルゴリズム 35
ビジュアルサーボシステムの感度
Camera
Image Plane
Object
D
B
B
x
x
x
1
2X
Z∆
∆
f
• カメラが動くことによる像の変位∂ξ
∂p=
− f
DfBD2
− fD −fB
D2
• ヤコビ行列J =
∂ξ
∂p
ステレオ,8点アルゴリズム 36
感度解析の例
• たとえば 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
ステレオ,8点アルゴリズム 37
カメラの動きやすさ
カメラの運動 −→ 特徴量の運動
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
ステレオ,8点アルゴリズム 38
感度の解釈
Camera
Image Plane
Object
D
B
B
x
x
x
1
2X
Z∆
∆
f
• カメラのu1 (= X)方向の運動は大きな画像の変化 (v1方向)
Image Plane
x1
x2
ステレオ,8点アルゴリズム 39
感度の解釈
• u2 (= −Z)方向の運動は小さな画像の変化 (v2方向)
x1
x2
Image Plane
• ビジュアルサーボは画像偏差に基づいてアクチュエータを動かすので,X方向にはかたく,Z方向には柔らかくなる.
• ヤコビ行列の特異値分解を用いることで制御システムの持つ特性を評価できる.
ステレオ,8点アルゴリズム 40
可操作度
• 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 |)
ステレオ,8点アルゴリズム 41
可操作度の例
θ
θ 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のときに最適姿勢
ステレオ,8点アルゴリズム 42
可操作楕円体
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
ステレオ,8点アルゴリズム 43
ステレオ
����� ����
��� 1 ��� 2
��� ������
R TR T
C
A A
C
1 21 2
1 2
12
R t
• 左右に少し離れた位置に 2台のカメラをおくと,それらのカメラの像は少し異る.その違いはカメラの相対的な位置と対象表面までの距離によって決まる.
• 左右のカメラの相対的な位置がわかり,左右のカメラで見ている対象の対応がつくならば,画像の違いを用いて距離が逆算できる.この原理を三角法と呼ぶ.
ステレオ,8点アルゴリズム 44
透視射影
����� ����
��� 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
が成り立つ.
ステレオ,8点アルゴリズム 45
位置の復元
• カメラ 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が未知の場合はどうすれば良いか考えてみよ.
ステレオ,8点アルゴリズム 46
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
ステレオ,8点アルゴリズム 47
エピポーラ幾何
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をエピポールという.
• 空間上の点のひとつの点に対し,ひとつのエピポーラ平面が規定され,それぞれの画像平面との交線 (エピポーラ線)が定まる.エピポーラ線はエピポールを中心とした扇形の構造をなす.つまり,すべてのエピポーラ線はエピポールを通る.
• 空間上の点を複数のカメラに投影した場合には複数の画像間に特有の幾何が現れる.このような幾何のことをエピポーラ幾何という.
ステレオ,8点アルゴリズム 48
エピポーラ拘束
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が同一平面上に存在する (エピポーラ拘束)
ステレオ,8点アルゴリズム 49
基礎方程式と基礎行列
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
ステレオ,8点アルゴリズム 50
基本方程式
x1
x1Rx2
m 1
m2
ひとつのカメラが移動しながら画像を撮る場合や,カメラは固定して対象が移動する場合は,まったく同じ内部パラメータのカメラ2台によるステレオとみなすことができ,そのときは基本行列 (essential matrix)
E = TR
が本質的な役割を果たす.
• エピポーラ拘束 (Rm1, m2, tは同一平面)
mT2 (t ∧Rm1) = 0
• 基本方程式mT
2 Em1 = 0
ステレオ,8点アルゴリズム 51
基本行列
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
ステレオ,8点アルゴリズム 52
基本行列の性質
• 基本行列の集合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点アルゴリズム 53
基本行列の推定(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点アルゴリズム 54
基本行列の推定(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となるように修正
ステレオ,8点アルゴリズム 55
運動パラメタの復元
• E = UΣV Tと特異値分解
• t = σ1u3 (u3 は U の第3列)
• R = URzVT
ステレオ,8点アルゴリズム 56
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点アルゴリズム 57
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点アルゴリズム 58
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
• 画面離散化• ノイズ• 正規化
ステレオ,8点アルゴリズム 59
離散化なしノイズなし正規化なし
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
ステレオ,8点アルゴリズム 60
離散化ありノイズなし正規化なし
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
ステレオ,8点アルゴリズム 61
離散化ありノイズあり正規化なし
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
ステレオ,8点アルゴリズム 62
離散化ありノイズあり正規化あり
-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
ステレオ,8点アルゴリズム 63
離散化なしノイズなし正規化なし
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
ステレオ,8点アルゴリズム 64
離散化ありノイズなし正規化なし
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
ステレオ,8点アルゴリズム 65
離散化ありノイズあり正規化なし
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
ステレオ,8点アルゴリズム 66
離散化ありノイズあり正規化あり
-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
ステレオ,8点アルゴリズム 67