imgproj.c

sparse bundle ajustment的源码

  double t12;
  double t13;
  double t15;
  double t16;
  double t18;
  double t2;
  double t20;
  double t21;
  double t22;
  double t24;
  double t27;
  double t29;
  double t3;
  double t31;
  double t33;
  double t38;
  double t4;
  double t43;
  double t48;
  double t5;
  double t51;
  double t52;
  double t6;
  double t66;
  double t7;
  double t70;
  double t71;
  double t72;
  double t75;
  double t77;
  double t84;
  double t89;
  double t90;
  double t92;
  double t93;
  double t95;
  {
    t1 = a[0];
    t2 = v[1];
    t3 = t2*t2;
    t4 = 2.0*t3;
    t5 = v[2];
    t6 = t5*t5;
    t7 = 2.0*t6;
    t10 = a[4];
    t11 = v[0];
    t12 = t11*t2;
    t13 = t11*t11;
    t15 = sqrt(1.0-t13-t3-t6);
    t16 = t15*t5;
    t18 = 2.0*t12+2.0*t16;
    t20 = a[1];
    t21 = t11*t5;
    t22 = t15*t2;
    t24 = 2.0*t21-2.0*t22;
    t27 = M[0];
    t29 = M[1];
    t31 = M[2];
    t33 = -t27*t11-t2*t29-t5*t31;
    t38 = t15*t31+t11*t29-t2*t27;
    t43 = t15*t27+t2*t31-t5*t29;
    t48 = t15*t29+t27*t5-t11*t31;
    t51 = -t33*t5+t15*t38-t43*t2+t48*t11+t[2];
    t52 = 1/t51;
    t66 = -t33*t2+t15*t48-t38*t11+t43*t5+t[1];
    t70 = t51*t51;
    t71 = 1/t70;
    t72 = (t1*(-t33*t11+t15*t43-t48*t5+t38*t2+t[0])+t10*t66+t51*t20)*t71;
    jacmS[0][0] = (t1*(1.0-t4-t7)+t10*t18+t20*t24)*t52-t72*t24;
    t75 = t1*a[3];
    t77 = a[2];
    t84 = (t75*t66+t77*t51)*t71;
    jacmS[1][0] = (t75*t18+t77*t24)*t52-t84*t24;
    t89 = 2.0*t13;
    t90 = 1.0-t89-t7;
    t92 = t2*t5;
    t93 = t15*t11;
    t95 = 2.0*t92+2.0*t93;
    jacmS[0][1] = (t1*(2.0*t12-2.0*t16)+t10*t90+t20*t95)*t52-t95*t72;
    jacmS[1][1] = (t75*t90+t95*t77)*t52-t84*t95;
    t109 = 2.0*t92-2.0*t93;
    t111 = 1.0-t89-t4;
    jacmS[0][2] = (t1*(2.0*t21+2.0*t22)+t10*t109+t20*t111)*t52-t111*t72;
    jacmS[1][2] = (t75*t109+t111*t77)*t52-t84*t111;
    return;
  }
}

#include <math.h>
void calcImgProjJacRTS(double a[5],double v[3],double t[3],double M[3],
double jacmRT[2][6],double jacmS[2][3])
{
  double t1;
  double t10;
  double t102;
  double t107;
  double t109;
  double t11;
  double t112;
  double t119;
  double t12;
  double t126;
  double t13;
  double t139;
  double t142;
  double t15;
  double t152;
  double t157;
  double t16;
  double t169;
  double t17;
  double t170;
  double t173;
  double t175;
  double t177;
  double t179;
  double t18;
  double t19;
  double t192;
  double t193;
  double t195;
  double t197;
  double t2;
  double t20;
  double t21;
  double t211;
  double t213;
  double t23;
  double t25;
  double t27;
  double t29;
  double t3;
  double t33;
  double t34;
  double t36;
  double t37;
  double t38;
  double t39;
  double t4;
  double t40;
  double t44;
  double t45;
  double t46;
  double t47;
  double t49;
  double t5;
  double t51;
  double t53;
  double t54;
  double t57;
  double t6;
  double t60;
  double t63;
  double t69;
  double t7;
  double t70;
  double t8;
  double t84;
  double t88;
  double t89;
  double t9;
  double t90;
  double t93;
  double t95;
  {
    t1 = a[0];
    t2 = v[0];
    t3 = M[0];
    t4 = t2*t3;
    t5 = v[1];
    t6 = M[1];
    t7 = t5*t6;
    t8 = v[2];
    t9 = M[2];
    t10 = t8*t9;
    t11 = t2*t2;
    t12 = t5*t5;
    t13 = t8*t8;
    t15 = sqrt(1.0-t11-t12-t13);
    t16 = 1/t15;
    t17 = t15*t3;
    t18 = t5*t9;
    t19 = t8*t6;
    t20 = t17+t18-t19;
    t21 = t16*t20;
    t23 = t16*t6;
    t25 = -t23*t2-t9;
    t27 = t16*t9;
    t29 = -t2*t27+t6;
    t33 = a[4];
    t34 = t5*t3;
    t36 = t15*t6;
    t37 = t8*t3;
    t38 = t2*t9;
    t39 = t36+t37-t38;
    t40 = t16*t39;
    t44 = t15*t9;
    t45 = t2*t6;
    t46 = t16*t3;
    t47 = t2*t8;
    t49 = 2.0*t34-t40*t2+t15*t25-t2*t29-t44-t45-t47*t46;
    t51 = a[1];
    t53 = t44+t45-t34;
    t54 = t16*t53;
    t57 = t2*t5;
    t60 = 2.0*t37-t54*t2+t15*t29+t46*t57+t25*t2+t36-t38;
    t63 = -t4-t7-t10;
    t69 = -t63*t8+t15*t53-t20*t5+t39*t2+t[2];
    t70 = 1/t69;
    t84 = -t63*t5+t39*t15-t53*t2+t20*t8+t[1];
    t88 = t69*t69;
    t89 = 1/t88;
    t90 = (t1*(-t63*t2+t15*t20-t39*t8+t53*t5+t[0])+t33*t84+t51*t69)*t89;
    jacmRT[0][0] = (t1*(t4+t7+t10-t21*t2-t25*t8+t5*t29)+t33*t49+t51*t60)*t70-
t60*t90;
    t93 = t1*a[3];
    t95 = a[2];
    t102 = (t93*t84+t95*t69)*t89;
    jacmRT[1][0] = (t93*t49+t95*t60)*t70-t60*t102;
    t107 = -t46*t5+t9;
    t109 = t5*t8;
    t112 = -t27*t5-t3;
    t119 = t7+t4+t10-t40*t5-t112*t2+t107*t8;
    t126 = 2.0*t19-t54*t5+t15*t112-t107*t5-t17-t18-t23*t57;
    jacmRT[0][1] = (t1*(2.0*t45-t21*t5+t15*t107+t23*t109+t112*t5+t44-t34)+t119*
t33+t126*t51)*t70-t90*t126;
    jacmRT[1][1] = (t93*t119+t95*t126)*t70-t126*t102;
    t139 = -t46*t8-t6;
    t142 = -t23*t8+t3;
    t152 = 2.0*t18-t40*t8+t15*t142+t27*t47+t139*t8+t17-t19;
    t157 = t10+t4+t7-t54*t8-t139*t5+t142*t2;
    jacmRT[0][2] = (t1*(2.0*t38-t21*t8+t15*t139-t142*t8-t36-t37-t109*t27)+t33*
t152+t51*t157)*t70-t90*t157;
    jacmRT[1][2] = (t93*t152+t95*t157)*t70-t102*t157;
    jacmRT[0][3] = t1*t70;
    jacmRT[1][3] = 0.0;
    jacmRT[0][4] = t33*t70;
    jacmRT[1][4] = t93*t70;
    jacmRT[0][5] = t51*t70-t90;
    jacmRT[1][5] = t95*t70-t102;
    t169 = 2.0*t12;
    t170 = 2.0*t13;
    t173 = t15*t8;
    t175 = 2.0*t57+2.0*t173;
    t177 = t15*t5;
    t179 = 2.0*t47-2.0*t177;
    jacmS[0][0] = (t1*(1.0-t169-t170)+t33*t175+t179*t51)*t70-t90*t179;
    jacmS[1][0] = (t93*t175+t95*t179)*t70-t102*t179;
    t192 = 2.0*t11;
    t193 = 1.0-t192-t170;
    t195 = t15*t2;
    t197 = 2.0*t109+2.0*t195;
    jacmS[0][1] = (t1*(2.0*t57-2.0*t173)+t33*t193+t51*t197)*t70-t90*t197;
    jacmS[1][1] = (t93*t193+t95*t197)*t70-t102*t197;
    t211 = 2.0*t109-2.0*t195;
    t213 = 1.0-t192-t169;
    jacmS[0][2] = (t1*(2.0*t47+2.0*t177)+t211*t33+t51*t213)*t70-t90*t213;
    jacmS[1][2] = (t93*t211+t95*t213)*t70-t102*t213;
    return;
  }
}

#include <math.h>
void calcImgProjJacRT(double a[5],double v[3],double t[3],double M[3],
double jacmRT[2][6])
{
  double t1;
  double t10;
  double t102;
  double t107;
  double t109;
  double t11;
  double t112;
  double t119;
  double t12;
  double t126;
  double t13;
  double t139;
  double t142;
  double t15;
  double t152;
  double t157;
  double t16;
  double t17;
  double t18;
  double t19;
  double t2;
  double t20;
  double t21;
  double t23;
  double t25;
  double t27;
  double t29;
  double t3;
  double t33;
  double t34;
  double t36;
  double t37;
  double t38;
  double t39;
  double t4;
  double t40;
  double t44;
  double t45;
  double t46;
  double t47;
  double t49;
  double t5;
  double t51;
  double t53;
  double t54;
  double t57;
  double t6;
  double t60;
  double t63;
  double t69;
  double t7;
  double t70;
  double t8;
  double t84;
  double t88;
  double t89;
  double t9;
  double t90;
  double t93;
  double t95;
  {
    t1 = a[0];
    t2 = v[0];
    t3 = M[0];
    t4 = t2*t3;
    t5 = v[1];
    t6 = M[1];
    t7 = t5*t6;
    t8 = v[2];
    t9 = M[2];
    t10 = t8*t9;
    t11 = t2*t2;
    t12 = t5*t5;
    t13 = t8*t8;
    t15 = sqrt(1.0-t11-t12-t13);
    t16 = 1/t15;
    t17 = t15*t3;
    t18 = t5*t9;
    t19 = t8*t6;
    t20 = t17+t18-t19;
    t21 = t16*t20;
    t23 = t16*t6;
    t25 = -t23*t2-t9;
    t27 = t16*t9;
    t29 = -t2*t27+t6;
    t33 = a[4];
    t34 = t5*t3;
    t36 = t15*t6;
    t37 = t8*t3;
    t38 = t2*t9;
    t39 = t36+t37-t38;
    t40 = t16*t39;
    t44 = t15*t9;
    t45 = t2*t6;
    t46 = t16*t3;
    t47 = t2*t8;
    t49 = 2.0*t34-t40*t2+t15*t25-t2*t29-t44-t45-t47*t46;
    t51 = a[1];
    t53 = t44+t45-t34;
    t54 = t16*t53;
    t57 = t2*t5;
    t60 = 2.0*t37-t54*t2+t15*t29+t46*t57+t25*t2+t36-t38;
    t63 = -t4-t7-t10;
    t69 = -t63*t8+t15*t53-t20*t5+t39*t2+t[2];
    t70 = 1/t69;
    t84 = -t63*t5+t39*t15-t53*t2+t20*t8+t[1];
    t88 = t69*t69;
    t89 = 1/t88;
    t90 = (t1*(-t63*t2+t15*t20-t39*t8+t53*t5+t[0])+t33*t84+t51*t69)*t89;
    jacmRT[0][0] = (t1*(t4+t7+t10-t21*t2-t25*t8+t5*t29)+t33*t49+t51*t60)*t70-
t60*t90;
    t93 = t1*a[3];
    t95 = a[2];
    t102 = (t93*t84+t95*t69)*t89;
    jacmRT[1][0] = (t93*t49+t95*t60)*t70-t60*t102;
    t107 = -t46*t5+t9;
    t109 = t5*t8;
    t112 = -t27*t5-t3;
    t119 = t7+t4+t10-t40*t5-t112*t2+t107*t8;
    t126 = 2.0*t19-t54*t5+t15*t112-t107*t5-t17-t18-t23*t57;
    jacmRT[0][1] = (t1*(2.0*t45-t21*t5+t15*t107+t23*t109+t112*t5+t44-t34)+t119*
t33+t126*t51)*t70-t90*t126;
    jacmRT[1][1] = (t93*t119+t95*t126)*t70-t126*t102;
    t139 = -t46*t8-t6;
    t142 = -t23*t8+t3;
    t152 = 2.0*t18-t40*t8+t15*t142+t27*t47+t139*t8+t17-t19;
    t157 = t10+t4+t7-t54*t8-t139*t5+t142*t2;
    jacmRT[0][2] = (t1*(2.0*t38-t21*t8+t15*t139-t142*t8-t36-t37-t109*t27)+t33*
t152+t51*t157)*t70-t90*t157;
    jacmRT[1][2] = (t93*t152+t95*t157)*t70-t102*t157;
    jacmRT[0][3] = t1*t70;
    jacmRT[1][3] = 0.0;
    jacmRT[0][4] = t33*t70;
    jacmRT[1][4] = t93*t70;
    jacmRT[0][5] = t51*t70-t90;
    jacmRT[1][5] = t95*t70-t102;
    return;
  }
}