CActionRobotMovement2D.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 "obs-precomp.h"  // Precompiled headers

#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/random.h>
#include <mrpt/math/point_poses2vectors.h>
#include <mrpt/math/wrap2pi.h>

using namespace mrpt::obs;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CActionRobotMovement2D, CAction, mrpt::obs)

/*---------------------------------------------------------------
                                                Constructor
  ---------------------------------------------------------------*/
CActionRobotMovement2D::CActionRobotMovement2D()
        : poseChange(mrpt::make_aligned_shared<CPosePDFGaussian>()),
          rawOdometryIncrementReading(),
          estimationMethod(emOdometry),
          hasEncodersInfo(false),
          encoderLeftTicks(0),
          encoderRightTicks(0),
          hasVelocities(false),
          velocityLocal(.0, .0, .0),
          motionModelConfiguration(),
          m_fastDrawGauss_Z(),
          m_fastDrawGauss_M()
{
}

/*---------------------------------------------------------------
  Implements the writing to a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CActionRobotMovement2D::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 7;
        else
        {
                uint32_t i = static_cast<uint32_t>(estimationMethod);

                out << i;

                // Added in version 2:
                // If the estimation method is emOdometry, save the rawOdo + config data
                // instead of
                //  the PDF itself:
                if (estimationMethod == emOdometry)
                {
                        // The odometry data:
                        out << rawOdometryIncrementReading;

                        i = static_cast<uint32_t>(motionModelConfiguration.modelSelection);
                        out << i;
                        out << motionModelConfiguration.gaussianModel.a1
                                << motionModelConfiguration.gaussianModel.a2
                                << motionModelConfiguration.gaussianModel.a3
                                << motionModelConfiguration.gaussianModel.a4
                                << motionModelConfiguration.gaussianModel.minStdXY
                                << motionModelConfiguration.gaussianModel.minStdPHI;

                        out << motionModelConfiguration.thrunModel.nParticlesCount
                                << motionModelConfiguration.thrunModel.alfa1_rot_rot
                                << motionModelConfiguration.thrunModel.alfa2_rot_trans
                                << motionModelConfiguration.thrunModel.alfa3_trans_trans
                                << motionModelConfiguration.thrunModel.alfa4_trans_rot
                                << motionModelConfiguration.thrunModel.additional_std_XY
                                << motionModelConfiguration.thrunModel.additional_std_phi;
                }
                else
                {
                        // The PDF:
                        out << (*poseChange);
                }

                // Added in version 1:
                out << hasVelocities;
                if (hasVelocities) out << velocityLocal;  // v7

                out << hasEncodersInfo;
                if (hasEncodersInfo)
                        out << encoderLeftTicks << encoderRightTicks;  // added if() in v7

                // Added in version 6
                out << timestamp;
        }
}

/*---------------------------------------------------------------
  Implements the reading from a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CActionRobotMovement2D::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 4:
                case 5:
                case 6:
                case 7:
                {
                        int32_t i;

                        // Read the estimation method first:
                        in >> i;
                        estimationMethod = static_cast<TEstimationMethod>(i);

                        // If the estimation method is emOdometry, save the rawOdo + config
                        // data instead of
                        //  the PDF itself:
                        if (estimationMethod == emOdometry)
                        {
                                // The odometry data:
                                in >> rawOdometryIncrementReading;

                                in >> i;
                                motionModelConfiguration.modelSelection =
                                        static_cast<TDrawSampleMotionModel>(i);

                                in >> motionModelConfiguration.gaussianModel.a1 >>
                                        motionModelConfiguration.gaussianModel.a2 >>
                                        motionModelConfiguration.gaussianModel.a3 >>
                                        motionModelConfiguration.gaussianModel.a4 >>
                                        motionModelConfiguration.gaussianModel.minStdXY >>
                                        motionModelConfiguration.gaussianModel.minStdPHI;

                                in >> i;
                                motionModelConfiguration.thrunModel.nParticlesCount = i;
                                in >> motionModelConfiguration.thrunModel.alfa1_rot_rot >>
                                        motionModelConfiguration.thrunModel.alfa2_rot_trans >>
                                        motionModelConfiguration.thrunModel.alfa3_trans_trans >>
                                        motionModelConfiguration.thrunModel.alfa4_trans_rot;

                                if (version >= 5)
                                {
                                        in >>
                                                motionModelConfiguration.thrunModel.additional_std_XY >>
                                                motionModelConfiguration.thrunModel.additional_std_phi;
                                }
                                else
                                {
                                        motionModelConfiguration.thrunModel.additional_std_XY =
                                                motionModelConfiguration.thrunModel.additional_std_phi =
                                                        0;
                                }

                                // Re-build the PDF:
                                computeFromOdometry(
                                        rawOdometryIncrementReading, motionModelConfiguration);
                        }
                        else
                        {
                                // Read the PDF directly from the stream:
                                CPosePDF::Ptr pc;
                                in >> pc;
                                poseChange = pc;
                        }

                        in >> hasVelocities;
                        if (version >= 7)
                        {
                                if (hasVelocities) in >> velocityLocal;
                        }
                        else
                        {
                                float velocityLin, velocityAng;
                                in >> velocityLin >> velocityAng;
                                velocityLocal.vx = velocityLin;
                                velocityLocal.vy = .0f;
                                velocityLocal.omega = velocityAng;
                        }
                        in >> hasEncodersInfo;
                        if (version < 7 || hasEncodersInfo)
                        {
                                in >> i;
                                encoderLeftTicks = i;
                                in >> i;
                                encoderRightTicks = i;
                        }
                        else
                        {
                                encoderLeftTicks = 0;
                                encoderRightTicks = 0;
                        }

                        if (version >= 6)
                                in >> timestamp;
                        else
                                timestamp = INVALID_TIMESTAMP;
                }
                break;

                case 3:
                {
                        int32_t i;

                        // Read the estimation method first:
                        in >> i;
                        estimationMethod = static_cast<TEstimationMethod>(i);

                        // If the estimation method is emOdometry, save the rawOdo + config
                        // data instead of
                        //  the PDF itself:
                        if (estimationMethod == emOdometry)
                        {
                                // The odometry data:
                                in >> rawOdometryIncrementReading;

                                in >> i;
                                motionModelConfiguration.modelSelection =
                                        static_cast<TDrawSampleMotionModel>(i);

                                float dum1, dum2, dum3;

                                in >> dum1 >> dum2 >> dum3 >>
                                        motionModelConfiguration.gaussianModel.minStdXY >>
                                        motionModelConfiguration.gaussianModel.minStdPHI;

                                // Leave the default values for a1,a2,a3,a4:
                                in >> i;
                                motionModelConfiguration.thrunModel.nParticlesCount = i;
                                in >> motionModelConfiguration.thrunModel.alfa1_rot_rot;
                                in >> motionModelConfiguration.thrunModel.alfa2_rot_trans >>
                                        motionModelConfiguration.thrunModel.alfa3_trans_trans >>
                                        motionModelConfiguration.thrunModel.alfa4_trans_rot;

                                // Re-build the PDF:
                                computeFromOdometry(
                                        rawOdometryIncrementReading, motionModelConfiguration);
                        }
                        else
                        {
                                // Read the PDF directly from the stream:
                                CPosePDF::Ptr pc;
                                in >> pc;
                                poseChange = pc;
                        }

                        in >> hasVelocities;
                        {
                                float velocityLin, velocityAng;
                                in >> velocityLin >> velocityAng;
                                velocityLocal.vx = velocityLin;
                                velocityLocal.vy = .0f;
                                velocityLocal.omega = velocityAng;
                        }
                        in >> hasEncodersInfo;

                        in >> i;
                        encoderLeftTicks = i;
                        in >> i;
                        encoderRightTicks = i;
                }
                break;

                case 2:
                {
                        int32_t i;

                        // Read the estimation method first:
                        in >> i;
                        estimationMethod = static_cast<TEstimationMethod>(i);

                        // If the estimation method is emOdometry, save the rawOdo + config
                        // data instead of
                        //  the PDF itself:
                        if (estimationMethod == emOdometry)
                        {
                                // The odometry data:
                                in >> rawOdometryIncrementReading;

                                //                              TMotionModelOptions_OLD_VERSION_2       dummy;
                                //                              in.ReadBuffer( &dummy,
                                // sizeof(TMotionModelOptions_OLD_VERSION_2) );
                                uint8_t dummy[44];
                                in.ReadBuffer(&dummy, sizeof(dummy));

                                motionModelConfiguration = TMotionModelOptions();

                                // Re-build the PDF:
                                computeFromOdometry(
                                        rawOdometryIncrementReading, motionModelConfiguration);
                        }
                        else
                        {
                                // Read the PDF directly from the stream:
                                CPosePDF::Ptr pc;
                                in >> pc;
                                poseChange = pc;
                        }

                        in >> hasVelocities;
                        {
                                float velocityLin, velocityAng;
                                in >> velocityLin >> velocityAng;
                                velocityLocal.vx = velocityLin;
                                velocityLocal.vy = .0f;
                                velocityLocal.omega = velocityAng;
                        }
                        in >> hasEncodersInfo;
                        in >> i;
                        encoderLeftTicks = i;
                        in >> i;
                        encoderRightTicks = i;
                }
                break;
                case 0:
                case 1:
                {
                        int32_t i;
                        {
                                CPosePDF::Ptr pc;
                                in >> pc;
                                poseChange = pc;
                        }
                        in >> i;

                        // copy:
                        estimationMethod = static_cast<TEstimationMethod>(i);

                        // Simulate the "rawOdometryIncrementReading" as the mean value:
                        if (estimationMethod == emOdometry)
                                poseChange->getMean(rawOdometryIncrementReading);
                        else
                                rawOdometryIncrementReading = CPose2D(0, 0, 0);

                        if (version >= 1)
                        {
                                in >> hasVelocities;
                                {
                                        float velocityLin, velocityAng;
                                        in >> velocityLin >> velocityAng;
                                        velocityLocal.vx = velocityLin;
                                        velocityLocal.vy = .0f;
                                        velocityLocal.omega = velocityAng;
                                }
                                in >> hasEncodersInfo;
                                in >> i;
                                encoderLeftTicks = i;
                                in >> i;
                                encoderRightTicks = i;
                        }
                        else
                        {
                                // Default values:
                                hasVelocities = hasEncodersInfo = false;
                                encoderLeftTicks = encoderRightTicks = 0;
                                velocityLocal = mrpt::math::TTwist2D(.0, .0, .0);
                        }
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

/*---------------------------------------------------------------
                                        computeFromEncoders
 ---------------------------------------------------------------*/
void CActionRobotMovement2D::computeFromEncoders(
        double K_left, double K_right, double D)
{
        if (hasEncodersInfo)
        {
                const double As =
                        0.5 * (K_right * encoderRightTicks + K_left * encoderLeftTicks);
                const double Aphi =
                        (K_right * encoderRightTicks - K_left * encoderLeftTicks) / D;

                double x, y;
                if (Aphi != 0)
                {
                        const double R = As / Aphi;
                        x = R * sin(Aphi);
                        y = R * (1 - cos(Aphi));
                }
                else
                {
                        x = As;
                        y = 0;
                }

                // Build the whole PDF with the current parameters:
                computeFromOdometry(CPose2D(x, y, Aphi), motionModelConfiguration);
        }
}

/*---------------------------------------------------------------
                                        computeFromOdometry
 ---------------------------------------------------------------*/
void CActionRobotMovement2D::computeFromOdometry(
        const CPose2D& odometryIncrement, const TMotionModelOptions& options)
{
        // Set the members data:
        estimationMethod = emOdometry;
        rawOdometryIncrementReading = odometryIncrement;
        motionModelConfiguration = options;

        if (options.modelSelection == mmGaussian)
                computeFromOdometry_modelGaussian(odometryIncrement, options);
        else
                computeFromOdometry_modelThrun(odometryIncrement, options);
}

/*---------------------------------------------------------------
                                                TMotionModelOptions
 ---------------------------------------------------------------*/
CActionRobotMovement2D::TMotionModelOptions::TMotionModelOptions()
        : modelSelection(CActionRobotMovement2D::mmGaussian),
          gaussianModel(),
          thrunModel()
{
        gaussianModel.a1 = 0.01f;
        gaussianModel.a2 = RAD2DEG(0.001f);
        gaussianModel.a3 = DEG2RAD(1.0f);
        gaussianModel.a4 = 0.05f;

        gaussianModel.minStdXY = 0.01f;
        gaussianModel.minStdPHI = DEG2RAD(0.2f);

        thrunModel.nParticlesCount = 300;
        thrunModel.alfa1_rot_rot = 0.05f;
        thrunModel.alfa2_rot_trans = DEG2RAD(4.0f);
        thrunModel.alfa3_trans_trans = 0.01f;
        thrunModel.alfa4_trans_rot = RAD2DEG(0.0001f);
        thrunModel.additional_std_XY = 0.001f;
        thrunModel.additional_std_phi = DEG2RAD(0.05f);
}

/*---------------------------------------------------------------
                                computeFromOdometry_modelGaussian
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::computeFromOdometry_modelGaussian(
        const CPose2D& odometryIncrement, const TMotionModelOptions& o)
{
        // The Gaussian PDF:
        // ---------------------------
        poseChange = mrpt::make_aligned_shared<CPosePDFGaussian>();
        CPosePDFGaussian* aux;
        aux = static_cast<CPosePDFGaussian*>(poseChange.get());

        // See http://www.mrpt.org/Probabilistic_Motion_Models
        // -----------------------------------

        // Build the odometry noise matrix:
        double Al = odometryIncrement.norm();
        CMatrixDouble31 ODO_INCR = CMatrixDouble31(odometryIncrement);
        CMatrixDouble33 C_ODO;
        C_ODO(0, 0) = square(
                o.gaussianModel.minStdXY + o.gaussianModel.a1 * Al +
                o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
        C_ODO(1, 1) = square(
                o.gaussianModel.minStdXY + o.gaussianModel.a1 * Al +
                o.gaussianModel.a2 * fabs(odometryIncrement.phi()));
        C_ODO(2, 2) = square(
                o.gaussianModel.minStdPHI + o.gaussianModel.a3 * Al +
                o.gaussianModel.a4 * fabs(odometryIncrement.phi()));

        // Build the transformation matrix:
        CMatrixDouble33 H;

        double cos_Aphi_2 = cos(odometryIncrement.phi() / 2);
        double sin_Aphi_2 = sin(odometryIncrement.phi() / 2);

        H(0, 0) = H(1, 1) = cos_Aphi_2;
        H(0, 1) = -(H(1, 0) = sin_Aphi_2);
        H(2, 2) = 1;

        // Build the Jacobian matrix:
        CMatrixDouble33 J = H;
        J(0, 2) = -sin_Aphi_2 * ODO_INCR(0, 0) - cos_Aphi_2 * ODO_INCR(1, 0);
        J(1, 2) = cos_Aphi_2 * ODO_INCR(0, 0) - sin_Aphi_2 * ODO_INCR(1, 0);

        // The mean:
        aux->mean = odometryIncrement;

        // The covariance:
        J.multiply_HCHt(C_ODO, aux->cov);
}

/*---------------------------------------------------------------
                                computeFromOdometry_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::computeFromOdometry_modelThrun(
        const CPose2D& odometryIncrement, const TMotionModelOptions& o)
{
        // The Gaussian PDF:
        // ---------------------------
        CPosePDFParticles* aux;
        static CPose2D nullPose(0, 0, 0);

        poseChange = mrpt::make_aligned_shared<CPosePDFParticles>();
        aux = static_cast<CPosePDFParticles*>(poseChange.get());

        // Set the number of particles:
        aux->resetDeterministic(nullPose, o.thrunModel.nParticlesCount);

        // ---------------------------------------------------------------------------------------------
        // The following is an implementation from Thrun et al.'s book
        // (Probabilistic Robotics),
        //  page 136. Here "odometryIncrement" actually represents the incremental
        //  odometry difference.
        // ---------------------------------------------------------------------------------------------

        // The increments in odometry:
        float Arot1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
                                          ? atan2(odometryIncrement.y(), odometryIncrement.x())
                                          : 0;
        float Atrans = odometryIncrement.norm();
        float Arot2 = math::wrapToPi(odometryIncrement.phi() - Arot1);

        // Draw samples:
        for (size_t i = 0; i < o.thrunModel.nParticlesCount; i++)
        {
                float Arot1_draw = Arot1 -
                                                   (o.thrunModel.alfa1_rot_rot * fabs(Arot1) +
                                                        o.thrunModel.alfa2_rot_trans * Atrans) *
                                                           getRandomGenerator().drawGaussian1D_normalized();
                float Atrans_draw =
                        Atrans -
                        (o.thrunModel.alfa3_trans_trans * Atrans +
                         o.thrunModel.alfa4_trans_rot * (fabs(Arot1) + fabs(Arot2))) *
                                getRandomGenerator().drawGaussian1D_normalized();
                float Arot2_draw = Arot2 -
                                                   (o.thrunModel.alfa1_rot_rot * fabs(Arot2) +
                                                        o.thrunModel.alfa2_rot_trans * Atrans) *
                                                           getRandomGenerator().drawGaussian1D_normalized();

                // Output:
                aux->m_particles[i].d->x(
                        Atrans_draw * cos(Arot1_draw) +
                        motionModelConfiguration.thrunModel.additional_std_XY *
                                getRandomGenerator().drawGaussian1D_normalized());
                aux->m_particles[i].d->y(
                        Atrans_draw * sin(Arot1_draw) +
                        motionModelConfiguration.thrunModel.additional_std_XY *
                                getRandomGenerator().drawGaussian1D_normalized());
                aux->m_particles[i].d->phi(
                        Arot1_draw + Arot2_draw +
                        motionModelConfiguration.thrunModel.additional_std_phi *
                                getRandomGenerator().drawGaussian1D_normalized());
                aux->m_particles[i].d->normalizePhi();
        }
}

/*---------------------------------------------------------------
                                drawSingleSample
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::drawSingleSample(CPose2D& outSample) const
{
        // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
        //  the parameters to draw new samples:
        if (estimationMethod == emOdometry)
        {
                if (motionModelConfiguration.modelSelection == mmGaussian)
                        drawSingleSample_modelGaussian(outSample);
                else
                        drawSingleSample_modelThrun(outSample);
        }
        else
        {
                // If is not odometry, just employ the stored distribution:
                poseChange->drawSingleSample(outSample);
        }
}

/*---------------------------------------------------------------
                                drawSingleSample_modelGaussian
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::drawSingleSample_modelGaussian(
        CPose2D& outSample) const
{
        // In the Gaussian case it is more efficient just to
        // draw a sample from the already computed PDF:
        poseChange->drawSingleSample(outSample);
}

/*---------------------------------------------------------------
                                drawSingleSample_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::drawSingleSample_modelThrun(
        CPose2D& outSample) const
{
        // ---------------------------------------------------------------------------------------------
        // The following is an implementation from Thrun et al.'s book
        // (Probabilistic Robotics),
        //  page 136. Here "odometryIncrement" actually represents the incremental
        //  odometry difference.
        // ---------------------------------------------------------------------------------------------

        // The increments in odometry:
        float Arot1 = (rawOdometryIncrementReading.y() != 0 ||
                                   rawOdometryIncrementReading.x() != 0)
                                          ? atan2(
                                                        rawOdometryIncrementReading.y(),
                                                        rawOdometryIncrementReading.x())
                                          : 0;
        float Atrans = rawOdometryIncrementReading.norm();
        float Arot2 = math::wrapToPi(rawOdometryIncrementReading.phi() - Arot1);

        float Arot1_draw =
                Arot1 -
                (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot1) +
                 motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
                        getRandomGenerator().drawGaussian1D_normalized();
        float Atrans_draw =
                Atrans -
                (motionModelConfiguration.thrunModel.alfa3_trans_trans * Atrans +
                 motionModelConfiguration.thrunModel.alfa4_trans_rot *
                         (fabs(Arot1) + fabs(Arot2))) *
                        getRandomGenerator().drawGaussian1D_normalized();
        float Arot2_draw =
                Arot2 -
                (motionModelConfiguration.thrunModel.alfa1_rot_rot * fabs(Arot2) +
                 motionModelConfiguration.thrunModel.alfa2_rot_trans * Atrans) *
                        getRandomGenerator().drawGaussian1D_normalized();

        // Output:
        outSample.x(
                Atrans_draw * cos(Arot1_draw) +
                motionModelConfiguration.thrunModel.additional_std_XY *
                        getRandomGenerator().drawGaussian1D_normalized());
        outSample.y(
                Atrans_draw * sin(Arot1_draw) +
                motionModelConfiguration.thrunModel.additional_std_XY *
                        getRandomGenerator().drawGaussian1D_normalized());
        outSample.phi(
                Arot1_draw + Arot2_draw +
                motionModelConfiguration.thrunModel.additional_std_phi *
                        getRandomGenerator().drawGaussian1D_normalized());
        outSample.normalizePhi();
}

/*---------------------------------------------------------------
                                prepareFastDrawSingleSamples
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::prepareFastDrawSingleSamples() const
{
        // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
        //  the parameters to draw new samples:
        if (estimationMethod == emOdometry)
        {
                if (motionModelConfiguration.modelSelection == mmGaussian)
                        prepareFastDrawSingleSample_modelGaussian();
                else
                        prepareFastDrawSingleSample_modelThrun();
        }
}

/*---------------------------------------------------------------
                                fastDrawSingleSample
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::fastDrawSingleSample(CPose2D& outSample) const
{
        // Only in the case of "emOdometry" we have the rawOdometryMeasurement and
        //  the parameters to draw new samples:
        if (estimationMethod == emOdometry)
        {
                if (motionModelConfiguration.modelSelection == mmGaussian)
                        fastDrawSingleSample_modelGaussian(outSample);
                else
                        fastDrawSingleSample_modelThrun(outSample);
        }
        else
        {
                // If is not odometry, just employ the stored distribution:
                poseChange->drawSingleSample(outSample);
        }
}

/*---------------------------------------------------------------
                                prepareFastDrawSingleSample_modelGaussian
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::prepareFastDrawSingleSample_modelGaussian() const
{
        MRPT_START

        ASSERT_(IS_CLASS(poseChange, CPosePDFGaussian));

        CMatrixDouble33 D;

        const CPosePDFGaussian* gPdf =
                static_cast<const CPosePDFGaussian*>(poseChange.get());
        const CMatrixDouble33& cov = gPdf->cov;

        m_fastDrawGauss_M = gPdf->mean;

        /** Computes the eigenvalues/eigenvector decomposition of this matrix,
        *    so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the
        *         eigenvectors and the diagonal matrix D contains the eigenvalues
        *    as diagonal elements, sorted in <i>ascending</i> order.
        */
        cov.eigenVectors(m_fastDrawGauss_Z, D);

        // Scale eigenvectors with eigenvalues:
        D = D.array().sqrt().matrix();
        m_fastDrawGauss_Z = m_fastDrawGauss_Z * D;

        MRPT_END
}

/*---------------------------------------------------------------
                                prepareFastDrawSingleSample_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::prepareFastDrawSingleSample_modelThrun() const {}
/*---------------------------------------------------------------
                                prepareFastDrawSingleSample_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::fastDrawSingleSample_modelGaussian(
        CPose2D& outSample) const
{
        CVectorFloat rndVector(3, 0);
        for (size_t i = 0; i < 3; i++)
        {
                float rnd = getRandomGenerator().drawGaussian1D_normalized();
                for (size_t d = 0; d < 3; d++)
                        rndVector[d] += (m_fastDrawGauss_Z.get_unsafe(d, i) * rnd);
        }

        outSample = CPose2D(
                m_fastDrawGauss_M.x() + rndVector[0],
                m_fastDrawGauss_M.y() + rndVector[1],
                m_fastDrawGauss_M.phi() + rndVector[2]);
}

/*---------------------------------------------------------------
                                prepareFastDrawSingleSample_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::fastDrawSingleSample_modelThrun(
        CPose2D& outSample) const
{
        drawSingleSample_modelThrun(outSample);
}