/*--------------------------------------------------------------- changeCoordinatesReference ---------------------------------------------------------------*/ void CPointPDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase ) { const CMatrixDouble33 &M = newReferenceBase.getRotationMatrix(); // The mean: mean = newReferenceBase + mean; // The covariance: M.multiply_HCHt(CMatrixDouble33(cov), cov); // save in cov }
/*--------------------------------------------------------------- changeCoordinatesReference ---------------------------------------------------------------*/ void CPoint2DPDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase ) { // Clip the 3x3 rotation matrix const CMatrixDouble22 M = newReferenceBase.getRotationMatrix().block(0,0,2,2); // The mean: mean = CPoint2D( newReferenceBase + mean ); // The covariance: cov = M*cov*M.transpose(); }
/** Return one or both of the following 6x6 Jacobians, useful in graph-slam * problems... */ void SE_traits<3>::jacobian_dP1DP2inv_depsilon( const CPose3D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2) { const CMatrixDouble33& R = P1DP2inv.getRotationMatrix(); // The rotation matrix. // Common part: d_Ln(R)_dR: CMatrixFixedNumeric<double, 3, 9> dLnRot_dRot(UNINITIALIZED_MATRIX); CPose3D::ln_rot_jacob(R, dLnRot_dRot); if (df_de1) { matrix_VxV_t& J1 = *df_de1; // This Jacobian has the structure: // [ I_3 | -[d_t]_x ] // Jacob1 = [ ---------+------------------- ] // [ 0_3x3 | dLnR_dR * (...) ] // J1.zeros(); J1(0, 0) = 1; J1(1, 1) = 1; J1(2, 2) = 1; J1(0, 4) = P1DP2inv.z(); J1(0, 5) = -P1DP2inv.y(); J1(1, 3) = -P1DP2inv.z(); J1(1, 5) = P1DP2inv.x(); J1(2, 3) = P1DP2inv.y(); J1(2, 4) = -P1DP2inv.x(); alignas(MRPT_MAX_ALIGN_BYTES) const double aux_vals[] = { 0, R(2, 0), -R(1, 0), -R(2, 0), 0, R(0, 0), R(1, 0), -R(0, 0), 0, // ----------------------- 0, R(2, 1), -R(1, 1), -R(2, 1), 0, R(0, 1), R(1, 1), -R(0, 1), 0, // ----------------------- 0, R(2, 2), -R(1, 2), -R(2, 2), 0, R(0, 2), R(1, 2), -R(0, 2), 0}; const CMatrixFixedNumeric<double, 9, 3> aux(aux_vals); // right-bottom part = dLnRot_dRot * aux J1.block(3, 3, 3, 3) = (dLnRot_dRot * aux).eval(); } if (df_de2) { // This Jacobian has the structure: // [ -R | 0_3x3 ] // Jacob2 = [ ---------+------------------- ] // [ 0_3x3 | dLnR_dR * (...) ] // matrix_VxV_t& J2 = *df_de2; J2.zeros(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) J2.set_unsafe(i, j, -R.get_unsafe(i, j)); alignas(MRPT_MAX_ALIGN_BYTES) const double aux_vals[] = { 0, R(0, 2), -R(0, 1), 0, R(1, 2), -R(1, 1), 0, R(2, 2), -R(2, 1), // ----------------------- -R(0, 2), 0, R(0, 0), -R(1, 2), 0, R(1, 0), -R(2, 2), 0, R(2, 0), // ----------------------- R(0, 1), -R(0, 0), 0, R(1, 1), -R(1, 0), 0, R(2, 1), -R(2, 0), 0}; const CMatrixFixedNumeric<double, 9, 3> aux(aux_vals); // right-bottom part = dLnRot_dRot * aux J2.block(3, 3, 3, 3) = (dLnRot_dRot * aux).eval(); } }