CActionRobotMovement2D.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 CActionRobotMovement2D_H
#define CActionRobotMovement2D_H

#include <mrpt/obs/CAction.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/utils/poly_ptr_ptr.h>
#include <mrpt/math/lightweight_geom_data.h>

namespace mrpt
{
namespace obs
{
/** Represents a probabilistic 2D movement of the robot mobile base
 *
 *  See the tutorial on <a
 * href="http://www.mrpt.org/Probabilistic_Motion_Models" >probabilistic motion
 * models</a>.
 *
 * \note [New in MRPT 1.5.0] Velocity is now encoded as mrpt::math::TTwist2D as
 * a more general version of the old (linVel, angVel).
 * \sa CAction
 * \ingroup mrpt_obs_grp
 */
class CActionRobotMovement2D : public CAction
{
        DEFINE_SERIALIZABLE(CActionRobotMovement2D)

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

        /** Constructor */
        CActionRobotMovement2D();

        /** The 2D pose change probabilistic estimation. */
        mrpt::utils::poly_ptr_ptr<mrpt::poses::CPosePDF::Ptr> poseChange;
        /** This is the raw odometry reading, and only is used when
         * "estimationMethod" is "TEstimationMethod::emOdometry" */
        mrpt::poses::CPose2D rawOdometryIncrementReading;
        /** This fields indicates the way in which this estimation was obtained. */
        TEstimationMethod estimationMethod;

        /** If "true" means that "encoderLeftTicks" and "encoderRightTicks" contain
         * valid values. */
        bool hasEncodersInfo;
        /** For odometry only: the ticks count for each wheel FROM the last reading
         * (positive means FORWARD, for both wheels);
          * \sa hasEncodersInfo
          */
        int32_t encoderLeftTicks, encoderRightTicks;

        /** If "true" means that "velocityLin" and "velocityAng" contain valid
         * values. */
        bool hasVelocities;
        /** If "hasVelocities"=true, the robot velocity in local (robot frame, +X
         * forward) coordinates. */
        mrpt::math::TTwist2D velocityLocal;

        double velocityLin() const { return velocityLocal.vx; }
        double velocityAng() const { return velocityLocal.omega; }
        enum TDrawSampleMotionModel
        {
                mmGaussian = 0,
                mmThrun
        };
        /** The parameter to be passed to "computeFromOdometry". */
        struct TMotionModelOptions
        {
                /** Default values loader. */
                TMotionModelOptions();

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

                /** Options for the gaussian model, which generates a CPosePDFGaussian
                 * object in poseChange
                  * See docs in :
                 * http://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
                  */
                struct TOptions_GaussianModel
                {
                        float a1, a2, a3, a4, minStdXY, minStdPHI;
                } gaussianModel;

                /** Options for the Thrun's model, which generates a CPosePDFParticles
                 * object in poseChange
                  * See docs in :
                 * http://www.mrpt.org/tutorials/programming/odometry-and-motion-models/probabilistic_motion_models/
                  */
                struct TOptions_ThrunModel
                {
                        /** The default number of particles to generate in a internal
                         * representation (anyway you can draw as many samples as you want
                         * through CActionRobotMovement2D::drawSingleSample)  */
                        uint32_t nParticlesCount;
                        float alfa1_rot_rot;
                        float alfa2_rot_trans;
                        float alfa3_trans_trans;
                        float alfa4_trans_rot;

                        /** An additional noise added to the thrun model (std. dev. in
                         * meters and radians). */
                        float additional_std_XY, additional_std_phi;
                } thrunModel;

        } 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_modelGaussian, computeFromOdometry_modelThrun
          */
        void computeFromOdometry(
                const mrpt::poses::CPose2D& odometryIncrement,
                const TMotionModelOptions& options);

        /** If "hasEncodersInfo"=true, this method updates the pose estimation
         * according to the ticks from both encoders and the passed parameters,
         * which is passed internally to the method "computeFromOdometry" with the
         * last used PDF options (or the defualt ones if not explicitly called by
         * the user).
          *
          * \param K_left The meters / tick ratio for the left encoder.
          * \param K_right The meters / tick ratio for the right encoder.
          * \param D The distance between both wheels, in meters.
          */
        void computeFromEncoders(double K_left, double K_right, double D);

        /** Using this method instead of "poseChange->drawSingleSample()" may be
         * more efficient in most situations.
          * \sa CPosePDF::drawSingleSample
          */
        void drawSingleSample(mrpt::poses::CPose2D& outSample) const;

        /** Call this before calling a high number of times "fastDrawSingleSample",
         * which is much faster than "drawSingleSample"
          */
        void prepareFastDrawSingleSamples() const;

        /** Faster version than "drawSingleSample", but requires a previous call to
         * "prepareFastDrawSingleSamples"
          */
        void fastDrawSingleSample(mrpt::poses::CPose2D& outSample) const;

   protected:
        /** Computes the PDF of the pose increment from an odometry reading, using a
         * Gaussian approximation as the motion model.
         * \sa computeFromOdometry
         */
        void computeFromOdometry_modelGaussian(
                const mrpt::poses::CPose2D& odometryIncrement,
                const TMotionModelOptions& o);

        /** Computes the PDF of the pose increment from an odometry reading, using
         * the motion model from Thrun's book.
         * This model is discussed in "Probabilistic Robotics", Thrun, Burgard, and
         * Fox, 2006, pp.136.
         * \sa computeFromOdometry
         */
        void computeFromOdometry_modelThrun(
                const mrpt::poses::CPose2D& odometryIncrement,
                const TMotionModelOptions& o);

        /** The sample generator for the model "computeFromOdometry_modelGaussian",
         * internally called when the user invokes "drawSingleSample".
          */
        void drawSingleSample_modelGaussian(mrpt::poses::CPose2D& outSample) const;

        /** The sample generator for the model "computeFromOdometry_modelThrun",
         * internally called when the user invokes "drawSingleSample".
          */
        void drawSingleSample_modelThrun(mrpt::poses::CPose2D& outSample) const;

        /** Internal use
          */
        void prepareFastDrawSingleSample_modelGaussian() const;

        /** Internal use
          */
        void prepareFastDrawSingleSample_modelThrun() const;

        /** Internal use
          */
        void fastDrawSingleSample_modelGaussian(
                mrpt::poses::CPose2D& outSample) const;

        /** Internal use
          */
        void fastDrawSingleSample_modelThrun(mrpt::poses::CPose2D& outSample) const;

        /** Auxiliary matrix
          */
        mutable mrpt::math::CMatrixDouble33 m_fastDrawGauss_Z;
        mutable mrpt::poses::CPose2D m_fastDrawGauss_M;

};  // End of class def.

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

#endif