CActionRobotMovement3D.h

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

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

namespace mrpt
{
namespace 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.
          */
        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.

}  // End of namespace
}  // End of namespace

#endif