/*---------------------------------------------------------------
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();
}
}
