class mrpt::poses::CRobot2DPoseEstimator

A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization/SLAM data.

The implemented model is a state vector:

TPose2D (x,y,phi) + TTwist2D (vx,vy,omega) The filter can be asked for an extrapolation for some arbitrary time t `, and it’ll do a simple linear prediction. All methods are thread-safe.

#include <mrpt/poses/CRobot2DPoseEstimator.h>

class CRobot2DPoseEstimator
{
public:
    // structs

    struct TOptions;

    //
fields

    TOptions params;

    // construction

    CRobot2DPoseEstimator();

    //
methods

    void reset();
    void processUpdateNewPoseLocalization(const mrpt::math::TPose2D& newPose, mrpt::Clock::time_point tim);

    void processUpdateNewOdometry(
        const mrpt::math::TPose2D& newGlobalOdometry,
        mrpt::Clock::time_point cur_tim,
        bool hasVelocities = false,
        const mrpt::math::TTwist2D& newRobotVelLocal = mrpt::math::TTwist2D()
        );

    bool getCurrentEstimate(mrpt::math::TPose2D& pose, mrpt::math::TTwist2D& velLocal, mrpt::math::TTwist2D& velGlobal, mrpt::Clock::time_point tim_query = mrpt::Clock::now()) const;
    bool getLatestRobotPose(mrpt::math::TPose2D& pose) const;
    bool getLatestRobotPose(mrpt::poses::CPose2D& pose) const;

    static void extrapolateRobotPose(
        const mrpt::math::TPose2D& p,
        const mrpt::math::TTwist2D& robot_vel_local,
        const double delta_time,
        mrpt::math::TPose2D& new_p
        );
};

Fields

TOptions params

parameters of the filter.

Construction

CRobot2DPoseEstimator()

Default constructor.

Methods

void reset()

Resets all internal state.

void processUpdateNewPoseLocalization(const mrpt::math::TPose2D& newPose, mrpt::Clock::time_point tim)

Updates the filter with new global-coordinates localization data from a localization or SLAM source.

Updates the filter so the pose is tracked to the current time.

Parameters:

tim	The timestamp of the sensor readings used to evaluate localization / SLAM.

void processUpdateNewOdometry(
    const mrpt::math::TPose2D& newGlobalOdometry,
    mrpt::Clock::time_point cur_tim,
    bool hasVelocities = false,
    const mrpt::math::TTwist2D& newRobotVelLocal = mrpt::math::TTwist2D()
    )

Updates the filter with new odometry readings.

Updates the filter so the pose is tracked to the current time.

bool getCurrentEstimate(mrpt::math::TPose2D& pose, mrpt::math::TTwist2D& velLocal, mrpt::math::TTwist2D& velGlobal, mrpt::Clock::time_point tim_query = mrpt::Clock::now()) const

Get the estimate for a given timestamp (defaults to now()), obtained as:

last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw

Returns:

true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.