/* Code automatically generated by maple's codegen package */ /* Computation of the predicted projection of a 3D point and its jacobians */ #include #define IDX3x3_11 0 #define IDX3x3_12 1 #define IDX3x3_13 2 #define IDX3x3_21 3 #define IDX3x3_22 4 #define IDX3x3_23 5 #define IDX3x3_31 6 #define IDX3x3_32 7 #define IDX3x3_33 8 /* computes Q in "n", i.e. the predicted projection of a point "M" on a camera * whose rotational and translational parameters are "qr" and "t". "a" is the * camera intrisic calibration matrix */ void calcImgProj(a,qr,t,M,n) double a[9]; double qr[4]; double t[3]; double M[3]; double n[2]; { double t11; double t13; double t14; double t15; double t16; double t17; double t19; double t2; double t21; double t25; double t3; double t30; double t36; double t5; double t51; double t6; double t62; double t65; double t8; double t9; { t2 = qr[1]; t3 = M[0]; t5 = qr[2]; t6 = M[1]; t8 = qr[3]; t9 = M[2]; t11 = -t2*t3-t5*t6-t8*t9; t13 = qr[0]; t14 = t13*t13; t15 = t2*t2; t16 = t5*t5; t17 = t8*t8; t19 = 1/(t14+t15+t16+t17); t21 = t13*t19; t25 = t13*t3+t5*t9-t8*t6; t30 = t13*t6+t8*t3-t2*t9; t36 = t13*t9+t2*t6-t5*t3; t51 = -t11*t5*t19+t21*t30-t36*t2*t19+t25*t8*t19+t[1]; t62 = -t11*t8*t19+t21*t36-t25*t5*t19+t30*t2*t19+t[2]; t65 = 1/t62; n[0] = (a[IDX3x3_11]*(-t11*t2*t19+t25*t21-t30*t8*t19+t36*t5*t19+t[0])+a [IDX3x3_12]*t51+a[IDX3x3_13]*t62)*t65; n[1] = (a[IDX3x3_22]*t51+a[IDX3x3_23]*t62)*t65; return; } } /* computes A and B in "jacmRT" and "jacmS". In other words, for a point "M" * projecting on a camera whose rotational and translational parameters are "qr" * and "t", "jacmRT" is the jacobian of its predicted image projection with * regard to "qr", "t". "jacmS" is the jacobian of "M"'s predicted projection * with regard to "M". "a" is the camera intrisic calibration matrix. * Either "jacmRT" or "jacmS" can be NULL, in which case they are not computed */ void calcImgProjJacRTS(a,qr,t,M,jacmRT,jacmS) double a[9]; double qr[4]; double t[3]; double M[3]; double jacmRT[2][7]; double jacmS[2][3]; { double t1; double t10; double t11; double t110; double t114; double t115; double t116; double t118; double t12; double t120; double t127; double t129; double t13; double t130; double t137; double t138; double t14; double t140; double t141; double t143; double t147; double t15; double t153; double t156; double t16; double t162; double t17; double t172; double t18; double t187; double t188; double t19; double t191; double t197; double t2; double t20; double t21; double t210; double t220; double t228; double t24; double t240; double t241; double t242; double t243; double t247; double t248; double t250; double t253; double t254; double t256; double t26; double t269; double t27; double t272; double t273; double t275; double t28; double t289; double t29; double t291; double t3; double t30; double t33; double t34; double t35; double t37; double t38; double t39; double t4; double t40; double t42; double t43; double t45; double t46; double t47; double t48; double t5; double t50; double t53; double t54; double t57; double t6; double t60; double t61; double t62; double t64; double t65; double t66; double t68; double t69; double t7; double t71; double t72; double t75; double t78; double t79; double t8; double t80; double t82; double t83; double t84; double t86; double t87; double t9; double t95; double t96; { t1 = a[IDX3x3_11]; t2 = qr[1]; t3 = M[0]; t4 = t2*t3; t5 = qr[2]; t6 = M[1]; t7 = t5*t6; t8 = qr[3]; t9 = M[2]; t10 = t8*t9; t11 = -t4-t7-t10; t12 = t11*t2; t13 = qr[0]; t14 = t13*t13; t15 = t2*t2; t16 = t5*t5; t17 = t8*t8; t18 = t14+t15+t16+t17; t19 = t18*t18; t20 = 1/t19; t21 = t20*t13; t24 = 1/t18; t26 = t5*t9; t27 = t8*t6; t28 = t13*t3+t26-t27; t29 = t24*t28; t30 = t14*t20; t33 = t13*t24; t34 = t33*t3; t35 = t27*t24; t37 = t8*t3; t38 = t2*t9; t39 = t13*t6+t37-t38; t40 = t39*t8; t42 = 2.0*t40*t21; t43 = t26*t24; t45 = t2*t6; t46 = t5*t3; t47 = t13*t9+t45-t46; t48 = t47*t5; t50 = 2.0*t48*t21; t53 = a[IDX3x3_12]; t54 = t11*t5; t57 = t24*t39; t60 = t33*t6; t61 = t38*t24; t62 = t47*t2; t64 = 2.0*t62*t21; t65 = t37*t24; t66 = t28*t8; t68 = 2.0*t66*t21; t69 = 2.0*t54*t21+t57-2.0*t30*t39+t60-t61+t64+t65-t68; t71 = a[IDX3x3_13]; t72 = t11*t8; t75 = t24*t47; t78 = t33*t9; t79 = t46*t24; t80 = t28*t5; t82 = 2.0*t80*t21; t83 = t45*t24; t84 = t39*t2; t86 = 2.0*t21*t84; t87 = 2.0*t21*t72+t75-2.0*t30*t47+t78-t79+t82+t83-t86; t95 = -t72*t24+t33*t47-t80*t24+t84*t24+t[2]; t96 = 1/t95; t110 = -t54*t24+t33*t39-t62*t24+t66*t24+t[1]; t114 = t95*t95; t115 = 1/t114; t116 = (t1*(-t12*t24+t33*t28-t40*t24+t48*t24+t[0])+t53*t110+t71*t95)*t115; t118 = a[IDX3x3_22]; t120 = a[IDX3x3_23]; t127 = (t118*t110+t120*t95)*t115; if(jacmRT){ jacmRT[0][0] = (t1*(2.0*t12*t21+t29-2.0*t30*t28+t34-t35+t42+t43-t50)+t53* t69+t71*t87)*t96-t116*t87; jacmRT[1][0] = (t118*t69+t120*t87)*t96-t127*t87; t129 = t4*t24; t130 = t11*t24; t137 = t10*t24; t138 = t20*t2; t140 = 2.0*t40*t138; t141 = t7*t24; t143 = 2.0*t48*t138; t147 = 2.0*t54*t138; t153 = t79+t147-t86-t78-t83-t75+2.0*t47*t15*t20-2.0*t66*t138; t156 = 2.0*t72*t138; t162 = t65+t156-t64+t60+2.0*t80*t138-t61+t57-2.0*t39*t15*t20; jacmRT[0][1] = (t1*(t129-t130+2.0*t11*t15*t20-2.0*t21*t28*t2+t137+t140+t141 -t143)+t53*t153+t71*t162)*t96-t116*t162; jacmRT[1][1] = (t118*t153+t120*t162)*t96-t127*t162; t172 = t20*t5; t187 = 2.0*t66*t172; t188 = t141-t130+2.0*t11*t16*t20-2.0*t21*t39*t5+t129+t143+t137-t187; t191 = 2.0*t72*t172; t197 = t35+t191-t50-t34-t43-t29+2.0*t28*t16*t20-2.0*t84*t172; jacmRT[0][2] = (t1*(t83+t147-t82+t78+2.0*t40*t172-t79+t75-2.0*t47*t16*t20)+ t53*t188+t71*t197)*t96-t116*t197; jacmRT[1][2] = (t118*t188+t120*t197)*t96-t127*t197; t210 = t20*t8; t220 = t43+t191-t42+t34+2.0*t62*t210-t35+t29-2.0*t28*t17*t20; t228 = t137-t130+2.0*t11*t17*t20-2.0*t21*t47*t8+t141+t187+t129-t140; jacmRT[0][3] = (t1*(t61+t156-t68-t60-t65-t57+2.0*t39*t17*t20-2.0*t48*t210)+ t53*t220+t71*t228)*t96-t116*t228; jacmRT[1][3] = (t118*t220+t120*t228)*t96-t127*t228; jacmRT[0][4] = t1*t96; jacmRT[1][4] = 0.0; jacmRT[0][5] = t53*t96; jacmRT[1][5] = t96*t118; jacmRT[0][6] = t71*t96-t116; jacmRT[1][6] = t120*t96-t127; } if(jacmS){ t240 = t15*t24; t241 = t14*t24; t242 = t17*t24; t243 = t16*t24; t247 = t2*t5*t24; t248 = t33*t8; t250 = 2.0*t247+2.0*t248; t253 = t2*t8*t24; t254 = t33*t5; t256 = 2.0*t253-2.0*t254; jacmS[0][0] = (t1*(t240+t241-t242-t243)+t53*t250+t71*t256)*t96-t116*t256; jacmS[1][0] = (t118*t250+t120*t256)*t96-t127*t256; t269 = t243+t241-t240-t242; t272 = t5*t8*t24; t273 = t33*t2; t275 = 2.0*t272+2.0*t273; jacmS[0][1] = (t1*(2.0*t247-2.0*t248)+t53*t269+t71*t275)*t96-t116*t275; jacmS[1][1] = (t118*t269+t120*t275)*t96-t127*t275; t289 = 2.0*t272-2.0*t273; t291 = t242+t241-t243-t240; jacmS[0][2] = (t1*(2.0*t253+2.0*t254)+t53*t289+t71*t291)*t96-t116*t291; jacmS[1][2] = (t118*t289+t120*t291)*t96-t127*t291; } return; } }