CActionRobotMovement2D.cpp

Go to the documentation of this file.

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

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

#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/point_poses2vectors.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>

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);
        const auto& gm = motionModelConfiguration.gaussianModel;
        out.WriteAs<float>(gm.a1);
        out.WriteAs<float>(gm.a2);
        out.WriteAs<float>(gm.a3);
        out.WriteAs<float>(gm.a4);
        out.WriteAs<float>(gm.minStdXY);
        out.WriteAs<float>(gm.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);

                auto& gm = motionModelConfiguration.gaussianModel;
                gm.a1 = in.ReadAs<float>();
                gm.a2 = in.ReadAs<float>();
                gm.a3 = in.ReadAs<float>();
                gm.a4 = in.ReadAs<float>();
                gm.minStdXY = in.ReadAs<float>();
                gm.minStdPHI = in.ReadAs<float>();

                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);
}

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

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

    // Build the odometry noise matrix:
    double Al = odometryIncrement.norm();
    auto 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:
    aux->cov = mrpt::math::multiply_HCHt(J, C_ODO);
}

/*---------------------------------------------------------------
                computeFromOdometry_modelThrun
  ---------------------------------------------------------------*/
void CActionRobotMovement2D::computeFromOdometry_modelThrun(
    const CPose2D& odometryIncrement, const TMotionModelOptions& o)
{
    // The Gaussian PDF:
    // ---------------------------
    const mrpt::math::TPose2D nullPose(0, 0, 0);

    poseChange = CPosePDFParticles::Create();
    auto* aux = dynamic_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));

    const auto* gPdf = dynamic_cast<const CPosePDFGaussian*>(poseChange.get());
    ASSERT_(gPdf != nullptr);
    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.
     */
    std::vector<double> eigvals;
    cov.eig_symmetric(m_fastDrawGauss_Z, eigvals);
    CMatrixDouble33 D;
    D.setDiagonal(eigvals);

    // Scale eigenvectors with eigenvalues:
    D.asEigen() = 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(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);
}