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
Pose estimation using 4 points v.4g2 2
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.
Pose estimation using 4 points v.4g2 3
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
Pose estimation using 4 points v.4g2 4
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)
Pose estimation using 4 points v.4g2 6
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
Pose estimation using 4 points v.4g2 7
• 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
Pose estimation using 4 points v.4g2 8
Using Taylor series http://www.fepress.org/files/math_primer_fe_taylor.pdf
Pose estimation using 4 points v.4g2 9
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
Pose estimation using 4 points v.4g2 10
Pose estimation using 4 points v.4g2 11
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
Pose estimation using 4 points v.4g2 12
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
Pose estimation using 4 points v.4g2 13
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
Pose estimation using 4 points v.4g2 15
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
Pose estimation using 4 points v.4g2 16
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.
Pose estimation using 4 points v.4g2 17
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.
Pose estimation using 4 points v.4g2 18
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,.
Pose estimation using 4 points v.4g2 19
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
Pose estimation using 4 points v.4g2 20
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
Pose estimation using 4 points v.4g2 21
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
Pose estimation using 4 points v.4g2 22
• %*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),