Pose Estimation Using Four Corresponding Points

Pose estimation using 4 points v.4g2 1

Pose Estimation Using Four Corresponding Points

M.L. Liu and K.H. Wong, "Pose Estimation using Four Corresponding Points", Pattern Recognition Letters, Volume

20, Number 1 January 1999, pp. 69-74.

KH Wong


Introduction

• In this system, correspondences between four points on a rigid solid object and four points in an image are used to estimate the new position and orientation of that rigid object under full perspective projection. Synthetic and real data have been used to verify our method.


Problem definition• At time t, we have the model of the object A1,t, A2,t,

A3,t, A4,t, f (focal length)

• And corresponding image points a1,t, a2,t, a3,t, a4,t

• We want to find the pose l1,t, l2,t, l3,t, l4,t

• From l1,t, l2,t, l3,t, l4,t we can find the rotation and translation of the object with respect to the camera coordinates


Formulation

• P is the center of perceptivity (camera center)

• The vector PAn,t

• passes through (unit vector un,t ) passes through points an,t and An,t

• The length is ln,t

• Equation (1) and (2)

5

Error functions

• Pose estimation using 4 points v.4g2

22

,,,,2

,2

,

,,,,2

,2

,

,,

,,

,,

2 iserror The

and between distance trueThe

2 and between distance predicted The

point image theof vector D-3 theSince

, as empredict thcan But we

resp. , namely and of lengths vector knownot do weAssume

j,ti,ttitik

tjkti

ktj

ktii,j

j,ti,tj,ti,t

titik

tjkti

ktj

ktij,ti,t

Tyixii,ti,t

ktj

kti

tjtii,ti,t

AAuullll e

AAAA

uullllAA

fuuua

ll

llPAPA

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



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.




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


Netwon’s method: http://en.wikipedia.org/wiki/Newton%27s_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



The top 6 error functions

•

)1(2

)1(2

)1(2

)1(2

)1(2

)1(2

nscombinitiodifferent 4 have wej,i 1,2,34,ifor

)1(2 since

22

,,,,2

,2

,43

22

,,,,2

,2

,42

22

,,,,2

,2

,32

22

,,,,2

,2

,41

22

,,,,2

,2

,31

22

,,,,2

,2

,21

22

,,,,2

,2

,

fAAuullll e

eAAuullll e

dAAuullll e

cAAuullll e

bAAuullll e

aAAuullll e

AAuullll e

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

kti,ji

j,ti,ttitik

tjkti

ktj

ktii,j


The extra error function

•

kt

kt

kt

kt

kt

kt

kt

geeeeee

E

Set

,4,3

,4,2

,3,2

,4,1

,3,1

,2,1


Stack up all error functions

•

)4,3();4,2();3,2();2,2();4,1();3,1();2,1(:43,2,1

about functionserror such 7 are There

)(_

_seriesiki/Tayloredia.org/w//en.wikip:http(expansion seriesTaylor variableMulti Use

4

1

1

indexesmandn

e

ignoredtermshigherlleee

m

n

nn

n

mkm

km

14

Stack up all 7 error

functions •

Pose estimation using 4 points v.4g2

Tkkkkkkkk

Tkkkkkkkkk

Tkkkkkkkkk

k

Tkkkkkkkkkk

kk

kk

kk

kk

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

kt

k

n

n

kn

kn

n

mkm

km

Ni

iii

llllllllh

llllllllJEor

llllllllJE

EE

llllllllJEE

llllllll

lg

lg

lg

lg

le

le

le

le

le

le

le

geeeeee

geeeeee

E

llleee

Naxaxfafxf

axafafxf

144

133

122

111

144

133

122

111

41

431

321

211

1

1

41

431

321

211

11

41

4

31

3

21

2

11

1

4321

1

,4,1

2

,3,1

1

,3,1

4

,2,1

3

,2,1

2

,2,1

1

,2,1

,4,3

,4,2

,3,2

,4,1

,3,1

,2,1

1

1,4,3

1,4,2

1,3,2

1,4,1

1,3,1

1,2,1

1

4

1

11

1

,,, where

,,,

,,,,

0set so , errors square of vector theminimize want toWe

),definition Series(Taylor ,,,

::::::::::::

:::

::

dimension- of seriesTaylor , )()()()(

dimension one of seriesTaylor , ))((')()(

http://en.wikibooks.org/wiki/Calculus/Taylor_serieshttp://en.wikipedia.org/wiki/Taylor_series

http://en.wikibooks.org/wiki/Calculus/Taylor_series

http://en.wikipedia.org/wiki/Taylor_series


The iteration algorithm

•

loop of end1

) guessed theupdate (to

or,

,,,

)(_

,)( then

,,, where, Since

found becan and So

1equation using ... findcan wesmall toois until loop

guess agiven ,3210iteration At

used, is algorithm iterativeon Gauss_Netw a, find want towegiven, are ) points 3D of (distances model Theknwon also are points theseof )u,u,u,(u vectorsD-3hence measured, are ,,, points image , t At time

Define

1

43211

41

31

21

1

144

133

122

111

1

144

133

122

111

,3,1,2,1

0

4321

4321

4321

4321

kkLhLL

hllllllll

llllllllh

EJinversepseudoh

orEJJJh

llllllllLhhJE

J E

g,eehk

L..,,,k

L,A,A,AA

aaaallllL

kk

TkkkkTkkkk

Tkkkkkkkk

k

kTT

Tkkkkkkkkk

k

ttt

k

T


Summary

• A 4-point pose estimation algorithm is introduced.

• The mathematical formulas of the algorithm are shown.

• It is fast and accurate algorithm for pose estimation because no rotation matrix is used hence make it less complicated, stable and efficient.


Discussion

• The error function g (or e4) corresponded to the square of the length of a vector. But the other errors e1,..,e4 are the squares of the squares of the difference between two vectors. They don’t seem to match. We can try to make gg^2 and see if it works better or not. My guess is it is more or less the same because near convergence they all become very small.


Further work

• Extend to N-points– Randomly select 4 pints form the object and calculate

the pose, all the poses should agree, use RANSAC to select the best solution.

• Add Kalman filter for pose estimation to make it more robust.

• Extend it to Structure from motion– Assume all planar structure first and gradually improve

the structure results, use a 2-pass: (i) Pose (ii) Structure, alternating algorithm,.


Appendix:Modified algorithm

Warning: This modified Implementation is not very stable and accurate.

• We prefer the Jacobean J to be a square matrix, so Inverse can be applied

• Combine square-error function to make J a 4x4 matrix

• e1=e12+e23• e2=e14+e24• e3=e34+e13• e4=g


The terms of the Jacobean

•

Jacobean theof entriesother find:Exercise

)cos())cos(2(4

0)cos(22)cos(22

2 22

2 22

2 2

thembetween angle theis rs,unit vecto are , where),cos(

,simplicityfor skipped are ,

2,1212,12,1212

22

1

2,1212,12,1212

22

1

1

3,232322

32

23,23232

23

22

1

2,121212

22

12,12121

22

21

1

2

3,232322

32

2

1

2

2,132212

22

1

1

3,22,1

1

1

,,

2

,

4

4

3

4

2

4

1

4

4

3

3

3

2

3

1

3

4

2

3

2

2

2

1

2

4

1

3

1

2

1

1

1

llDllll

llDllll

lDuullll

Duullll

lDuullll

Duullll

lDuullll

lDuullll

lee

le

uuuu

AADtk

le

le

le

le

le

le

le

le

le

le

le

le

le

le

le

le

J

jijijiii

jiji


Find g or e4 (the choice of points is not the same as the paper)

•

const

const

const

const

jiji

Vtellltellltellltellltellltelllg

Vuuullluuullluuullluuullluuullluuulllg

Vululuulluulluulluull

VVVV

VVVululululululg

AAV

233322231321312

134321241311412

332332313313312312

432432413413412412

33443333323213131212

4,31,32,3

4,31,32,3334433113322

,

)(g

given is model thector when contant ve a is

)()()(

to from vector D-3 theis

33223

31322

31221

43213

41312

41211

uuuteuuuteuuuteuuuteuuuteuuute


• %*feb 2014*** jacob (a 4x4 matrix )for • %Liu M.L.and Wong K.H., "Pose Estimation • %Using Four Corresponding Points",• %Pattern Recognition Letters, Volume 20,• % Number 1 January 1999, pp. 69-74.• function four_point_algo_mlliu_khwong• clc,clear;• disp('test four_point_algo_mlliuTest Jacobian');• %assume they are all real• syms f l1 l2 l3 l4 cos_12 cos_13 cos_14 …• cos_23 cos_24 cos_34...\• D12 D13 D14 D23 D24 D34 ...\• te11 te12 te13 te21 te22 te23 real• %Vectors of all 6 combinations of 3D-points A1,A2,A3,A4• %cos_12=cos(angle between unit vectors of PA1 and PA2),and(P=camera center)• dl = [l1,l2,l3,l4]';• • %---error functions of the 6 combinations, • %see equation 3 of liu+wong paper• e12=(l1^2+l2^2-2*l1*l2*cos_12-D12)^2; • e13=(l1^2+l3^2-2*l1*l3*cos_13-D13)^2; • e14=(l1^2+l4^2-2*l1*l4*cos_14-D14)^2;• e23=(l2^2+l3^2-2*l2*l3*cos_23-D23)^2; • e24=(l2^2+l4^2-2*l2*l4*cos_24-D24)^2; • e34=(l3^2+l4^2-2*l3*l4*cos_34-D34)^2; • e1=e12+e23• e2=e14+e24• e3=e34+e13•

• % for g %--------------------------------------------

• g=l1*l2*l4*te11-l3*l1*l4*te12-l2*l3*l4*te13 ...\

• -l2*l1*l3*te21-l3*l1*l3*te22-l2*l3*l3*te23;

• e4=g;• %----------------------------------------

---------• E=[e1 e2 e3 e4 ];• J = jacobian(E,dl);• disp('J ='), disp(J), size(J),

Documents

Pose Estimation Using Four Corresponding Points