This preview has intentionally blurred sections. Sign up to view the full version.
View Full DocumentThis preview has intentionally blurred sections. Sign up to view the full version.
View Full DocumentThis preview has intentionally blurred sections. Sign up to view the full version.
View Full DocumentThis preview has intentionally blurred sections. Sign up to view the full version.
View Full Document
Unformatted text preview: X0 = (X07 Y07 Z0)T: 3—D coordinates of a point at time to
T : (T1, T2, T3)T: Translation vector spherical coordinates: slant, QT, and tilt7 ngT: T = (sinBTcosgzﬁT7 sinBTsingzﬁT7 cos6T)T. q = ((10, C117 C127 013V: Quaternion q : (sin %n7 cos %)T Rotation Matrix: qg — q? — q; + 1% 2(Qoqi + q2t13) 2(qoq2 — Inga) R 2 2(Q0611 — 61293) —q3 + (Ii — (13+ (132, 2(Q1CI2 + (Jags) 2((10612 + qu3) 2(Q1CJ2 — (10613) —q§ — (Ii + (1% + ‘19? 2’ 27 2 w=ﬂw§+w§+w§andw7é0 T
01(t) : ( ““0 sin w my sin 7” “’2 sin 7”, cos w—t ) 7
w w w 2 3—D motion: Xt R(q(t))X0 + tT. Perspective projection: X : (X17X27X3)7 Unknowns: a : (6T, ¢T,Wx,wy,wz, Z0)T.
Problem: determine a7 given {Xs7 XS+1,  ~ ~ , XSJFN}7
Where Xt = (Xt,yt)T,t =s,~7s+N7 8 Z 0 Let ht(a) be the 3—D coordinates of a point at time t, given parameter a:
ht(a) = R(q(t))X0 + tT. Given image coordinates xt, and depth Zt, its corresponding 3—D coordinates are:
Xt = (ZtXt/f7 Zth/ﬂ Zt)T~
Let p = (0, 07 1)7 then Zt = p ht(a). Novv7 Xt becomes $t/f Xt : p ht(a) yt/f The objective function to be minimized is given by: s+N E2(a) = Z H Xi — Ma) Ila $t/f Xt : p ht(a) yt/f 1
xt/f
8 8T 8T .
a—Eli(Xt—ht(a)):tp8—ai yt/f —t8—ai’ 1:1727
1
$t/f
8 (9R (9R
8—ai(Xt_ht(a)):pa—aiX0 yt/f —6—aiX0 12345
1
3150/10 JCt/f ﬁve/f
8
a—aJXt — W” = PR yo/f yt/f — R yo/f
1 1 1 Where 8T _ aT _
8&1 — 891“ — T = (sin6TcosqﬁT, sin6TsinqﬁT, COSQT)T. cos 6T cos qST 8T _ 8T _
COSHT sinng 7 Fag—@— — Sin 6T — sin 6T sin qu sin 6T cos ng q = ((107 (117 (12; Q3)T: Quaternion T
q(t) : ““0 sin w my sin 7” “’2 sin 7” cos w—t 7
w w 2 7 w 2 7 2 2 7 Rotation Matrix: qg — q? — q; + 1% 2(Qoqi + q2t13) 2(qoq2 — (Ma) R 2 2(Q0611 — 61293) —q3 + 61% — (13+ (132, 2(Q1CI2 + (Jags) 2(Q092 + (11613) 2(Q1Q2 — 61093) —(1(i— (1% + (13+ €19? w=,/w%+w§+w§,andw7é0. 3 3
Where Dk : aR/aqk, k : 0,1,2,3, are given by
QO Q1 (12 —611 QO —Q3
D0 2 2 (11 —q0 q3 ’ D1 : 2 go (11 q2 7
Q2 —Q3 —610 (13 Q2 —91
—Q2 Q3 (10 (J3 q2 —(11
D2 : 2 —C]3 —C]2 Q1 ’ D3 2 2 —Q2 (13 QO
C10 (11 Q2 (11 —(10 —Q3 7 q = ((107 (117 (12; Q3)T: Quaternion “’2 sin 7” cos w—t 7
w 2 Rotation Matrix: 2 go — q? — q; + 1% 2((10Q1 + q2t13) 2(qoq2 — (Ma) R 2 2(Q0611 — 61293) —q3 + (I? — (13+ (132, 2(Q1CI2 + (Jags) 2(Q092 + (11613) 2(Q1Q2 — 61093) —qg — (1%4’ (13+ £19? (15) w=,/w%+w§+w§,andw7é0. Now, aqk/aai, k: = 0, 1, 2,3, 2‘ = 3,4, 5, are given by wt 8(11 8mm:(i—::—§)Sin—+%Z—§CO 08—7 8me—w—w3ySiD—tw g%—— ysinw7t——%w$wyc0s2,g% (i—Z—f’ﬁln
35; = sin 353;, 5 ysm
331: : —w—3WZ sin 1% — 511130111; cos—“1,35: 93” sin 1%
35;: = (i Mm : 5: 55: sin + t wmwy 2 COSw — wt
COS 7, s+N =2  Xt— an? (17) Then the ﬁrst partial derivatives of the function E2 with respect to ai are given by: 8E2 s+N a .
8a =2Z(X h(t ))Taai(Xt—ht(a))7 1:1,27~~76. (18) The second partial derivatives are obtained by ignoring the second derivatives of the model function:
82E2 s+N a a
~2 X—h T—X—h 7 ’7'21,2,~~,6. 19
8511an E 8a1( t t(a)) 8aj( t t(a)) 1 J ( )
Let ﬁl dif —%%"§:, and or” dif %ai%2j the elements in matrix [oz ] . The minimization problem is reduced to iteratively solving the following linear equation: 2 akléal = 5k, (20)
1:1 where m is the number of unknown parameters. Here m : 6. Algorithm: TrajFit 1. Compute E2(a) in Equation (17). Set A : 0.001. 2. Compute 52‘ and 05277 Where z',j : 17 2,    7 6, using equations (18) and (19)7 respectively. 3. Compute matrix [04] by augmenting its diagonal elements: Gagj = arm(1 + /\)7 and I — i 4. Solve Equation (20) for 6(a) and evaluate E2(a + 6a). 5. If E2(a + 6a) 2 E2(a), increase A by a factor and go back to 2. 6. If E2(a + 6a) < E2(a), decrease /\ by a factor, update the trial solution a <— a + 6a, and go back to 2. ...
View
Full Document
 Spring '07
 Shah

Click to edit the document details