CActionRobotMovement3D.h

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 |
   +------------------------------------------------------------------------+ */
#ifndef CActionRobotMovement3D_H
#define CActionRobotMovement3D_H

#include <mrpt/obs/CAction.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>

namespace mrpt::obs
{
/** Represents a probabilistic 3D (6D) movement.
*   Currently this can be determined from visual odometry for full 6D, or from
* wheel encoders for 2D movements only.
* Here implemented the  motion model from the next article: 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
* \ingroup mrpt_obs_grp
* \sa CAction
*/
class CActionRobotMovement3D : public CAction
{
    DEFINE_SERIALIZABLE(CActionRobotMovement3D)

   public:
    /** A list of posible ways for estimating the content of a
     * CActionRobotMovement3D object.
        */
    enum TEstimationMethod
    {
        emOdometry = 0,
        emVisualOdometry
    };

    CActionRobotMovement3D();

    /** The 3D pose change probabilistic estimation. It can be converted to/from
     * these alternative classes:
      * - mrpt::poses::CPose3DQuatPDFGaussian
      */
    mrpt::poses::CPose3DPDFGaussian poseChange;

    /** This is the raw odometry reading, and only is used when
     * "estimationMethod" is "TEstimationMethod::emOdometry" */
    mrpt::poses::CPose3D rawOdometryIncrementReading;
    /** This fields indicates the way this estimation was obtained.
      */
    TEstimationMethod estimationMethod;

    enum TDrawSampleMotionModel
    {
        mmGaussian = 0,
        mm6DOF
    };

    /** The parameter to be passed to "computeFromOdometry". */
    struct TMotionModelOptions
    {
        /** Default values loader. */
        TMotionModelOptions();

        /** The model to be used. */
        TDrawSampleMotionModel modelSelection;

        struct TOptions_6DOFModel
        {
            /** Options for the 6DOFModel model  which generates a
             * CPosePDFParticles object an then create from that
             * CPosePDFGaussian object  in poseChange */
            uint32_t nParticlesCount;
            float a1, a2, a3, a4, a5, a6, a7, a8, a9, a10;
            /** An additional noise added to the 6DOF model (std. dev. in meters
             * and radians). */
            float additional_std_XYZ, additional_std_angle;
        } mm6DOFModel;

    } motionModelConfiguration;

    /** Computes the PDF of the pose increment from an odometry reading and
     * according to the given motion model (speed and encoder ticks information
     * is not modified).
      * According to the parameters in the passed struct, it will be called one
     * the private sampling functions (see "see also" next).
      * \sa computeFromOdometry_model6DOF
      */
    void computeFromOdometry(
        const mrpt::poses::CPose3D& odometryIncrement,
        const TMotionModelOptions& options);
    /** Computes the PDF of the pose increment from an odometry reading, using
     * the motion model for 6 DOF.
     *  The source: 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
     * \sa computeFromOdometry
     */
    void computeFromOdometry_model6DOF(
        const mrpt::poses::CPose3D& odometryIncrement,
        const TMotionModelOptions& o);

    /** Each "true" entry means that the corresponding "velocities" element
     * contains valid data - There are 6 entries.
      */
    std::vector<bool> hasVelocities;

    /** The velocity of the robot in each of 6D:
     * v_x,v_y,v_z,v_yaw,v_pitch,v_roll (linear in meters/sec and angular in
     * rad/sec).
      */
    mrpt::math::CVectorFloat velocities;

};  // End of class def.

}
#endif