Post on 14-Dec-2015
Image Processing and Computer Vision
Chapter 10: Pose estimation by the iterative method
(restart at week 10)
Pose estimation V4h3 1
Overview
• Define the terms• Define Structure From Motion SFM• Methods for SFM• Define pose estimation, and why we need to
study it• Newton's method• Iterative algorithm for pose estimation
Pose estimation V4h3 2
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Define the terms
3
• 3D Model=Xj =[X,Y,Z]T: where i=feature index =1,2…n features.
• X can found by manual measurement
• Pose t is the Rotation (R) and Translation (T) of the object at a time t, where t={R3x3,T3x1} t
• qti = [u;v]t
i is the image point of the ith 3D feature at time t
Xi=1=[102,18,23]T
X
Y
Z
Xi=2=[92,126,209]T
u= horizontal image position, v=vertical image position
Pose estimation V4h3
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
What is Structure From Motion SFM?• 3D Model=Xj : where j=feature index =1,2…n features
4
Time (t)t=1 t= 2 t=3 …
11333 ),( tTR
object) on the feature D-3each of ZY,(X,object theof structure
and , is at time Pose :Find To
312index feature features imageExtract
21index at time images Capture
1333 t
ti
)T(Rt
..n,i ,x
,...Γ, t
12
tiq
11
tiq
1
tniq
t
niq
21333 ),( tTR
31333 ),( tTR tTR ),( 1333
Pose estimation V4h3
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Methods of Structure From Motion SFM :3D reconstruction from N-frames
• Factorization (linear, fast, not too accurate)• Bundle adjustment BA (slower but more accurate),
can use factorization results as the first guess. – Non-linear iterative methods are more accurate than
linear method, require first guess (e.g. From factorization).– Many different implementations, but the concept is the
same.– Two-step Bundle Adjustment (a special form of Bundle
adjustment BA) • Iterative pose estimation• Iterative structure reconstruction
Pose estimation V4h3 5
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Motivation• In order to understand bundle adjustment for 3-D model
structure reconstruction from N-frames, we need to understand pose estimation first.
• Pose estimation problem definition: There are N features in a known 3D object .
• We take m pictures of the object at different views.• Input :
• We know the n model points of the object• Image sequence I1,I2,…Im.• Each image has n image feature points
• Output (structure=model, and motion=pose)• Pose (R-rotation, T-translation) of the object in 3-D at each image.• Model of the object (X, Y, Z of each of the n feature points on the
object)
Pose estimation V4h3 6
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Example: Bundle adjustment 3D reconstruction (see also http://www.cse.cuhk.edu.hk/khwong/demo/index.html)
• Grand Canyon Demo• Flask• Robot
Pose estimation V4h3 7
http://www.youtube.com/watch?v=2KLFRILlOjc
http://www.youtube.com/watch?v=4h1pN2DIs6g
http://www.youtube.com/watch?v=ONx4cyYYyrIhttp://www.youtube.com/watch?v=xgCnV--wf2k
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
The iterative SFM alternating (2-step)bundle adjustment
• Break down the system into two phases:--SFM1: find pose phase--SFM2: find model phase
• Initialize first guess of model – The first guess is a flat model perpendicular to the image
and is Zinit away (e.g. Zinit = 0.5 meters or any reasonable guess)
• Iterative while ( Err is not small )• {
– SFM1: find pose phase– SFM2: find model phase– Measurement error(Err) or(model and pose stabilized)
• }
Pose estimation V4h3 8
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Define SFM1: the pose estimation algorithm (assume the model is found or given)
• Use KLT (FeatureDetector interface (or lkdemo.c) in opencv, or http://www.cs.ubc.ca/~lowe/keypoints/) to obtain features in [u,v]T
• There are t=1,2,…, image frames, • So there are t=1={R,T} t=1 , t=2={R,T} t=2 , …., t=={R,T} t= poses.
• Given: focal length f and one model Mi=[X,Y,Z]I,with i=1,..,N features• Initialize first guess of model
– The first guess is a flat model perpendicular to the image and is Zinit away (e.g. Zinit = 0.5 meters or any reasonable guess)
– For (t=1; t<; t++) – {(for every time frame t, use all N features, run SFM1 once); – so SFM1 {SFM1: find pose : to find t } runs times here– }– After the above is run– t=1={R,T} t=1 , t=2={R,T} t=2 , …., t=={R,T} t= poses are found
Pose estimation V4h3 9
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Define SFM2: the structure estimation algorithm (assume poses are found or given)
To be discussed in the next chapter : Bundle adjustment
• Similar to pose estimation.– In pose estimation: model is known, pose is
unknown.– Here (Model finding by the iterative method)
Assume pose is known, model is unknown.– The ideas of the algorithms are similar.
Pose estimation V4h3 10
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
SFM1 : find pose (R3x3,T3x1)t
Pose estimationOne image (taken at time t) is enough
for finding the pose at time t, if the model is known.
Pose estimation V4h3 11
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Problem setting
Pose estimation V4h3 12
•
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Pose estimation problem definition (given model points and one image at t, find pose )
• There are N 3-D feature points on the model. The relative positions of the 3D features are known through measurements.
• At time t (t=1,..m)there are N image features {qi=1..,N }t
• Assume you know the correspondences for all i=1,…,N, That means:
(X,Y,Z)i=1,..N {qi=1..,N }t
• The target is to find R,T from {qi=1..,N }t
• Only one image at t is need.
3
2
1
'
'
'
model thein points featureN are There
,...,1
'
',
'
'
T
T
T
Z
Y
X
R
Z
Y
X
Ni
Z
Yf
Z
Xf
v
uq
T
i
i
i
i
i
ii
Pose estimation V4h313
ModelR (rotation), T (translation)
[Xi=1,Yi=1,Zi=1]T
[Xi=2,Yi=2,Zi=2]T
Worldcoordinates
[X’i=1,Y’i=1,Z’i=1]T
i=1,…,NTotal N-features
[X’i=2,Y’i=2,Z’i=2]T
Verri and Truccoby 3 of P333or 2chapter See
resp. axes aginst rotated angles are ,,
))cos(cos())cos(sin( +))sin()sin(cos())sin(sin( + ))cos()sin(cos(-
))cos(sin(-))cos(cos( +))sin()sin(sin(-))sin(cos( + ))cos()sin(sin(
)sin())sin(cos(-))cos(cos(
321
213132131321
213132131321
23232
r Vision-D computees for ry TechnquIntroducto
ZX,Y,
R
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Example of pose estimation
• We know the 3-D positions of the features on this box. (e.g. 4 points as shown, corners of a 10cm^3 cube)
• In the image at time t, we know the correspondences of which corners appear in the image and their image positions. (image correspondences)
• We can find R,T from this image at time t1.
Pose estimation V4h3
14Time t=0 Time t=t1
R,T
qi=1,t=0
qi=2,t=0 qi=2,t=t1
qi=1,t=t1
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 0:Pose estimation & image correspondence
a) What are the input and
output of a pose estimation algorithm?
b) How many images are enough for pose estimation?
c) Estimate the correspondences and Fill in the blanks
Pose estimation V4h3 15
R,T
qi=1,t=0
qi=2,t=0qi=2,t=t1
qi=1,t=t1
Image at Time t=t0
Image atTime t=t1
u=0 1 2 3 4 5
V543210
Timet
q1 q2 q3 q4
u v u v u v u v
t=t0 2 4 3 1 3 3 4 4
t=t1 2 3 2 0 3 2 4 2
u=0 1 2 3 4 5
q4
q3
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 1: Newton’s method (An itervative method )
• An iterative method for finding the solution of a non-linear system
• Exercise 1.Find sqrt(5), same as find the non-linear function. – f(x)=x2-5=0– Taylor series (by definition)– f(x)=f(x0)+f’(x0)*(x-x0)=0– f’(x0)=2*x0, so– f(x)=f(x0)+2*x0*(x-x0)=0
• First guess, x0=2. • f(x)=f(x0)+ f’(x0) *(x-x0)0• 0 f(x0) + f’(x0) *(x-x0)• [0-f(x0)]/f’(x0) (x-x0)• [0-(x0
2-5)]/2*x0 = x (x-x0)• [0-(x0
2-5)]/2*x0 = x• Take x0=2, [0-(22-5)]/2*2 = x• ¼= x• Since x (x-x0), • x=new guess, x0=old_guess • ¼ x-2, x 2.25• That means the next guess is x x2.25.• Exercise: Complete the steps to find the
solution.• For your reference: sqrt(5)=2.2360679 (by
calculator)
Pose estimation V4h3 16
http://www.ugrad.math.ubc.ca/coursedoc/math100/notes/approx/newton.html
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
The main idea of Newton's method
• We saw this formula before: f(x)=f(x0)+f’(x0)*(x-x0)0 -----(i)
• From f(x)=f(x0)+f’(x0)*(x-x0)0
• 0 f(x0)+f’(x0)*(x-x0)
• 0 - f(x0)= f’(x0)*(x-x0)
• [0 - f(x0)]/ f’(x0)=x=(x-x0)
• We can compute x=[0 - f(x0)]/ f’(x0), then
• Since x=(x-x0), so x=x0+ x
• That means: Xnew_guess= x0(old_guess) + x
Pose estimation V4h3 17
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Pose estimation in 3D
Pose estimation V4h3 18
qi=1,t0
qi=2,t0
Camera center
3D object at t0
3D object at t=t1
qi=1,t1
qi=2,t1
•
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 2From 3-D :P=[X,Y,Z]T to 2-D :q=[u,v]T image projection
)1(_________?_________
)1(________?_________
?___part with in the Fill :Exercise1***
students)for (exercise onsmanipulati someAfter
'
',
'
'
.length focal of camera aby captured
are projection image The
'
'
'
].,position D-3 new in the is ],point model ,motion After
s.expression hesimplify t todropped is index The
matrix rotational theis
on vector, translati theis , Ar time
3333231
3333231
,...,1
3
2
1
333231
232221
131211
3
2
1
333231
232221
131211
3211
bTZrYrXr
fv
aTZrYrXr
fu
Z
Yf
Z
Xf
v
uq
f
q
T
T
T
Z
Y
X
rrr
rrr
rrr
T
T
T
Z
Y
X
R
Z
Y
X
Z'[X',Y'Z [X,YR,T
t
rrr
rrr
rrr
R
TTTTtt
iiii
iiii
i
i
i
i
i
ii
Ni
i
i
i
i
i
i
i
i
i
T
T
Pose estimation V4h3 19
Here you relate pose (R,T) and model (X,Y,Z)i with image point (u,v)
qi=1,t
qi=2,t
Camera center
3D objectimage
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 3
•
Pose estimation V4h3 20
1
1
1
R
Verri and Truccoby 3 of P333or 2chapter See
small. are resp.) axes aginst rotated (angles ,, that whenShow
))cos(cos())cos(sin( +))sin()sin(cos())sin(sin( + ))cos()sin(cos(-
))cos(sin(-))cos(cos( +))sin()sin(sin(-))sin(cos( + ))cos()sin(sin(
)sin())sin(cos(-))cos(cos(
12
13
23
333231
232221
131211
321
213132131321
213132131321
23232
333231
232221
131211
rrr
rrr
rrr
r Vision-D computees for ry TechnquIntroducto
ZX,Y,
rrr
rrr
rrr
R
Given
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
(3b)----- termssmallorder higher )~
(),
~(~),(
termssmallorder higher )~
(),
~(
),~
(),(
(3a)----- termssmallorder higher )~
(),
~(~),(
termssmallorder higher )~
(),
~(
),~
(),(
ion,approximat seriesTalyor using
constant, a is)point model 3D a(M ed,approximat is ~~~~~~~
If
)2(),(
)2(),(
(1a),1(b) rewrite , ofpoint image theis point, model theis
resp., axes ZY,X, around anglesrotation are ,,
y.repectivel axes ZY,X, along ons translatiare ,,
pose for the Assume
6
1,
6
1,,
6
1,
6
1,,
321321
,3333231
2232221
,3333231
1131211
321
321
321321
mmm m
iviiivi
mmm m
iviiviivi
mmm m
iuiiiui
mmm m
iuiiuiiui
i
iiviii
iiii
iiuiii
iiii
iT
iiithT
iiii
TR,T
MgvMgv
MgMgMgv
similarly
MguMgu
MgMgMgu
TTT
bMgTZrYrXr
TZrYrXrfv
aMgTZrYrXr
TZrYrXrfu
PvuxiZYXM
TTT
TTTθ
Pose estimation V4h3 21
Using Taylor series http://www.fepress.org/files/math_primer_fe_taylor.pdf
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 4
•
Pose estimation V4h3 22
..)~
(),
~(
)~
(~),
~(
)~
(),
~(
...)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
etc. of instead ,,,,, use and
notationsummation he without tformula following theRewrite 4)
:Answer
? ~
and ~
of in terms ~
and ~
are What 3)
~~~~~~~ where
:Answer
?~
and between difference theis What 2)
:Answer
M of examplean give,M Describe 1)
333
22
23
111
333
222
111
6
1
654321
41
321321
ii
iviviv
iviviv
mmm m
iv
m
MgMgMg
MgMgMg
Mg
T
TTT
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Continue
•
Pose estimation V4h3 23
)4()~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(~
terms,smallorder higher sation termdifferentiorder first ),~
(),(
point image 2D theofcomponent verticalfor theSimilarly
)4()~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(~
terms,smallorder higher sation termdifferentiorder first ),~
(
termssmallorder higher sation termdifferentiorder first ~),
~(~
and ),(
~
guessed a ofcomponent horizontal image theis ~ and ,explicitly (3b) and (3a) rewrite weIf
333
,22
2
,11
1
,
333
,22
2
,11
1
,
,,
333
,22
2
,11
1
,
333
,22
2
,11
1
,
,
,
,
bTTT
MgTT
T
MgTT
T
Mg
MgMgMgvv
MgMgv
v
aTTT
MgTT
T
MgTT
T
Mg
MgMgMguu
Mg
uu
Mgu
Mgu
u
iviviv
ivivivii
ivivi
i
iuiuiu
iuiuiuii
iu
ii
iui
iui
i
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Continue
Pose estimation V4h3 24
3
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
),~
(,
),~
(,
),~
(
,),
~(
,),
~(
,),
~(
as wellAs
),~
(,
),~
(,
),~
(
,),
~(
,),
~(
,),
~(
derivaties partial thesefind toneed We
T
Mg
T
Mg
T
Mg
MgMgMg
T
Mg
T
Mg
T
Mg
MgMgMg
iviviv
iviviv
iuiuiu
iuiuiu
•
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Find partial derivatives
•
Pose estimation V4h3 25
iRi
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
iii
iii
iiui
Ri
Ri
iii
iii
Ri
Ri
iii
iii
i
i
i
i
i
ii
i
i
i
i
i
i
YZ
Xf
Z
Z
Xf
Z
Z
ZX
fZX
f
TZYXTZYX
fMgu
bZ
Yf
TZYX
TZYXfv
aZ
Xf
TZYX
TZYXfu
Z
Yf
Z
Xf
v
uq
T
T
T
Z
Y
X
TRM
Z
Y
X
R
T
21
211
1
312
123
1
,
1
312
213
312
123
3
2
1
12
13
23
12
13
23
321
),(
)5(
),5(
formulars twoabove theCombine
'
',
'
',
1
1
1
1
1
1
small are ,,, If
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
•
Pose estimation V4h3 26
22
2
3122
2
22
22
312
123
2
312
123
1
1
1
1
1
,
Ri
Rii
Rii
Ri
Ri
Rii
Ri
Rii
Ri
iRi
iii
Ri
Ri
iRi
Ri
Ri
Ri
Ri
RiR
i
Ri
Ri
Ri
iii
iii
i
Ri
Ri
iii
iii
Z
ZZXXf
ZZ
fZZ
Z
XfX
ZfZX
TZYX
Z
f
ZfZX
Z
Z
Zf
Zf
XX
Zf
ZX
fTZYX
TZYXf
u
Z
Xf
TZYX
TZYXfu
,312
123Ri
Ri
iii
iii
Z
Xf
TZYX
TZYXfu
Note
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
•
Pose estimation V4h3 27
22
3
33
312
123
3
2
312
123
2
1
312
123
1
3
312
123
3
312
123
1
0
,
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
Ri
iii
iii
i
iii
iii
i
Ri
iii
iii
i
Ri
iiii
iii
i
Ri
Ri
iii
iii
Z
fX
Z
fX
T
Z
Z
ZX
f
T
ZX
f
T
TZYXTZYX
f
T
u
T
TZYXTZYX
f
T
u
Z
f
T
TZYXTZYX
f
T
u
Z
fYTZYXTZYX
fu
Z
Xf
TZYX
TZYXfu
,312
123Ri
Ri
iii
iii
Z
Xf
TZYX
TZYXfu
Note
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
•
Pose estimation V4h328
233
22
11
33
222
211
312
213
312
123
),~
(
0),
~(
),~
(
),~
(
),~
(
),~
(
,
Ri
Riiui
iui
Ri
iui
Ri
iiui
Ri
Rii
Riiiui
Ri
Riiiui
Ri
Ri
iii
iiii
Ri
Ri
iii
iiii
Z
Xf
T
Mg
T
u
T
Mg
T
u
Z
f
T
Mg
T
u
Z
Yf
Mgu
Z
ZZXXf
Mgu
Z
XYf
Mgu
Z
Yf
TZYX
TZYXfv
Z
Xf
TZYX
TZYXfu
233
22
11
33
222
211
312
213
312
123
),~
(
),~
(
0),
~(
),~
(
),~
(
),~
(
,
Ri
Riivi
Ri
ivi
ivi
Ri
iivi
Ri
iR
iivi
Ri
Rii
Riiivi
Ri
Ri
iii
iiii
Ri
Ri
iii
iiii
Z
Yf
T
Mg
T
v
Z
f
T
Mg
T
v
T
Mg
T
v
Z
Xf
Mgv
Z
XYf
Mgv
Z
ZZYYf
Mgv
Z
Yf
TZYX
TZYXfv
Z
Xf
TZYX
TZYXfu
Similarly, so we have the results for vi ,
Exercise 5: complete the proofs on the right hand side
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 6
•
Pose estimation V4h3 29
)6(
~
~
~
~
~
~
~
~
Jocobian therepresent toUse
~
~
~
~
~
~
),~
(______
),~
(),~
(______
),~
(
),~
(),~
(),~
(______
),~
(),~
(
~
~blanks in the fill (4b), and (4a) combine
33
22
11
33
22
11
33
22
11
33
22
11
3
,
1
,
3
,
1
,
3
,
2
,
1
,
2
,
1
,
TT
TT
TTj
vv
uue
j
TT
TT
TT
T
Mg
T
MgMgMgT
Mg
T
Mg
T
MgMgMg
vv
uue
ii
ii
iviviviv
iuiuiuiuiu
ii
ii
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
continue
Pose estimation V4h330
)7(
~
~
~
~
~
~
0
0
~
~
),7(
~
~
~
~
~
~
~
~
(5) recall
),~
(),~
(),~
(),~
(),~
(),~
(
),~
(),~
(),~
(),~
(),~
(),~
(
thatproved have We
1633
22
11
33
22
11
62
222
222
12
1633
22
11
33
22
11
62
12
)12(
623
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
b
TT
TT
TT
Z
Yf
Z
f
Z
Xf
Z
XYf
Z
ZZYYf
Z
Xf
Z
f
Z
Yf
Z
ZZXXf
Z
XYf
vv
uu
soa
TT
TT
TTj
vv
uue
T
Mg
T
Mg
T
MgMgMgMgT
Mg
T
Mg
T
MgMgMgMg
j
Ri
Ri
Ri
Ri
i
Ri
iR
i
Ri
Rii
Rii
Ri
Ri
Ri
Ri
i
Ri
Rii
Rii
Ri
Rii
ii
ii
ii
iii
iviviviviviv
iuiuiuiuiuiu
•
Measured
Guessed :have hats , when guessed pose is given, guessed u,v are found by equation 5(a),5(b)
To be found (1,2,3,T1,2,3)
Calculated fromguessed(1,2,3,T1,2,3)
j2x6
Important idea
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Exercise 7(7a) Referring to previous notes and write the answers
• (7b) Rewrite equation 7(b) if measured ui=132,vi=215,
• Xi=100,Yi=200,Zi=300, focal length is 788 and guessed pose t is [0.1,0.2, 0.3, 1111,2222,3333]T. Angles are in radian.
• After you put in the above values to equation 7(b) what are the unknowns left?
Pose estimation V4h3 31
?),
~(
?),
~(
?),
~(
?),
~(
3
,
1
,
1
,
1
,
T
Mg
T
Mg
T
Mg
Mg
iv
iv
iu
iu
•
Pose estimation V4h3 32
)9(*
~
~
~
~
~
~
,:Jacobain Put the
)8(
~
~
~
~
~
~
0
0
:
:
0
0
~
~
~
~
~
~
:
~
~:
~
~
:
relationsmatrix stack the N,,1,i points, encecorrespond N have we t,frameeach For
166212
33
22
11
33
22
11
62
1
1633
22
11
33
22
11
62
222
222
1
222
222
12
1633
22
11
33
22
11
62
1
12
11
11
12
1
12
16
NN
NNi
i
NNi
Ri
Ri
Ri
Ri
i
Ri
iR
i
Ri
Rii
Rii
Ri
Ri
Ri
Ri
i
Ri
Rii
Rii
Ri
Rii
i
Ri
Ri
Ri
Ri
i
Ri
iR
i
Ri
Rii
Rii
Ri
Ri
Ri
Ri
i
Ri
Rii
Rii
Ri
Rii
N
NNi
i
NNiNi
NiNi
ii
ii
NNi
i
N
JE
TT
TT
TTj
j
J
TT
TT
TT
Z
Yf
Z
f
Z
Xf
Z
XYf
Z
ZZYYf
Z
Xf
Z
f
Z
Yf
Z
ZZXXf
Z
XYf
Z
Yf
Z
f
Z
Xf
Z
XYf
Z
ZZYYf
Z
Xf
Z
f
Z
Yf
Z
ZZXXf
Z
XYf
E
TT
TT
TTj
j
vv
uu
vv
uu
e
e
E
At time t, there are N features
. and relate :8b Exercise. term the Write:8a Exercise 3 ii jJj
• The formulas apply to one frame at time t. There are 1=1,2,…N features.
• each time t, t is found.• SFM1 will run times,
each time is independent.
From Eq. 7b
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
The Jacobian (J) can also be written as follows (J can be found when guessed pose and model M are given)
•
626
6
3
3
6
1
6
1
2
1
2
1
1
1
1
1
),~
(
),~
(
:::::
::::::
:::),
~(
),~
(
::
::::::
),~
(
),~
(
:::),
~(
),~
(
),~
(
),~
(
N
Niv
Niu
Iiv
Iiu
iv
iu
v
u
v
u
Mg
Mg
Mg
Mg
Mg
Mg
Mg
Mg
Mg
Mg
J
Pose estimation V4h3 33
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Define terms for the iterative algorithm
•
. from computed becan Then
method. squares-least aby for solvecan we
equations, are theremeasured, are encescorrespondpoint ,,1 If
camera by the measurable ispoint image :
given) are points model (allgiven ispoint model :
jacobian, thecalled is
321321
θR,T
θ
N..NI,.i
q
iM
Now
TTT
J
i
thi
T
Pose estimation V4h3 34
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
Recall: The main idea of Newton's method• We saw this formula before: f(x)=f(x0)+f’(x0)*(x-x0)0 -----(i)
• From f(x)=f(x0)+f’(x0)*(x-x0)0
• 0 - f(x0)= f’(x0)*(x-x0)----------------------(ii)
• [0 - f(x0)]/ f’(x0)=x=(x-x0)
• We can compute x=[0 - f(x0)]/ f’(x0), then
• Since x=(x-x0), so x=x0+ x
• That means: Xnew_guess= x0(old_guess) + x
• In our pose estimation algo. X becomes • E=J* , J-1*E= , this is similar to (ii) , J-11/f’(x0), E [0 - f(x0)] , x
• E=[umeasure-uguess]
• J-1*E==(k+1 new_guess)- (kth-old_guess)
• Use J-1*E = to find • since =(k+1 new_guess)- (kth-old_guess)
• (k+1_new_guess)= (kth_old_guess) + , see next slidePose estimation V4h3 35
Intro. | Motivation | Pose est.| Newton’s method | Iterative method
The iterative algorithm (SFM1)
•
Pose estimation V4h3 36
}
~~ is guessNext
enough small is ifBreak
)~
(
inverse pseudo use no if // )~
( and find
{
1max_0Iterate
*)~
(
findcan we,~
guessfirst on the Base
* so
, *
1
10
110
01
00
0
321321
1
kkk
k
kkk
-kk
kkk
k
T
-
EJ
JJE
) k,kK,k (k
EJ
TTT
ΔθEJ
JE
6233
22
11
33
22
11
321321
321321
~
~
~
~
~
~pose guessed
~
~,
~,
~,
~,
~,
~~find want to that wepose theis
,,,,,
:
NTT
TT
TT
TTT
TTT
Note
T
T
• The formulas apply to one frame at time t. There are 1=1,2,…N features.
• each time t, t is found.• SFM1 will times, each
time is independent.• In the end, t=1={R,T} t=1 , t=2={R,T} t=2 , …., t=={R,T} t= poses are found
SFM1: This algorithm is to find the pose
Summary
• Studied the iterative algorithm for pose estimation
Pose estimation V4h3 37
Appendix
Pose estimation V4h3 38
rot_syms.m : use the matlab symbolic processor to show varies possible arrangements of the Rotational matrix
• % rot_syms.m• %show varies arrangement of the • %R rotation matrix, khw 140319• syms an_x an_y an_z real• Rz=[cos(an_z) sin(an_z) 0• -sin(an_z) cos(an_z) 0• 0 0 1];• Ry=[cos(an_y) 0 -sin(an_y)• 0 1 0 • sin(an_y) 0 cos(an_y)];• Rx=[1 0 0• 0 cos(an_x) sin(an_x)• 0 -sin(an_x) cos(an_x)];• Rxyz= Rz*Ry*Rx %do x first then y, z• transpose_Rxyz= (Rz*Ry*Rx)'• inverse_Rxyz= inv(Rz*Ry*Rx)• • Rzyx= Rx*Ry*Rz %do z first then y, x• transpose_Rzyx= (Rx*Ry*Rz)' %do z first, then z, and y• inverse_Rzyx= inv(Rx*Ry*Rz)%do z first, then z, and y•
• %ANOTHER SET , IN THIS SET r_x=Rx', r_y=Ry', r_z=Rz'• rz=Rz';• ry=Ry';• rx=Rx';• • rxyz= rz*ry*rx %do x first then y, z• transpose_rxyz= (rz*ry*rx)'• inverse_rxyz= inv(rz*ry*rx)• • rzyx= rx*ry*rz %do z first then y, x• transpose_rzyx= (rx*ry*rz)' %do z first, then z, and y• inverse_Rzyx= inv(rx*ry*rz)%do z first, then z, and y
Pose estimation V4h3 39
Output of rot_syms.m (page1)• >> rot_syms• Rxyz =• [ cos(an_y)*cos(an_z), cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y)]• [ -cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z)]• [ sin(an_y), -cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]• • transpose_Rxyz =• [ cos(an_y)*cos(an_z), -cos(an_y)*sin(an_z), sin(an_y)]• [ cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), -cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z), cos(an_x)*cos(an_y)]
• inverse_Rxyz =• [ cos(an_y)*cos(an_z), -cos(an_y)*sin(an_z), sin(an_y)]• [ cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), -cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z), cos(an_x)*cos(an_y)]
• Rzyx =• [ cos(an_y)*cos(an_z), cos(an_y)*sin(an_z), -sin(an_y)]• [ cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x), cos(an_x)*cos(an_y)]
• transpose_Rzyx =• [ cos(an_y)*cos(an_z), cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y)]• [ cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x)]• [ -sin(an_y), cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]
• inverse_Rzyx =• [ cos(an_y)*cos(an_z), cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y)]• [ cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x)]• [ -sin(an_y), cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]
Pose estimation V4h340
Output of rot_syms.m (page2)
• rxyz =• [ cos(an_y)*cos(an_z), cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y)]• [ cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x)]• [ -sin(an_y), cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]• • transpose_rxyz =• [ cos(an_y)*cos(an_z), cos(an_y)*sin(an_z), -sin(an_y)]• [ cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x), cos(an_x)*cos(an_y)]
• inverse_rxyz =• [ cos(an_y)*cos(an_z), cos(an_y)*sin(an_z), -sin(an_y)]• [ cos(an_z)*sin(an_x)*sin(an_y) - cos(an_x)*sin(an_z), cos(an_x)*cos(an_z) + sin(an_x)*sin(an_y)*sin(an_z), cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) + cos(an_x)*cos(an_z)*sin(an_y), cos(an_x)*sin(an_y)*sin(an_z) - cos(an_z)*sin(an_x), cos(an_x)*cos(an_y)]
• rzyx =• [ cos(an_y)*cos(an_z), -cos(an_y)*sin(an_z), sin(an_y)]• [ cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), -cos(an_y)*sin(an_x)]• [ sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z), cos(an_x)*cos(an_y)]
• transpose_rzyx =• [ cos(an_y)*cos(an_z), cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y)]• [ -cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z)]• [ sin(an_y), -cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]
• inverse_Rzyx =• [ cos(an_y)*cos(an_z), cos(an_x)*sin(an_z) + cos(an_z)*sin(an_x)*sin(an_y), sin(an_x)*sin(an_z) - cos(an_x)*cos(an_z)*sin(an_y)]• [ -cos(an_y)*sin(an_z), cos(an_x)*cos(an_z) - sin(an_x)*sin(an_y)*sin(an_z), cos(an_z)*sin(an_x) + cos(an_x)*sin(an_y)*sin(an_z)]• [ sin(an_y), -cos(an_y)*sin(an_x), cos(an_x)*cos(an_y)]
Pose estimation V4h341
Rotation matrix
•
Pose estimation V4h3 42
sosmall, are ,,when ,
1
1
1
R
small are ,, when Also,
1)det(,,
))cos(cos())cos(sin( +))sin()sin(cos())sin(sin( + ))cos()sin(cos(-
))cos(sin(-))cos(cos( +))sin()sin(sin(-))sin(cos( + ))cos()sin(sin(
)sin())sin(cos(-))cos(cos(
321
12
13
23
333231
232221
131211
321
31
213132131321
213132131321
33232
333231
232221
131211
rrr
rrr
rrr
RIRRRR
And
rrr
rrr
rrr
R
TT
Matlab for pose Jacobian matrix (full) • %********************feb 2013*** for extended lowe******************************• function TestJacobian• % Try to solve the differentiate equations without simplification• clc,clear;• disp('TestJacobian');• syms a b c; %yaw( around x axis), pitch(around y), roll(aroudn z) respectively• syms f X Y Z T1 T2 T3;• F = [• %u• f*((cos(b)*cos(c)*X - cos(b)*sin(c)*Y + sin(b)*Z+ T1)...• /((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X ...• + (cos(a)*sin(b)*sin(c)+ sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3));• %v• ((sin(a)*sin(b)*cos(c)+ cos(a)*sin(c))*X ...• + (-sin(a)*sin(b)*sin(c)+ cos(a)*cos(c))*Y - sin(a)*sin(b)*Z + T2)...• /((-cos(a)*sin(b)*cos(c) + sin(a)*sin(c))*X + (cos(a)*sin(b)*sin(c)...• + sin(a)*cos(c))*Y + cos(a)*cos(b)*Z + T3)]• V = [a,b,c];• Fjaco = jacobian(F,V);• disp('Fjaco =');• disp(Fjaco);• size(Fjaco)• Fjaco(1,1)• %************************
Pose estimation V4h3 43
Matlab for pose Jacobian matrix (approximation)
• '===test jacobian for chang,wong ieee_mm 2 pass lowe= for lowe212.m======'• %use twist (small) angles approximation. • clear, clc;• • syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3 tt1 tt2 tt3• R=[1 -aa3 aa2; aa3 1 -aa1; -aa2 aa1 1];• dR=[1 -a3 a2; a3 1 -a1; -a2 a1 1];• M=[x;y;z];• TT=[tt1;tt2;tt3];• dt=[t1;t2;t3]• XYZ=dR*R*M+TT+dt; % R is a matrix multiplication transform• u=f*XYZ(1)/XYZ(3);• v=f*XYZ(2)/XYZ(3);• • ja=jacobian([u ;v],[a1 a2 a3])
Pose estimation V4h3 44
Alternative form: jacobian for chang,wong ieee_mm 2 pass lowe• '==========test jacobian for chang,wong ieee_mm 2 pass, for lowe212.m===='• clear• % a1=yaw, a2=pitch, a3=roll,• % t1=translation in x, t2=translation in y, t3=translation in z, • syms R dR M TT XYZ ZZ x y z f u v a1 a2 a3 t1 t2 t3 aa1 aa2 aa3 tt1 tt2 tt3
• R=[1 -aa3 aa2• aa3 1 -aa1• -aa2 aa1 1];• dR=[1 -a3 a2• a3 1 -a1• -a2 a1 1];
• M=[x;y;z];• TT=[tt1;tt2;tt3];• dt=[t1;t2;t3]• % XX=(dR.*R)*M+TT; %not correct, becuase R is a matrix multiplication transform• XYZ=dR*R*M+TT+dt; %correct, becuase R is a matrix multiplication transform• % XX=(dR+R)*M+TT; %not correct becuase R is not an addition transform• u=f*XYZ(1)/XYZ(3);• v=f*XYZ(2)/XYZ(3);• %diff (u,a3)• %diff (v,a3)• ja=jacobian([u ;v],[a1 a2 a3])• jt=jacobian([u ;v],[t1 t2 t3])
Pose estimation V4h3 45
Answer0: Exercise 0:Pose estimation & image correspondence
a) What are the input and
output of a pose estimation algorithm?
b) How many images are enough for pose estimation?
c) Estimate the correspondences and fill in the blanks
Pose estimation V4h3 46
R,T
qi=1,t=0
qi=2,t=0qi=2,t=t1
qi=1,t=t1
Image at Time t=t0
Image atTime t=t1
u=0 1 2 3 4 5
V543210
Timet
q1 q2 q3 q4
u v u v u v u v
t=t0 2 4 3 1 3 3 4 4
t=t1 2 3 2 0 3 2 4 2
u=0 1 2 3 4 5
q4
q3
Answer (a): Inputs: model of the object, (X,Y,Z)i of all i feature points, i=1,..,nOutput: R3x3,T3x1 of the object at time t
Answer (b): One
Answer 1: Newton’s method• An iterative method for finding the solution of a non-linear system• Exercise 1.Find sqrt(5), same as find the non-linear function.
sqrt(5)=2.2360679 (by calculator)– f(x)=x2-5=0– Taylor series (by definition)– f(x)=f(x0)+f’(x0)*(x-x0)=0– f’(x0)=2*x0, so– f(x)=f(x0)+2*x0*(x-x0)=0
Pose estimation V4h3 47
• Guess, x0=2.25 • f(x)=f(x0)+2*x0*(x-x0)=0• f(x)=(x02-5)+2*x0*(x-x0)=0• f(x)=(x02-5)+2*x0*(x-x0)=0• (5.06-5)+2*2.25*(x-2.25)=0• 0.06+4.5*(x-2.25)=0• X=((4.5*2.25)-0.06)/4.5• X=2.2366666 (temporally
solution, but is good enough.• ||Previous solution-current
solution||2 =||2.25-2.2366666||2=0.013333 (small enough), continue if needed...
• Otherwise the solution is• sqrt(5)=2.2366666.
http://www.ugrad.math.ubc.ca/coursedoc/math100/notes/approx/newton.html
Answer2 for exercise 2Image Projection
•
Pose estimation V4h3 48)1()/()(
)1()/()(
'
',
'
'
.length focal of camera aby captured
are projection image theSince,
)('
)('
)('
'
'
'
1 exercisefor Answer
3333231
2232221
3333231
1131211
,...,1
3333231
2232221
1131211
3
2
1
333231
232221
131211
3
2
1
bTZrYrXr
TZrYrXrfiiiiif
Z
Yfv
aTZrYrXr
TZrYrXrfiiiif
Z
Xfu
Z
Yf
Z
Xf
v
uq
f
q
iiiXTZrYrXrZ
iiXTZrYrXrY
iXTZrYrXrX
T
T
T
Z
Y
X
rrr
rrr
rrr
T
T
T
Z
Y
X
R
Z
Y
X
iii
iiiRi
Ri
i
iii
iiiRi
Ri
i
i
i
i
i
i
ii
Ni
Riiiii
Riiiii
Riiiii
i
i
i
i
i
i
i
i
i
T
Answer3 for exercise 3
•
Pose estimation V4h3 49
nscombinatioother similar to
because )*11*( , small are ,,when
1
1
1
1*11**11**1
1*1*11*
*11*1
))cos(cos())cos(sin( +))sin()sin(cos())sin(sin( + ))cos()sin(cos(-
))cos(sin(-))cos(cos( +))sin()sin(sin(-))sin(cos( + ))cos()sin(sin(
)sin())sin(cos(-))cos(cos(
equation following in thePut
sin
1cos
small is When
2133321321
12
13
23
132312
1321321
23
213132131321
213132131321
23232
333231
232221
131211
rrr
rrr
rrr
R
Answer4: Exercise 4
•
Pose estimation V4h3 50
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
)~
(),
~(
:Answer
)~
(),
~(
etc. of instead ,,,,, use and
notationsummation he without tformula following theRewrite 4)
~~,
~~:Answer
? ~
and ~
of in terms ~
and ~
are What 3)
~~~~~~~ where
~ value, truethe:Answer
?~
and between difference theis What 2)
point model a :Answer
M of examplean give,M Describe 1)
333
,22
2
,11
1
,
333
,22
2
,11
1
,
6
1
654321
1411
41
321321
ii
TTT
MgTT
T
MgTT
T
Mg
MgMgMg
Mg
T
T
TTT
guessed
iuiuiu
iuiuiu
mmm m
iv
m
•
Pose estimation V4h351
Similarly, so we have the results for vi , ??Answer5(left for student exercises): Exercise 5: complete the proves for the right hand side
233
22
11
33
222
211
312
113
312
123
),~
(
0),
~(
),~
(
),~
(
),~
(
),~
(
,
Ri
Riiui
iui
Ri
iui
Ri
iiui
Ri
Rii
Riiiui
Ri
Riiiui
Ri
Ri
iii
iii
Ri
Ri
iii
iiii
Z
Xf
T
Mg
T
u
T
Mg
T
u
Z
f
T
Mg
T
u
Z
Yf
Mgu
Z
ZZXXf
Mgu
Z
XYf
Mgu
Z
Yf
TZYX
TZYXfv
Z
Xf
TZYX
TZYXfu
233
22
11
33
222
211
312
213
312
123
),~
(
),~
(
0),
~(
),~
(
),~
(
),~
(
,
Ri
Riivi
Ri
ivi
ivi
Ri
iivi
Ri
iR
iivi
Ri
Rii
Riiivi
Ri
Ri
iii
iii
Ri
Ri
iii
iiii
Z
Yf
T
Mg
T
v
Z
f
T
Mg
T
v
T
Mg
T
v
Z
Xf
Mgv
Z
XYf
Mgv
Z
ZZYYf
Mgv
Z
Yf
TZYX
TZYXfv
Z
Xf
TZYX
TZYXfu
Answer 6: exercise 6
•
Pose estimation V4h3 52
)5(
~
~
~
~
~
~
~
~
~
~
~
~
~
~
),~
(),~
(),~
(),~
(),~
(),~
(
),~
(),~
(),~
(),~
(),~
(),~
(
~
~formmatrix ain it put (4b), and (4a) combine
33
22
11
33
22
11
33
22
11
33
22
11
3
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
3
,
2
,
1
,
TT
TT
TTj
vv
uue
TT
TT
TT
T
Mg
T
Mg
T
MgMgMgMgT
Mg
T
Mg
T
MgMgMgMg
vv
uue
ii
ii
iviviviviviv
iuiuiuiuiuiu
ii
ii
Answer7a: Exercise 7a• Check the previous notes and
Pose estimation V4h3 53
23
,
1
,
1
,
21
,
),~
(
0),
~(
),~
(
),~
(
Answer
Ri
Riiv
iv
Ri
iu
Ri
Riiiu
Z
Yf
T
Mg
T
Mg
Z
f
T
Mg
Z
XYf
Mg
?),
~(
?),
~(
?),
~(
?),
~(
3
,
1
,
1
,
1
,
T
Mg
T
Mg
T
Mg
Mg
iv
iv
iu
iu
Answer 7b•
Pose estimation V4h3 54
Tt
Ri
Ri
Ri
i
i
Ri
Ri
iii
ii
Ri
Ri
iii
iii
iii
Ri
Ri
Ri
Ri
i
Ri
iR
i
Ri
Rii
Rii
Ri
Ri
Ri
Ri
i
Ri
Rii
Rii
Ri
Rii
i
i
TTTθ
Y
Y
X
v
u
so
bZ
Yf
TZYX
TZYXfv
aZ
Xf
TZYX
TZYXfu
ZYX
TTTf
b
T
T
T
Z
Yf
Z
f
Z
Xf
Z
XYf
Z
ZZYYf
Z
Xf
Z
f
Z
Yf
Z
ZZXXf
Z
XYf
v
u
321321
312
213
312
123
321321
163
2
1
3
2
1
62
222
222
12
are unknownsonly the(7b), into above Put the
3333300200)1.0(100)2.0(
2222300)1.0(200100)3.0(
1111300)3.0(200)3.0(100
3333300200)1.0(100)2.0(
2222300)1.0(200100)3.0(788~
3333300200)1.0(100)2.0(
1111300)3.0(200)3.0(100788~
)5(
),5(
300,200,100
,3333~
,2222~
,1111~
,3.0~
,2.0~
,1.0~
since ,788 where
)7(
3333
2222
1111
3.0
2.0
1.0
0
0
~215
~132
Answer8: exercise 8
•
Pose estimation V4h3 55
3
222
222
3
0
0
term theWrite
i
Ri
Ri
Ri
Ri
i
Ri
iR
i
Ri
Rii
Rii
Ri
Ri
Ri
Ri
i
Ri
Rii
Rii
Ri
Rii
i
Z
Yf
Z
f
Z
Xf
Z
XYf
Z
ZZYYf
Z
Xf
Z
f
Z
Yf
Z
ZZXXf
Z
XYf
answer
j