Jacobian of Point Coordinates w.r.t. Parameters of General Calibrated Projective Camera

Transcription

1 Jacobian of Point Coordinates w.r.t. Parameters of General Calibrated Projective Camera Karel Lebeda, Simon Hadfield, Richard Bowden Introduction This is a supplementary technical report for ACCV04 paper: D Or Not D: Bridging the Gap Between Tracking and Structure from Motion []. It shows derivation of the Jacobian matrix of point x- and y-coordinates, with respect to parameters of a general, calibrated projective camera with 6 degrees of freedom ( for position and for rotation in the world coordinate frame). The Jacobian matrices are used in optimisation of the camera parameters, given a set of D points and their corresponding D projections. The optimisation using analytically estimated gradients generally converges faster, with lower computational costs than its numeric counterpart (using finite difference instead), while having a wider basin of convergence. We describe the derivation of the Jacobian in Section, and in Section 4 we provide the resulting C++ source code, which we believe will be of a practical value to the computer vision community. If you use this code in an academic work, please cite the paper []. Definition of Symbols and Relations Hereby, we follow a general notation of []. Assuming a D point X = (x, y, z) and a D point u = (u, v), their respective homogeneous representations are X = (κx, κy, κz, κ) and ũ = (λu, λv, λ) where κ and λ are unknown scalars. Without loss of generality we can assume that κ =. Then, the points are related by the projection equation of a general projective camera []: λ u v where P is the projection matrix: = ũ ũ ũ = ũ = P X = P [ X ]. () P = f KR [I C], ()

2 with f being the focal length (in world units, e.g. millimetres), K the matrix of intrinsic camera calibration parameters, R the rotation matrix relating the camera and world coordinate systems and C the position of the camera projection centre in the world coordinates. Assuming square pixels, we define K as: K = k 0 k k k, () where k is the focal length (in pixels, relating size of pixel to the world units and thus fixing the field of view of the camera) and (k, k ) are coordinates of the principal point. Both f and K are assumed known and fixed during the camera pose estimation. While C may be parameterised directly as C = C x C y C z, (4) this would be highly over-parameterised in the case of R as its full (orthonormal matrix) representation has 9 elements, but only degrees of freedom. Therefore we reparametrise the rotation using the angle-axis formulation: R = cos()i + sin() [a] + ( cos())aa, (5) using a rotation axis a and a rotation angle, where [ ] denotes the skew-symmetric matrix of cross product. These have in total 4 elements, which is still an over-parameterisation. However, we can exploit the nature of the rotation axis vector a and assume it is normalised to unit length. Then it is possible to represent both a and by one entity r = r r r = a, (6) = r, a = r/ r. (7) Substituting Equation (5) into (), we get the following scalar projection formulae: r ũ = (x C x ) k ( cos()) + cos () + k r r ( cos()) r sin() +(y C y ) k r r ( cos()) r sin() + k r r ( cos()) + r sin() +(z C z ) k r r ( cos()) + r sin() r + k ( cos()) + cos (), (8) ũ = (x C x ) k r r ( cos()) + r sin() + k r r ( cos()) r sin() r +(y C y ) k ( cos()) + cos () + k r r ( cos()) + r sin() +(z C z ) k r r ( cos()) r sin() r + k ( cos()) + cos (), (9)

3 and ( ũ = (x C x ) r r ( cos()) +(y C y ) +(z C z ) Then we can compute u and v simply as ) r sin() ( r r ( cos()) + r sin() ( ) r ( cos()) + cos () ). (0) u = ũ ũ, () v = ũ ũ. () Jacobian Derivation The next step is symbolic differentiation of the point coordinates and simplification of the resulting formulae [4]. We substitute Equations (7) and (8 0) into Equations (,), and differentiate with respect to all the camera parameters: J = [ r r r r r r C x C x C y C y C z C z ]. () This yields a rather complex set of derivatives. Therefore we show only two of them symbolically, as examples. Section 4 includes the expressions for the full Jacobian as source code. C x = k 4 (r (C z r C y r + r y r z) +(C z (r + r) + r r (C y y) (r + r)z) cos() +r (C y y) sin()) (r (C x r + C y r + C z r r x r y r z) +(C z (r + r) + r ( C x r C y r + r x + r y) (r + r)z) cos() + (C y r C x r + r x r y) sin()), (4)

4 r = 6 ( 8 ( (k r + k r )(C x r + C y r + C z r r x r y r z) +(C y k r r C x k r C z k (r + r) + C z k r r + C x k r r +C y k r r C x k r + k rx k r r x + k rx k r r y k r r y + k rz + k rz k r r z) cos() (C y k r + C z k r C x k r C y k r + k r x k r y +k r y k r z) sin()) (r ( C y r r C z r r + C x ( r + r + r) + rx rx rx +r r y + r r z) +(C z r r C x (rr + r r rr + rr + r r r + r) +C y r (r + r r + r (r + r)) + rr x + r rx rr x + rr x +r r rx + rx ry 4 rr y r r r y rr y r rz) cos() ( C y r + C z r (r + r) C y r r r C y r C x r (r + r r ) +r r x + rr x + ry + r r r y + ry r (r + r)z) sin()) + (r ( C x r C y r C z r + r x + r y + r z) +( C z (r + r) + r (C x r + C y r r x r y) + (r + r)z) cos() (C y r C x r + r x r y) sin()) ( 4 (C x k rr + C x k r r + C x k r r C x k rr + C x k rr +C x k r r + C x k r r r + C x k r C z (k r r + k (rr + rr + r r (r + r) r (r + r))) +C y ( k r (r + r r + r (r + r)) +k ( rr + rr + r (r + r) + r r (r + r))) k rr x k r rx k r rx + k rr x k rr x k r rx k r r rx k rx + k ry 4 + k rr y +k rr y k ry k rr y + k r r r y k r rr y +k rr y k r ry k r ry +(k r r + k (rr + rr + r r (r + r) r (r + r)))z) cos() + 5 ((C z k rr C z k rr + C z k r r C z k r +C y r (k r r + k (r r r)) C x (k r (r + r) + k r ( r + r + r)) +k r rx k rr x + k rr x + k r rx + k rx k rr y + k ry k r r r y + k r ry +r ( k r r + k ( r + r + r))z) + ( C y k rr C y k r C y k r r C y k r r r C y k r +C z r (k (r + r) + k (r r r )) +C x r ( k (r + r r ) + k (r + r)) +k r r x k r rx + k rr x k r rx + k rr y +k ry + k r r y + k r r r y + k ry r (k (r + r) + k (r r r ))z) sin()))) (r (C x r + C y r + C z r r x r y r z) +(C z (r + r) + r ( C x r C y r + r x + r y) (r + r)z) cos() + (C y r C x r + r x r y) sin()). (5) 4

5 4 Common Subexpression Elimination It is apparent that the partial derivatives contain many repeated subexpressions. Avoiding multiple evaluation of these decreases computation cost. Additionally, some of the expressions are independent of the particular point (X, u) and can be precomputed for further speed-up. We used an approach of [], which exploits the internal subexpression elimination of Mathematica [4] to generate a C++ code with minimal execution time. double v000 = pow(r,); double v00 = pow(r,); double v00 = pow(r,); double v00 = pow(r,); double v004 = pow(r,); double v005 = pow(r,); double v006 = v000 + v00 + v00; double v007 = /v006; double v008 = pow(v006,-); double v009 = sqrt(v006); double v00 = pow(v006,-.5); double v0 = sin(v009); double v0 = cos(v009); double v0 = -v0; double v04 = + v0; double v05 = r * r * v007 * v0; double v06 = v0/sqrt(v006); double v07 = r * r * v007 * v0; double v08 = r * r * v007 * v0; double v09 = r * v06; double v00 = r * v06; double v0 = r * v06; double v0 = r * r * v007 * v0; double v0 = r * r * v007 * v0; double v04 = -v06; double v05 = r * r * v007 * v0; double v06 = -v0; double v07 = -v00; double v08 = -v09; double v09 = r * r * v00 * v0; double v00 = r * r * v00 * v0; double v0 = r * r * v00 * v0; double v0 = -v00; double v0 = r * v007 * v04; double v04 = r * v007 * v04; double v05 = r * v09; double v06 = -v09; double v07 = v000 * v00 * v0; double v08 = v00 * v00 * v0; double v09 = -v0; double v040 = r * v007 * v04; double v04 = v00 * v00 * v0; double v04 = r * v0; double v04 = r * v04; double v044 = r * v0; double v045 = r * v07; double v046 = r * v07; double v047 = r * v08; double v048 = r * v04; double v049 = r * v08; double v050 = r * v04; double v05 = v000 * v007 * v04; double v05 = - * r * r * r * v008 * v04; double v05 = - * r * v000 * v008 * v04; double v054 = - * r * v000 * v008 * v04; double v055 = - * r * v00 * v008 * v04; double v056 = - * r * v00 * v008 * v04; double v057 = - * r * v00 * v008 * v04; double v058 = - * r * v00 * v008 * v04; double v059 = v0 + v05; double v060 = k * (v0 + v00 * v007 * v04); double v06 = k * v059; double v06 = k * v059; double v06 = k * (v0 + v00 * v007 * v04); double v064 = v0 - v05; double v065 = v00 + v044; double v066 = v06 + v04; double v067 = v0 - v04; double v068 = k * (v09 + v04); double v069 = k * v065; double v070 = k * v065; double v07 = k * (v0 + v04); double v07 = v07 - v044; double v07 = k * v066; double v074 = k * (v07 + v044); double v075 = k * (v08 + v04); double v076 = k * v066; double v077 = v06 + v045 + v05; double v078 = v07 + v046 + v054; double v079 = v060 + v069; double v080 = v06 + v07; double v08 = v06 + v074; double v08 = v06 + v076; double v08 = v00 * v00 * v0 - * v00 * v008 * v04 + v08 + * v040; double v084 = v068 + v07; double v085 = v070 + v075; double v086 = v00 * v007 * v0 + v06 + v05 - v04 + v05; double v087 = v00 * v007 * v0 + v04 + v05 + v08 + v05; double v088 = v05 + v0 + v0 + v045 + v05; double v089 = v0 + v09 + v04 + v046 + v054; double v090 = v08 + v09 + v040 + v047 + v055; double v09 = v05 + v0 + v040 + v048 + v056; double v09 = k * (v00 * v007 * v0 + v06 + v05 - v08 + v05) + k * v077; double v09 = k * (v07 + v049 + v057) + k * v086; double v094 = k * (v06 + v050 + v058) + k * v087; double v095 = k * (v00 * v007 * v0 + v04 + v05 + v04 + v05) + k * v078; double v096 = k * (v08 + v047 + v055) + k * v088; double v097 = k * (v08 + v048 + v056) + k * v089; double v098 = k * (v05 + v0 + v040 + v047 + v055) + k * v077; double v099 = k * (v08 + v09 + v040 + v048 + v056) + k * v078; double v00 = k * (v0 + v00 + v0 + v045 + v05) + k * v08; double v0 = k * (v07 + v04 + v06 + v046 + v054) + k * v08; double v0 = k * (v004 * v00 * v0 - * v004 * v008 * v04 + v06 + * v0)+ k * v090; double v0 = k * (v005 * v00 * v0 - * v005 * v008 * v04 + v07 + * v04) + k * v09; double v04 = k * (v000 * v007 * v0 + v06 + v05 - v07 + v05) + k * v089; double v05 = k * (v0 + v00 + v0 + v050 + v058) + k * v086; double v06 = k * (v000 * v007 * v0 + v04 + v05 + v07 + v05) + k * v088; double v07 = k * (v07 + v04 + v06 + v049 + v057) + k * v087; double v08 = k * (v0 + v09 + v04 + v049 + v057) + k * v090; double v09 = k * (v05 + v0 + v0 + v050 + v058) + k * v09; // Everything so far was point-independent, therefore could be precomputed only once for all. // Since now, we start using elements of X and therefore this needs to be done once for each X. for ( unsigned p = 0; p < NUM_POINTS; ++p ) { double x = X[p][0]; double y = X[p][]; double z = X[p][]; double v0 = -(Cz * v059) - Cy * v065 - Cx * v066 + v066 * x + v065 * y + v059 * z; double v = pow(v0,-); double v = /v0; double v = -(Cy * v079) - Cz * v08 - Cx * v084 + v084 * x + v079 * y + v08 * z; double v4 = -(Cz * v080) - Cx * v08 - Cy * v085 + v08 * x + v085 * y + v080 * z; double v5 = -(Cz * v078) - Cy * v086 - Cx * v09 + v09 * x + v086 * y + v078 * z; 5

6 double v6 = -(Cz * v077) - Cx * v087 - Cy * v090 + v087 * x + v090 * y + v077 * z; double v7 = -(Cz * v08) - Cy * v088 - Cx * v089 + v089 * x + v088 * y + v08 * z; // COEFFICIENTS FOR THE ST ROW (du by _) // dr J[p][0][0] = -(v * v4 * v5) + v * (-(Cz * v099) - Cx * v0 - Cy * v05 + v0 * x + v05 * y + v099 * z); // dr J[p][0][] = -(v * v4 * v6) + v * (-(Cz * v09) - Cx * v094 - Cy * v08 + v094 * x + v08 * y + v09 * z); // dr J[p][0][] = -(v * v4 * v7) + v * (-(Cx * v097) - Cz * v0 - Cy * v06 + v097 * x + v06 * y + v0 * z); // dcx J[p][0][] = (-v06 - v076) * v - v067 * v * v4; // dcy J[p][0][4] = (-v070 - v075) * v - v07 * v * v4; // dcz J[p][0][5] = (-v06 - v07) * v - v064 * v * v4; } //COEFFICIENTS FOR THE ND ROW (dv by _) // dr J[p][][0] = -(v * v * v5) + v * (-(Cy * v09) - Cz * v095 - Cx * v09 + v09 * x + v09 * y + v095 * z); // dr J[p][][] = -(v * v * v6) + v * (-(Cz * v098) - Cy * v0 - Cx * v07 + v07 * x + v0 * y + v098 * z); // dr J[p][][] = -(v * v * v7) + v * (-(Cy * v096) - Cz * v00 - Cx * v04 + v04 * x + v096 * y + v00 * z); // dcx J[p][][] = (-v068 - v07) * v - v067 * v * v; // dcy J[p][][4] = (-v060 - v069) * v - v07 * v * v; // dcz J[p][][5] = (-v06 - v074) * v - v064 * v * v; References [] R. S. Ballard. Converting symbolic Mathematica expressions to C code, 009. [] R. I. Hartley and A. Zisserman. Multiple View Geometry in Computer Vision. Cambridge University Press, second edition, 004. [] K. Lebeda, S. Hadfield, and R. Bowden. D or not D: Bridging the gap between tracking and structure from motion. In Proc. of ACCV, 04. [4] Wolfram Research, Inc. Mathematica, 0. 6