CPosePDF.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_VIRTUAL_SERIALIZABLE(CPosePDF, CSerializable, mrpt::poses)

void CPosePDF::jacobiansPoseComposition(
    const CPosePDFGaussian& x, const CPosePDFGaussian& u,
    CMatrixDouble33& df_dx, CMatrixDouble33& df_du)
{
    CPosePDF::jacobiansPoseComposition(x.mean, u.mean, df_dx, df_du);
}

/*---------------------------------------------------------------
                    jacobiansPoseComposition
 ---------------------------------------------------------------*/
void CPosePDF::jacobiansPoseComposition(
    const CPose2D& x, const CPose2D& u, CMatrixDouble33& df_dx,
    CMatrixDouble33& df_du, const bool compute_df_dx, const bool compute_df_du)

{
    const double spx = sin(x.phi());
    const double cpx = cos(x.phi());
    if (compute_df_dx)
    {
        /*
            df_dx =
            [ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ]
            [ 0, 1,  cos(phi_x)*x_u-sin(phi_x)*y_u ]
            [ 0, 0,                              1 ]
        */
        df_dx.unit(3, 1.0);

        const double xu = u.x();
        const double yu = u.y();

        df_dx.get_unsafe(0, 2) = -spx * xu - cpx * yu;
        df_dx.get_unsafe(1, 2) = cpx * xu - spx * yu;
    }

    if (compute_df_du)
    {
        /*
            df_du =
            [ cos(phi_x) , -sin(phi_x) ,  0  ]
            [ sin(phi_x) ,  cos(phi_x) ,  0  ]
            [         0  ,          0  ,  1  ]
        */
        // This is the homogeneous matrix of "x":
        df_du.get_unsafe(0, 2) = df_du.get_unsafe(1, 2) =
            df_du.get_unsafe(2, 0) = df_du.get_unsafe(2, 1) = 0;
        df_du.get_unsafe(2, 2) = 1;

        df_du.get_unsafe(0, 0) = cpx;
        df_du.get_unsafe(0, 1) = -spx;
        df_du.get_unsafe(1, 0) = spx;
        df_du.get_unsafe(1, 1) = cpx;
    }
}