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.

See also:

getLatestRobotPose

bool getLatestRobotPose(mrpt::math::TPose2D& pose) const

Get the latest known robot pose, either from odometry or localization.

get the current estimate

This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.

Returns:

false if there is not estimation yet.

true is the estimate can be trusted. False if the real observed data is too old.

See also:

getCurrentEstimate

bool getLatestRobotPose(mrpt::poses::CPose2D& pose) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

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

Auxiliary static method to extrapolate the pose of a robot located at “p” with velocities (v,w) after a time delay “delta_time”.