17 #include <Eigen/Dense>
48 pose.getAs12Vector(v);
61 tang2mat_jacob J = tang2mat_jacob::Zero();
62 J.block<3, 3>(9, 0) = Eigen::Matrix3d::Identity();
74 J(0, 9) = J(1, 10) = J(2, 11) = 1.0;
83 jacob.
block<9, 3>(0, 0).setZero();
84 jacob.
block<3, 3>(9, 0).setIdentity();
85 for (
int i = 0; i < 3; i++)
87 auto trg_blc = jacob.
block<3, 3>(3 * i, 3);
92 auto trg_blc = jacob.
block<3, 3>(9, 3);
105 jacob.
block<3, 3>(9, 0) = dRot.asEigen();
107 jacob.
block<3, 1>(3, 5) = -dRot.col(0);
108 jacob.
block<3, 1>(6, 4) = dRot.col(0);
110 jacob.
block<3, 1>(0, 5) = dRot.col(1);
111 jacob.
block<3, 1>(6, 3) = -dRot.col(1);
113 jacob.
block<3, 1>(0, 4) = -dRot.col(2);
114 jacob.
block<3, 1>(3, 3) = dRot.col(2);
123 const auto& Arot =
A.getRotationMatrix();
126 jacob.
block<9, 3>(0, 0).setZero();
127 jacob.
block<3, 3>(9, 0) =
A.getRotationMatrix().asEigen();
129 for (
int i = 0; i < 3; i++)
133 jacob.
block<3, 3>(3 * i, 3) = Arot.asEigen() * aux;
136 jacob.
block<3, 3>(9, 3) = Arot.asEigen() * aux;
149 const CPose3D DinvP1invP2 = Dinv + P1inv + P2;
160 matrix_TxT& J1 = df_de1.value().get();
166 J1 = dLnT_dT.asEigen() * J1a.
asEigen() * J1b.asEigen();
170 matrix_TxT& J2 = df_de2.value().get();
172 J2 = dLnT_dT * dAe_de;
181 matrix_MxM J = matrix_MxM::Zero();
185 for (
int c = 0; c < 4; c++)
186 for (
int r = 0; r < 4; r++)
187 for (
int q = 0; q < 3; q++) J(r * 3 + q, c * 3 + q) = B_HM(r, c);
195 matrix_MxM J = matrix_MxM::Zero();
197 const auto& AR =
A.getRotationMatrix();
198 for (
int c = 0; c < 4; c++) J.block<3, 3>(c * 3, c * 3) = AR.asEigen();
239 const auto bx = B.x(), by = B.y();
240 const auto cphia =
A.phi_cos(), sphia =
A.phi_sin();
242 matrix_MxM J = matrix_MxM::Identity();
243 J(0, 2) = -bx * sphia - by * cphia;
244 J(1, 2) = +bx * cphia - by * sphia;
251 matrix_MxM J = matrix_MxM::Identity();
252 const auto cphia =
A.phi_cos(), sphia =
A.phi_sin();
262 const auto c = D.phi_cos(), s = D.phi_sin();
279 const CPose2D P1invP2 = P1inv + P2;
280 const CPose2D DinvP1invP2 = Dinv + P1invP2;
284 auto& J1 = df_de1.value().get();
285 J1 = jacob_dAB_dA(Dinv, P1invP2).asEigen() * (-jacob_dDexpe_de(Dinv));
290 auto& J2 = df_de2.value().get();