CActionRobotMovement3D.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/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/poses/CPose3DPDFParticles.h>

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::random;

IMPLEMENTS_SERIALIZABLE(CActionRobotMovement3D, CAction, mrpt::obs)

/*---------------------------------------------------------------
                        Constructor
  ---------------------------------------------------------------*/
CActionRobotMovement3D::CActionRobotMovement3D()
    : poseChange(),
      rawOdometryIncrementReading(),
      estimationMethod(emOdometry),
      motionModelConfiguration(),
      hasVelocities(6, false),
      velocities(6)
{
    velocities.assign(.0);
}

uint8_t CActionRobotMovement3D::serializeGetVersion() const { return 1; }
void CActionRobotMovement3D::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    out.WriteAs<uint32_t>(estimationMethod);
    out << poseChange;
    out << hasVelocities << velocities;
    out << timestamp;
}

void CActionRobotMovement3D::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        {
            uint32_t i;

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

            in >> poseChange;
            in >> hasVelocities >> velocities;

            if (version >= 1)
                in >> timestamp;
            else
                timestamp = INVALID_TIMESTAMP;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
}

void CActionRobotMovement3D::computeFromOdometry(
    const CPose3D& odometryIncrement, const TMotionModelOptions& options)
{
    // Set the members data:
    estimationMethod = emOdometry;
    rawOdometryIncrementReading = odometryIncrement;
    motionModelConfiguration = options;

    if (options.modelSelection == mm6DOF)
        computeFromOdometry_model6DOF(odometryIncrement, options);
}

/*---------------------------------------------------------------
                        TMotionModelOptions
 ---------------------------------------------------------------*/
CActionRobotMovement3D::TMotionModelOptions::TMotionModelOptions()
    : modelSelection(mm6DOF), mm6DOFModel()
{
    mm6DOFModel.nParticlesCount = 300;
    mm6DOFModel.a1 = 0.05f;
    mm6DOFModel.a2 = 0.05f;
    mm6DOFModel.a3 = 0.05f;
    mm6DOFModel.a4 = 0.05f;
    mm6DOFModel.a5 = 0.05f;
    mm6DOFModel.a6 = 0.05f;
    mm6DOFModel.a7 = 0.05f;
    mm6DOFModel.a8 = 0.05f;
    mm6DOFModel.a9 = 0.05f;
    mm6DOFModel.a10 = 0.05f;
    mm6DOFModel.additional_std_XYZ = 0.001f;
    mm6DOFModel.additional_std_angle = DEG2RAD(0.05f);
}

/*---------------------------------------------------------------
                computeFromOdometry_model6DOF
    ---------------------------------------------------------------*/
void CActionRobotMovement3D::computeFromOdometry_model6DOF(
    const CPose3D& odometryIncrement, const TMotionModelOptions& o)
{
    // The Gaussian PDF:
    // ---------------------------
    CPose3DPDFParticles* aux;
    const mrpt::math::TPose3D nullPose(0, 0, 0, 0, 0, 0);

    mrpt::poses::CPose3DPDF::Ptr poseChangeTemp =
        mrpt::make_aligned_shared<CPose3DPDFParticles>();
    aux = static_cast<CPose3DPDFParticles*>(poseChangeTemp.get());

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

    // The motion model: A. L. Ballardini, A. Furlan, A. Galbiati, M. Matteucci,
    // F. Sacchi, D. G. Sorrenti An effective 6DoF motion model for 3D-6DoF
    // Monte Carlo Localization 4th Workshop on Planning, Perception and
    // Navigation for Intelligent Vehicles, IROS, 2012

    /*
        The brief description:
        Find XYZ deltas: dX=x(t)-x(t-1); dY=y(t)-y(t-1); dZ=z(t)-z(t-1)
        Find angular deltas (normalised): dRoll=roll(t)-roll(t-1);
       dPitch=pitch(t)-pitch(t-1); dYaw=yaw(t)-yaw(t-1)

        Here t - is time step

         Calculate the odometry for 6DOF:


                yaw1 = atan2 (dY,dX)
                pitch1 = atan2 (dZ , sqrt(dX^2 + dY^2 + dZ^2) )
                trans = sqrt(dX^2 + dY^2 + dZ^2)
                roll = dRoll
                yaw2= dYaw
                pitch2= dPitch


        Model the error:


                yaw1_draw = yaw1 + sample(a1 * yaw1 + a2 * trans )
                pitch1_draw = pitch1 + sample(a3 * dZ )
                trans_draw = trans + sample( a4 * trans + a5 * yaw2 + a6 * (roll
       + pitch2 ) )
                roll_draw = roll + sample(a7 * roll)
                pitch2_draw = pitch2 + sample(a8 * pitch2 )
                yaw2_draw = yaw2 + sample(a9 * yaw2 + a10 * trans )

         Here sample() - sampling from zero mean Gaussian distribution

        Calculate the spherical coordinates:
                x = trans_draw * sin( pitch1_draw ) * cos ( yaw1_draw )
                y = trans_draw * sin( pitch1_draw ) * sin ( yaw1_draw )
                z = trans_draw * cos( pitch1_draw )
         roll = roll_draw
        pitch = pitch1_draw + pitch2_draw
            yaw = yaw1_draw + yaw2_draw

        normalize_angle(roll, pitch, yaw )
    */

    // The increments in odometry:
    float Ayaw1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0)
                      ? atan2(odometryIncrement.y(), odometryIncrement.x())
                      : 0;

    float Atrans = odometryIncrement.norm();

    float Apitch1 = (odometryIncrement.y() != 0 || odometryIncrement.x() != 0 ||
                     odometryIncrement.z() != 0)
                        ? atan2(odometryIncrement.z(), odometryIncrement.norm())
                        : 0;

    float Aroll = odometryIncrement.roll();
    float Apitch2 = odometryIncrement.pitch();
    float Ayaw2 = odometryIncrement.yaw();

    // Draw samples:
    for (size_t i = 0; i < o.mm6DOFModel.nParticlesCount; i++)
    {
        float Ayaw1_draw =
            Ayaw1 +
            (o.mm6DOFModel.a1 * Ayaw1 + o.mm6DOFModel.a2 * Atrans) *
                getRandomGenerator().drawGaussian1D_normalized();
        float Apitch1_draw =
            Apitch1 +
            (o.mm6DOFModel.a3 * odometryIncrement.z()) *
                getRandomGenerator().drawGaussian1D_normalized();
        float Atrans_draw =
            Atrans +
            (o.mm6DOFModel.a4 * Atrans + o.mm6DOFModel.a5 * Ayaw2 +
             o.mm6DOFModel.a6 * (Aroll + Apitch2)) *
                getRandomGenerator().drawGaussian1D_normalized();

        float Aroll_draw = Aroll +
                           (o.mm6DOFModel.a7 * Aroll) *
                               getRandomGenerator().drawGaussian1D_normalized();
        float Apitch2_draw =
            Apitch2 +
            (o.mm6DOFModel.a8 * Apitch2) *
                getRandomGenerator().drawGaussian1D_normalized();
        float Ayaw2_draw =
            Ayaw2 +
            (o.mm6DOFModel.a9 * Ayaw2 + o.mm6DOFModel.a10 * Atrans) *
                getRandomGenerator().drawGaussian1D_normalized();

        // Output:
        aux->m_particles[i].d.x =
            Atrans_draw * sin(Apitch1_draw) * cos(Ayaw1_draw) +
            motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
                getRandomGenerator().drawGaussian1D_normalized();
        aux->m_particles[i].d.y = 
            Atrans_draw * sin(Apitch1_draw) * sin(Ayaw1_draw) +
            motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
                getRandomGenerator().drawGaussian1D_normalized();
        aux->m_particles[i].d.z = 
            Atrans_draw * cos(Apitch1_draw) +
            motionModelConfiguration.mm6DOFModel.additional_std_XYZ *
                getRandomGenerator().drawGaussian1D_normalized();

        double new_yaw =
            Ayaw1_draw + Ayaw2_draw +
            motionModelConfiguration.mm6DOFModel.additional_std_angle *
                getRandomGenerator().drawGaussian1D_normalized();
        double new_pitch =
            Apitch1_draw + Apitch2_draw +
            motionModelConfiguration.mm6DOFModel.additional_std_angle *
                getRandomGenerator().drawGaussian1D_normalized();
        double new_roll =
            Aroll_draw +
            motionModelConfiguration.mm6DOFModel.additional_std_angle *
                getRandomGenerator().drawGaussian1D_normalized();

        aux->m_particles[i].d.yaw = new_yaw;
        aux->m_particles[i].d.pitch = new_pitch;
        aux->m_particles[i].d.roll = new_roll;
    }

    poseChange.copyFrom(*poseChangeTemp);
}