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

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

IMPLEMENTS_SERIALIZABLE(CActionRobotMovement3D, CAction, mrpt::obs)

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

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

                out << i;

                // The PDF:
                out << poseChange;

                out << hasVelocities << velocities;

                out << timestamp;
        }
}

/*---------------------------------------------------------------
  Implements the reading from a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CActionRobotMovement3D::readFromStream(
        mrpt::utils::CStream& in, int 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;
        static CPose3D 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->setYawPitchRoll(new_yaw, new_pitch, new_roll);
                aux->m_particles[i].d->normalizeAngles();
        }

        poseChange.copyFrom(*poseChangeTemp);
}