imgproj.c

sparse bundle ajustment的源码

/* Code automatically generated by maple's codegen package */

/* Computation of the predicted projection of a 3D point and its jacobians */

#include "eucsbademo.h"

#include <math.h>
void calcImgProj(double a[5],double v[3],double t[3],double M[3],double n[2])
{
  double t1;
  double t11;
  double t13;
  double t14;
  double t15;
  double t17;
  double t2;
  double t21;
  double t26;
  double t3;
  double t31;
  double t42;
  double t5;
  double t50;
  double t53;
  double t6;
  double t8;
  double t9;
  {
    t1 = a[0];
    t2 = v[0];
    t3 = M[0];
    t5 = v[1];
    t6 = M[1];
    t8 = v[2];
    t9 = M[2];
    t11 = -t2*t3-t5*t6-t8*t9;
    t13 = t2*t2;
    t14 = t5*t5;
    t15 = t8*t8;
    t17 = sqrt(1.0-t13-t14-t15);
    t21 = t17*t3+t5*t9-t8*t6;
    t26 = t17*t6+t8*t3-t2*t9;
    t31 = t17*t9+t2*t6-t5*t3;
    t42 = -t11*t5+t17*t26-t2*t31+t21*t8+t[1];
    t50 = -t11*t8+t17*t31-t21*t5+t2*t26+t[2];
    t53 = 1/t50;
    n[0] = (t1*(-t11*t2+t17*t21-t26*t8+t31*t5+t[0])+a[4]*t42+a[1]*t50)*t53;
    n[1] = (t1*a[3]*t42+a[2]*t50)*t53;
    return;
  }
}

#include <math.h>
void calcImgProjJacKRTS(double a[5],double v[3],double t[3],double M[3],
double jacmKRT[2][11],double jacmS[2][3])
{
  double t1;
  double t10;
  double t104;
  double t109;
  double t111;
  double t114;
  double t12;
  double t121;
  double t128;
  double t13;
  double t14;
  double t141;
  double t144;
  double t154;
  double t159;
  double t16;
  double t17;
  double t171;
  double t172;
  double t175;
  double t177;
  double t179;
  double t18;
  double t181;
  double t19;
  double t194;
  double t195;
  double t197;
  double t199;
  double t2;
  double t20;
  double t213;
  double t215;
  double t22;
  double t23;
  double t24;
  double t25;
  double t27;
  double t28;
  double t29;
  double t3;
  double t30;
  double t33;
  double t39;
  double t4;
  double t40;
  double t41;
  double t47;
  double t49;
  double t5;
  double t51;
  double t52;
  double t54;
  double t56;
  double t58;
  double t6;
  double t60;
  double t64;
  double t66;
  double t7;
  double t70;
  double t71;
  double t73;
  double t75;
  double t77;
  double t8;
  double t80;
  double t83;
  double t9;
  double t91;
  double t92;
  double t93;
  double t95;
  double t97;
  {
    t1 = v[0];
    t2 = M[0];
    t3 = t1*t2;
    t4 = v[1];
    t5 = M[1];
    t6 = t4*t5;
    t7 = v[2];
    t8 = M[2];
    t9 = t7*t8;
    t10 = -t3-t6-t9;
    t12 = t1*t1;
    t13 = t4*t4;
    t14 = t7*t7;
    t16 = sqrt(1.0-t12-t13-t14);
    t17 = t16*t2;
    t18 = t4*t8;
    t19 = t7*t5;
    t20 = t17+t18-t19;
    t22 = t16*t5;
    t23 = t7*t2;
    t24 = t1*t8;
    t25 = t22+t23-t24;
    t27 = t16*t8;
    t28 = t1*t5;
    t29 = t4*t2;
    t30 = t27+t28-t29;
    t33 = -t1*t10+t16*t20-t25*t7+t30*t4+t[0];
    t39 = -t10*t7+t16*t30-t20*t4+t25*t1+t[2];
    t40 = 1/t39;
    jacmKRT[0][0] = t33*t40;
    t41 = a[3];
    t47 = -t4*t10+t16*t25-t30*t1+t20*t7+t[1];
    jacmKRT[1][0] = t41*t47*t40;
    jacmKRT[0][1] = 1.0;
    jacmKRT[1][1] = 0.0;
    jacmKRT[0][2] = 0.0;
    jacmKRT[1][2] = 1.0;
    jacmKRT[0][3] = 0.0;
    t49 = a[0];
    jacmKRT[1][3] = t49*t47*t40;
    jacmKRT[0][4] = t47*t40;
    jacmKRT[1][4] = 0.0;
    t51 = 1/t16;
    t52 = t51*t20;
    t54 = t51*t5;
    t56 = -t54*t1-t8;
    t58 = t51*t8;
    t60 = -t58*t1+t5;
    t64 = a[4];
    t66 = t51*t25;
    t70 = t51*t2;
    t71 = t1*t7;
    t73 = 2.0*t29-t66*t1+t16*t56-t60*t1-t27-t28-t70*t71;
    t75 = a[1];
    t77 = t51*t30;
    t80 = t4*t1;
    t83 = 2.0*t23-t77*t1+t16*t60+t70*t80+t56*t1+t22-t24;
    t91 = t39*t39;
    t92 = 1/t91;
    t93 = (t49*t33+t64*t47+t75*t39)*t92;
    jacmKRT[0][5] = (t49*(t3+t6+t9-t52*t1-t56*t7+t60*t4)+t64*t73+t75*t83)*t40-
t93*t83;
    t95 = t49*t41;
    t97 = a[2];
    t104 = (t95*t47+t97*t39)*t92;
    jacmKRT[1][5] = (t95*t73+t97*t83)*t40-t104*t83;
    t109 = -t70*t4+t8;
    t111 = t4*t7;
    t114 = -t58*t4-t2;
    t121 = t6+t3+t9-t66*t4-t114*t1+t109*t7;
    t128 = 2.0*t19-t77*t4+t114*t16-t109*t4-t17-t18-t54*t80;
    jacmKRT[0][6] = (t49*(2.0*t28-t52*t4+t16*t109+t54*t111+t114*t4+t27-t29)+t64
*t121+t75*t128)*t40-t93*t128;
    jacmKRT[1][6] = (t95*t121+t97*t128)*t40-t104*t128;
    t141 = -t70*t7-t5;
    t144 = -t54*t7+t2;
    t154 = 2.0*t18-t66*t7+t144*t16+t58*t71+t141*t7+t17-t19;
    t159 = t9+t3+t6-t77*t7-t141*t4+t144*t1;
    jacmKRT[0][7] = (t49*(2.0*t24-t52*t7+t16*t141-t144*t7-t22-t23-t58*t111)+t64
*t154+t75*t159)*t40-t93*t159;
    jacmKRT[1][7] = (t95*t154+t97*t159)*t40-t104*t159;
    jacmKRT[0][8] = t49*t40;
    jacmKRT[1][8] = 0.0;
    jacmKRT[0][9] = t64*t40;
    jacmKRT[1][9] = t95*t40;
    jacmKRT[0][10] = t75*t40-t93;
    jacmKRT[1][10] = t97*t40-t104;
    t171 = 2.0*t13;
    t172 = 2.0*t14;
    t175 = t16*t7;
    t177 = 2.0*t80+2.0*t175;
    t179 = t16*t4;
    t181 = 2.0*t71-2.0*t179;
    jacmS[0][0] = (t49*(1.0-t171-t172)+t64*t177+t75*t181)*t40-t93*t181;
    jacmS[1][0] = (t95*t177+t97*t181)*t40-t104*t181;
    t194 = 2.0*t12;
    t195 = 1.0-t194-t172;
    t197 = t16*t1;
    t199 = 2.0*t111+2.0*t197;
    jacmS[0][1] = (t49*(2.0*t80-2.0*t175)+t64*t195+t75*t199)*t40-t93*t199;
    jacmS[1][1] = (t95*t195+t97*t199)*t40-t104*t199;
    t213 = 2.0*t111-2.0*t197;
    t215 = 1.0-t194-t171;
    jacmS[0][2] = (t49*(2.0*t71+2.0*t179)+t64*t213+t75*t215)*t40-t93*t215;
    jacmS[1][2] = (t95*t213+t97*t215)*t40-t104*t215;
    return;
  }
}

#include <math.h>
void calcImgProjJacKRT(double a[5],double v[3],double t[3],double M[3],
double jacmKRT[2][11])
{
  double t1;
  double t10;
  double t104;
  double t109;
  double t111;
  double t114;
  double t12;
  double t121;
  double t128;
  double t13;
  double t14;
  double t141;
  double t144;
  double t154;
  double t159;
  double t16;
  double t17;
  double t18;
  double t19;
  double t2;
  double t20;
  double t22;
  double t23;
  double t24;
  double t25;
  double t27;
  double t28;
  double t29;
  double t3;
  double t30;
  double t33;
  double t39;
  double t4;
  double t40;
  double t41;
  double t47;
  double t49;
  double t5;
  double t51;
  double t52;
  double t54;
  double t56;
  double t58;
  double t6;
  double t60;
  double t64;
  double t66;
  double t7;
  double t70;
  double t71;
  double t73;
  double t75;
  double t77;
  double t8;
  double t80;
  double t83;
  double t9;
  double t91;
  double t92;
  double t93;
  double t95;
  double t97;
  {
    t1 = v[0];
    t2 = M[0];
    t3 = t1*t2;
    t4 = v[1];
    t5 = M[1];
    t6 = t4*t5;
    t7 = v[2];
    t8 = M[2];
    t9 = t7*t8;
    t10 = -t3-t6-t9;
    t12 = t1*t1;
    t13 = t4*t4;
    t14 = t7*t7;
    t16 = sqrt(1.0-t12-t13-t14);
    t17 = t16*t2;
    t18 = t4*t8;
    t19 = t7*t5;
    t20 = t17+t18-t19;
    t22 = t16*t5;
    t23 = t7*t2;
    t24 = t1*t8;
    t25 = t22+t23-t24;
    t27 = t16*t8;
    t28 = t1*t5;
    t29 = t4*t2;
    t30 = t27+t28-t29;
    t33 = -t1*t10+t16*t20-t25*t7+t30*t4+t[0];
    t39 = -t10*t7+t16*t30-t20*t4+t25*t1+t[2];
    t40 = 1/t39;
    jacmKRT[0][0] = t33*t40;
    t41 = a[3];
    t47 = -t4*t10+t16*t25-t30*t1+t20*t7+t[1];
    jacmKRT[1][0] = t41*t47*t40;
    jacmKRT[0][1] = 1.0;
    jacmKRT[1][1] = 0.0;
    jacmKRT[0][2] = 0.0;
    jacmKRT[1][2] = 1.0;
    jacmKRT[0][3] = 0.0;
    t49 = a[0];
    jacmKRT[1][3] = t49*t47*t40;
    jacmKRT[0][4] = t47*t40;
    jacmKRT[1][4] = 0.0;
    t51 = 1/t16;
    t52 = t51*t20;
    t54 = t51*t5;
    t56 = -t54*t1-t8;
    t58 = t51*t8;
    t60 = -t58*t1+t5;
    t64 = a[4];
    t66 = t51*t25;
    t70 = t51*t2;
    t71 = t1*t7;
    t73 = 2.0*t29-t66*t1+t16*t56-t60*t1-t27-t28-t70*t71;
    t75 = a[1];
    t77 = t51*t30;
    t80 = t4*t1;
    t83 = 2.0*t23-t77*t1+t16*t60+t70*t80+t56*t1+t22-t24;
    t91 = t39*t39;
    t92 = 1/t91;
    t93 = (t49*t33+t64*t47+t75*t39)*t92;
    jacmKRT[0][5] = (t49*(t3+t6+t9-t52*t1-t56*t7+t60*t4)+t64*t73+t75*t83)*t40-
t93*t83;
    t95 = t49*t41;
    t97 = a[2];
    t104 = (t95*t47+t97*t39)*t92;
    jacmKRT[1][5] = (t95*t73+t97*t83)*t40-t104*t83;
    t109 = -t70*t4+t8;
    t111 = t4*t7;
    t114 = -t58*t4-t2;
    t121 = t6+t3+t9-t66*t4-t114*t1+t109*t7;
    t128 = 2.0*t19-t77*t4+t114*t16-t109*t4-t17-t18-t54*t80;
    jacmKRT[0][6] = (t49*(2.0*t28-t52*t4+t16*t109+t54*t111+t114*t4+t27-t29)+t64
*t121+t75*t128)*t40-t93*t128;
    jacmKRT[1][6] = (t95*t121+t97*t128)*t40-t104*t128;
    t141 = -t70*t7-t5;
    t144 = -t54*t7+t2;
    t154 = 2.0*t18-t66*t7+t144*t16+t58*t71+t141*t7+t17-t19;
    t159 = t9+t3+t6-t77*t7-t141*t4+t144*t1;
    jacmKRT[0][7] = (t49*(2.0*t24-t52*t7+t16*t141-t144*t7-t22-t23-t58*t111)+t64
*t154+t75*t159)*t40-t93*t159;
    jacmKRT[1][7] = (t95*t154+t97*t159)*t40-t104*t159;
    jacmKRT[0][8] = t49*t40;
    jacmKRT[1][8] = 0.0;
    jacmKRT[0][9] = t64*t40;
    jacmKRT[1][9] = t95*t40;
    jacmKRT[0][10] = t75*t40-t93;
    jacmKRT[1][10] = t97*t40-t104;
    return;
  }
}

#include <math.h>
void calcImgProjJacS(double a[5],double v[3],double t[3],double M[3],
double jacmS[2][3])
{
  double t1;
  double t10;
  double t109;
  double t11;
  double t111;