CPose3DPDF.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/CPose3D.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DQuatPDF.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussianInf.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/serialization/CArchive.h>

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

IMPLEMENTS_VIRTUAL_SERIALIZABLE(CPose3DPDF, CSerializable, mrpt::poses)

/*---------------------------------------------------------------
                    copyFrom2D
  ---------------------------------------------------------------*/
CPose3DPDF* CPose3DPDF::createFrom2D(const CPosePDF& o)
{
    MRPT_START

    if (o.GetRuntimeClass() == CLASS_ID(CPosePDFGaussian))
    {
        CPose3DPDFGaussian* newObj = new CPose3DPDFGaussian();
        const CPosePDFGaussian* obj = static_cast<const CPosePDFGaussian*>(&o);

        newObj->mean = CPose3D(obj->mean);
        CMatrixDouble COV = CMatrixDouble(obj->cov);
        COV.setSize(6, 6);

        // Move covariances: phi<->z
        COV(3, 3) = COV(2, 2);
        COV(2, 2) = 0;
        COV(0, 3) = COV(3, 0) = COV(0, 2);
        COV(0, 2) = COV(2, 0) = 0;
        COV(1, 3) = COV(3, 1) = COV(1, 2);
        COV(1, 2) = COV(2, 1) = 0;

        newObj->cov = CMatrixDouble66(COV);

        return newObj;
    }
    else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFGaussianInf))
    {
        CPose3DPDFGaussianInf* newObj = new CPose3DPDFGaussianInf();
        const CPosePDFGaussianInf* obj =
            static_cast<const CPosePDFGaussianInf*>(&o);

        newObj->mean = CPose3D(obj->mean);
        CMatrixDouble COVINV = CMatrixDouble(obj->cov_inv);

        COVINV.setSize(6, 6);
        // Move covariances: phi<->z
        COVINV(3, 3) = COVINV(2, 2);
        COVINV(2, 2) = 0;
        COVINV(0, 3) = COVINV(3, 0) = COVINV(0, 2);
        COVINV(0, 2) = COVINV(2, 0) = 0;
        COVINV(1, 3) = COVINV(3, 1) = COVINV(1, 2);
        COVINV(1, 2) = COVINV(2, 1) = 0;

        newObj->cov_inv = CMatrixDouble66(COVINV);

        return newObj;
    }
    else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFParticles))
    {
        const CPosePDFParticles* obj =
            static_cast<const CPosePDFParticles*>(&o);
        CPose3DPDFParticles* newObj = new CPose3DPDFParticles(obj->size());

        CPosePDFParticles::CParticleList::const_iterator it1;
        CPose3DPDFParticles::CParticleList::iterator it2;
        for (it1 = obj->m_particles.begin(), it2 = newObj->m_particles.begin();
             it1 != obj->m_particles.end(); ++it1, ++it2)
        {
            it2->log_w = it1->log_w;
            it2->d = TPose3D(it1->d);
        }

        return newObj;
    }
    else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFSOG))
    {
        const CPosePDFSOG* obj = static_cast<const CPosePDFSOG*>(&o);
        CPose3DPDFSOG* newObj = new CPose3DPDFSOG(obj->size());

        CPosePDFSOG::const_iterator it1;
        CPose3DPDFSOG::iterator it2;

        for (it1 = obj->begin(), it2 = newObj->begin(); it1 != obj->end();
             ++it1, ++it2)
        {
            it2->log_w = it1->log_w;
            it2->val.mean.setFromValues(
                it1->mean.x(), it1->mean.y(), 0, it1->mean.phi(), 0, 0);

            it2->val.cov.zeros();

            it2->val.cov.get_unsafe(0, 0) = it1->cov.get_unsafe(0, 0);
            it2->val.cov.get_unsafe(1, 1) = it1->cov.get_unsafe(1, 1);
            it2->val.cov.get_unsafe(3, 3) =
                it1->cov.get_unsafe(2, 2);  // yaw <- phi

            it2->val.cov.get_unsafe(0, 1) = it2->val.cov.get_unsafe(1, 0) =
                it1->cov.get_unsafe(0, 1);

            it2->val.cov.get_unsafe(0, 3) = it2->val.cov.get_unsafe(3, 0) =
                it1->cov.get_unsafe(0, 2);

            it2->val.cov.get_unsafe(1, 3) = it2->val.cov.get_unsafe(3, 1) =
                it1->cov.get_unsafe(1, 2);
        }

        return newObj;
    }
    else
        THROW_EXCEPTION("Class of object not supported by this method!");

    MRPT_END
}

/*---------------------------------------------------------------
                    jacobiansPoseComposition
 ---------------------------------------------------------------*/
void CPose3DPDF::jacobiansPoseComposition(
    const CPose3D& x, const CPose3D& u, CMatrixDouble66& df_dx,
    CMatrixDouble66& df_du)
{
    // See this techical report:
    // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty

    // Direct equations (for the covariances) in yaw-pitch-roll are too complex.
    //  Make a way around them and consider instead this path:
    //
    //      X(6D)       U(6D)
    //        |           |
    //        v           v
    //      X(7D)       U(7D)
    //        |           |
    //        +--- (+) ---+
    //              |
    //              v
    //            RES(7D)
    //              |
    //              v
    //            RES(6D)
    //

    // FUNCTION = f_quat2eul(  f_quat_comp(  f_eul2quat(x) , f_eul2quat(u)  ) )
    //  Jacobians chain rule:
    //
    //  JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
    //  JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)

    // J_E2Q_dx:
    CMatrixFixedNumeric<double, 7, 6> J_E2Q_dx;  // Init to zeros
    {
        CMatrixFixedNumeric<double, 4, 3> dq_dr_sub(UNINITIALIZED_MATRIX);
        CQuaternionDouble q_dumm(UNINITIALIZED_QUATERNION);
        x.getAsQuaternion(q_dumm, &dq_dr_sub);
        J_E2Q_dx.get_unsafe(0, 0) = J_E2Q_dx.get_unsafe(1, 1) =
            J_E2Q_dx.get_unsafe(2, 2) = 1;
        J_E2Q_dx.insertMatrix(3, 3, dq_dr_sub);
    }

    // J_E2Q_du:
    CMatrixFixedNumeric<double, 7, 6> J_E2Q_du;  // Init to zeros
    {
        CMatrixFixedNumeric<double, 4, 3> dq_dr_sub(UNINITIALIZED_MATRIX);
        CQuaternionDouble q_dumm(UNINITIALIZED_QUATERNION);
        u.getAsQuaternion(q_dumm, &dq_dr_sub);
        J_E2Q_du.get_unsafe(0, 0) = J_E2Q_du.get_unsafe(1, 1) =
            J_E2Q_du.get_unsafe(2, 2) = 1;
        J_E2Q_du.insertMatrix(3, 3, dq_dr_sub);
    }

    // quat_df_dx & quat_df_du
    CMatrixDouble77 quat_df_dx(UNINITIALIZED_MATRIX),
        quat_df_du(UNINITIALIZED_MATRIX);
    const CPose3DQuat quat_x(x);
    const CPose3DQuat quat_u(u);

    CPose3DQuatPDF::jacobiansPoseComposition(
        quat_x,  // x
        quat_u,  // u
        quat_df_dx, quat_df_du);

    // And now J_Q2E:
    //         [  I_3   |    0         ]
    // J_Q2E = [ -------+------------- ]
    //         [  0     | dr_dq_angles ]
    //
    CMatrixFixedNumeric<double, 6, 7> J_Q2E;  // Init to zeros
    J_Q2E.get_unsafe(0, 0) = J_Q2E.get_unsafe(1, 1) = J_Q2E.get_unsafe(2, 2) =
        1;
    {
        // The end result of the pose composition, as a quaternion:
        CQuaternionDouble q_xu(UNINITIALIZED_QUATERNION);
        q_xu.crossProduct(quat_x.quat(), quat_u.quat());

        // Compute the jacobian:
        CMatrixFixedNumeric<double, 3, 4> dr_dq_sub_aux(UNINITIALIZED_MATRIX);
        double yaw, pitch, roll;
        q_xu.rpy_and_jacobian(roll, pitch, yaw, &dr_dq_sub_aux, false);

        CMatrixDouble44 dnorm_dq(UNINITIALIZED_MATRIX);
        q_xu.normalizationJacobian(dnorm_dq);

        CMatrixFixedNumeric<double, 3, 4> dr_dq_sub(UNINITIALIZED_MATRIX);
        dr_dq_sub.multiply(dr_dq_sub_aux, dnorm_dq);

        J_Q2E.insertMatrix(3, 3, dr_dq_sub);
    }

    // And finally:
    //  JACOB_dx = J_Q2E (6x7) * quat_df_dx (7x7) * J_E2Q_dx (7x6)
    //  JACOB_du = J_Q2E (6x7) * quat_df_du (7x7) * J_E2Q_du (7x6)
    df_dx.multiply_ABC(J_Q2E, quat_df_dx, J_E2Q_dx);
    df_du.multiply_ABC(J_Q2E, quat_df_du, J_E2Q_du);
}