Main MRPT website > C++ reference for MRPT 1.5.7
List of all members | Classes | Public Member Functions | Public Attributes | Static Private Member Functions | Private Attributes
mrpt::poses::CRobot2DPoseEstimator Class Reference

Detailed Description

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:

Definition at line 27 of file CRobot2DPoseEstimator.h.

#include <mrpt/poses/CRobot2DPoseEstimator.h>

Classes

struct  TOptions
 

Public Member Functions

 CRobot2DPoseEstimator ()
 Default constructor. More...
 
virtual ~CRobot2DPoseEstimator ()
 Destructor. More...
 
void reset ()
 Resets all internal state. More...
 
void processUpdateNewPoseLocalization (const mrpt::math::TPose2D &newPose, mrpt::system::TTimeStamp tim)
 Updates the filter with new global-coordinates localization data from a localization or SLAM source. More...
 
void processUpdateNewOdometry (const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
 Updates the filter with new odometry readings. More...
 
bool getCurrentEstimate (mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
 Get the estimate for a given timestamp (defaults to now()), obtained as: More...
 
bool getCurrentEstimate (mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
 
bool getCurrentEstimate (mrpt::poses::CPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
 
bool getLatestRobotPose (mrpt::math::TPose2D &pose) const
 Get the latest known robot pose, either from odometry or localization. More...
 
bool getLatestRobotPose (CPose2D &pose) const
 

Public Attributes

TOptions params
 parameters of the filter. More...
 

Static Private Member Functions

static void extrapolateRobotPose (const mrpt::math::TPose2D &p, const mrpt::math::TTwist2D &robot_vel_local, const double delta_time, mrpt::math::TPose2D &new_p)
 An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time". More...
 

Private Attributes

mrpt::synch::CCriticalSection m_cs
 
mrpt::system::TTimeStamp m_last_loc_time
 
mrpt::math::TPose2D m_last_loc
 Last pose as estimated by the localization/SLAM subsystem. More...
 
mrpt::math::TPose2D m_loc_odo_ref
 The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings) More...
 
mrpt::system::TTimeStamp m_last_odo_time
 
mrpt::math::TPose2D m_last_odo
 
mrpt::math::TTwist2D m_robot_vel_local
 Robot odometry-based velocity in a local frame of reference. More...
 

Constructor & Destructor Documentation

◆ CRobot2DPoseEstimator()

CRobot2DPoseEstimator::CRobot2DPoseEstimator ( )

Default constructor.

Definition at line 34 of file CRobot2DPoseEstimator.cpp.

◆ ~CRobot2DPoseEstimator()

CRobot2DPoseEstimator::~CRobot2DPoseEstimator ( )
virtual

Destructor.

Definition at line 42 of file CRobot2DPoseEstimator.cpp.

Member Function Documentation

◆ extrapolateRobotPose()

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

An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".

Definition at line 189 of file CRobot2DPoseEstimator.cpp.

References mrpt::poses::CPose2D::composePoint(), mrpt::math::TTwist2D::omega, mrpt::math::TPose2D::phi, R, mrpt::math::TTwist2D::vx, mrpt::math::TTwist2D::vy, mrpt::math::TPoint2D::x, mrpt::math::TPose2D::x, mrpt::math::TPoint2D::y, and mrpt::math::TPose2D::y.

◆ getCurrentEstimate() [1/3]

bool CRobot2DPoseEstimator::getCurrentEstimate ( mrpt::math::TPose2D pose,
mrpt::math::TTwist2D velLocal,
mrpt::math::TTwist2D velGlobal,
mrpt::system::TTimeStamp  tim_query = mrpt::system::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

Definition at line 119 of file CRobot2DPoseEstimator.cpp.

References INVALID_TIMESTAMP, mrpt::math::TPose2D::phi, mrpt::math::TTwist2D::rotate(), and mrpt::system::timeDifference().

Referenced by mrpt::slam::CMetricMapBuilderICP::processObservation().

◆ getCurrentEstimate() [2/3]

bool CRobot2DPoseEstimator::getCurrentEstimate ( mrpt::math::TPose2D pose,
float &  v,
float &  w,
mrpt::system::TTimeStamp  tim_query = mrpt::system::now() 
) const

◆ getCurrentEstimate() [3/3]

bool CRobot2DPoseEstimator::getCurrentEstimate ( mrpt::poses::CPose2D pose,
float &  v,
float &  w,
mrpt::system::TTimeStamp  tim_query = mrpt::system::now() 
) const

◆ getLatestRobotPose() [1/2]

bool CRobot2DPoseEstimator::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.
See also
getCurrentEstimate
Returns
true is the estimate can be trusted. False if the real observed data is too old.

Definition at line 162 of file CRobot2DPoseEstimator.cpp.

References INVALID_TIMESTAMP.

Referenced by mrpt::slam::CMetricMapBuilderICP::getCurrentPoseEstimation(), and mrpt::slam::CMetricMapBuilderICP::processObservation().

◆ getLatestRobotPose() [2/2]

bool CRobot2DPoseEstimator::getLatestRobotPose ( 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.

Definition at line 238 of file CRobot2DPoseEstimator.cpp.

References mrpt::poses::CPose2D::phi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

◆ processUpdateNewOdometry()

void CRobot2DPoseEstimator::processUpdateNewOdometry ( const mrpt::math::TPose2D newGlobalOdometry,
mrpt::system::TTimeStamp  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.

Definition at line 83 of file CRobot2DPoseEstimator.cpp.

References INVALID_TIMESTAMP, MRPT_END, MRPT_START, and mrpt::system::timeDifference().

Referenced by mrpt::slam::CMetricMapBuilderICP::processObservation().

◆ processUpdateNewPoseLocalization()

void CRobot2DPoseEstimator::processUpdateNewPoseLocalization ( const mrpt::math::TPose2D newPose,
mrpt::system::TTimeStamp  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
timThe timestamp of the sensor readings used to evaluate localization / SLAM.

Definition at line 64 of file CRobot2DPoseEstimator.cpp.

References INVALID_TIMESTAMP, and mrpt::system::timeDifference().

Referenced by mrpt::slam::CMetricMapBuilderICP::initialize(), and mrpt::slam::CMetricMapBuilderICP::processObservation().

◆ reset()

void CRobot2DPoseEstimator::reset ( )

Resets all internal state.

Definition at line 49 of file CRobot2DPoseEstimator.cpp.

References INVALID_TIMESTAMP.

Referenced by mrpt::slam::CMetricMapBuilderICP::initialize().

Member Data Documentation

◆ m_cs

mrpt::synch::CCriticalSection mrpt::poses::CRobot2DPoseEstimator::m_cs
private

Definition at line 86 of file CRobot2DPoseEstimator.h.

◆ m_last_loc

mrpt::math::TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_loc
private

Last pose as estimated by the localization/SLAM subsystem.

Definition at line 89 of file CRobot2DPoseEstimator.h.

◆ m_last_loc_time

mrpt::system::TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time
private

Definition at line 88 of file CRobot2DPoseEstimator.h.

◆ m_last_odo

mrpt::math::TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_odo
private

Definition at line 94 of file CRobot2DPoseEstimator.h.

◆ m_last_odo_time

mrpt::system::TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time
private

Definition at line 93 of file CRobot2DPoseEstimator.h.

◆ m_loc_odo_ref

mrpt::math::TPose2D mrpt::poses::CRobot2DPoseEstimator::m_loc_odo_ref
private

The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)

Definition at line 91 of file CRobot2DPoseEstimator.h.

◆ m_robot_vel_local

mrpt::math::TTwist2D mrpt::poses::CRobot2DPoseEstimator::m_robot_vel_local
private

Robot odometry-based velocity in a local frame of reference.

Definition at line 95 of file CRobot2DPoseEstimator.h.

◆ params

TOptions mrpt::poses::CRobot2DPoseEstimator::params

parameters of the filter.

Definition at line 83 of file CRobot2DPoseEstimator.h.




Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019