Image Processing and Computer Vision Chapter 10: Pose estimation by the iterative method (restart at...

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


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


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



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



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)



Example: Bundle adjustment 3D reconstruction (see also http://www.cse.cuhk.edu.hk/khwong/demo/index.html)

• Grand Canyon Demo• Flask• Robot


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


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)

• }



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



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.



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.



Problem setting


•


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


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


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.


14Time t=0 Time t=t1

R,T

qi=1,t=0

qi=2,t=0 qi=2,t=t1

qi=1,t=t1


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


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


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)


http://www.ugrad.math.ubc.ca/coursedoc/math100/notes/approx/newton.html




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 in 3D


qi=1,t0

qi=2,t0

Camera center

3D object at t0

3D object at t=t1

qi=1,t1

qi=2,t1

•


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


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


Exercise 3

•


1

1

1

R

Verri and Truccoby 3 of P333or 2chapter See

small. are resp.) axes aginst rotated (angles ,, that whenShow




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


(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θ


Using Taylor series http://www.fepress.org/files/math_primer_fe_taylor.pdf


Exercise 4

•


..)~

(),

~(

)~

(~),

~(

)~

(),

~(

...)~

(),

~(

)~

(),

~(

)~

(),

~(

)~

(),

~(

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


Continue

•


)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


Continue


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

•


Find partial derivatives

•


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


•


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


•


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


•


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


Exercise 6

•


)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


continue


)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


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?


?),

~(

?),

~(

?),

~(

?),

~(

3

,

1

,

1

,

1

,

T

Mg

T

Mg

T

Mg

Mg

iv

iv

iu

iu

•


)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


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



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



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


The iterative algorithm (SFM1)

•


}

~~ 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


Appendix


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


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)]


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)]


Rotation matrix

•


sosmall, are ,,when ,

1

1

1

R

small are ,, when Also,

1)det(,,




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)• %************************


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])


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])


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


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


• 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.



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

•


nscombinatioother similar to

because )*11*( , small are ,,when

1

1

1

1*11**11**1

1*1*11*

*11*1




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

•


)~

(),

~(

)~

(),

~(

)~

(),

~(

)~

(),

~(

)~

(),

~(

)~

(),

~(

: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

•


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

•


)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


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•


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

•


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

Date post:	14-Dec-2015
Category:	Documents
Upload:	dangelo-edgerly
View:	215 times
Download:	0 times

Image Processing and Computer Vision Chapter 10: Pose estimation by the iterative method (restart at...

Documents