CActionRobotMovement2D.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 "obs-precomp.h"  // Precompiled headers
 
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/serialization/CArchive.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::poses;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;
 
IMPLEMENTS_SERIALIZABLE(CActionRobotMovement2D, CAction, mrpt::obs)
 
// Note: dont move this to the .h, to avoid having to include the definition of
// the full pose type
CActionRobotMovement2D::CActionRobotMovement2D()
    : poseChange(mrpt::poses::CPosePDFGaussian::Create())
{
}
 
uint8_t CActionRobotMovement2D::serializeGetVersion() const { return 7; }
void CActionRobotMovement2D::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    out.WriteAs<uint32_t>(estimationMethod);
    // 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;
        out.WriteAs<uint32_t>(motionModelConfiguration.modelSelection);
        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;
}
 
void CActionRobotMovement2D::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t 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;
    const mrpt::math::TPose2D 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);
}