39 const CPose3D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2)
46 CPose3D::ln_rot_jacob(
R, dLnRot_dRot);
50 matrix_VxV_t& J1 = *df_de1;
61 J1(0, 4) = P1DP2inv.z();
62 J1(0, 5) = -P1DP2inv.
y();
63 J1(1, 3) = -P1DP2inv.z();
64 J1(1, 5) = P1DP2inv.
x();
65 J1(2, 3) = P1DP2inv.
y();
66 J1(2, 4) = -P1DP2inv.
x();
69 0,
R(2, 0), -
R(1, 0), -
R(2, 0), 0,
R(0, 0),
R(1, 0), -
R(0, 0), 0,
71 0,
R(2, 1), -
R(1, 1), -
R(2, 1), 0,
R(0, 1),
R(1, 1), -
R(0, 1), 0,
73 0,
R(2, 2), -
R(1, 2), -
R(2, 2), 0,
R(0, 2),
R(1, 2), -
R(0, 2), 0};
77 J1.block(3, 3, 3, 3) = (dLnRot_dRot * aux).eval();
86 matrix_VxV_t& J2 = *df_de2;
89 for (
int i = 0; i < 3; i++)
90 for (
int j = 0; j < 3; j++)
91 J2.set_unsafe(i, j, -
R.get_unsafe(i, j));
94 0,
R(0, 2), -
R(0, 1), 0,
R(1, 2), -
R(1, 1), 0,
R(2, 2), -
R(2, 1),
96 -
R(0, 2), 0,
R(0, 0), -
R(1, 2), 0,
R(1, 0), -
R(2, 2), 0,
R(2, 0),
98 R(0, 1), -
R(0, 0), 0,
R(1, 1), -
R(1, 0), 0,
R(2, 1), -
R(2, 0), 0};
102 J2.block(3, 3, 3, 3) = (dLnRot_dRot * aux).eval();
110 const CPose2D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2)
114 matrix_VxV_t& J1 = *df_de1;
120 J1.unit(VECTOR_SIZE, 1.0);
121 J1(0, 2) = -P1DP2inv.
y();
122 J1(1, 2) = P1DP2inv.
x();
131 matrix_VxV_t& J2 = *df_de2;
133 const double ccos = cos(P1DP2inv.
phi());
134 const double csin = sin(P1DP2inv.
phi());
136 const double vals[] = {-ccos, csin, 0, -csin, -ccos, 0, 0, 0, -1};