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:
t'
, and it'll do a simple linear prediction. All methods are thread-safe. 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... | |
mrpt::poses::CRobot2DPoseEstimator::CRobot2DPoseEstimator | ( | ) |
Default constructor.
|
virtual |
Destructor.
|
staticprivate |
An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
bool mrpt::poses::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
bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate | ( | mrpt::math::TPose2D & | pose, |
float & | v, | ||
float & | w, | ||
mrpt::system::TTimeStamp | tim_query = mrpt::system::now() |
||
) | const |
bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate | ( | mrpt::poses::CPose2D & | pose, |
float & | v, | ||
float & | w, | ||
mrpt::system::TTimeStamp | tim_query = mrpt::system::now() |
||
) | const |
bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose | ( | mrpt::math::TPose2D & | pose | ) | const |
Get the latest known robot pose, either from odometry or localization.
This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.
bool mrpt::poses::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.
void mrpt::poses::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.
void mrpt::poses::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.
tim | The timestamp of the sensor readings used to evaluate localization / SLAM. |
void mrpt::poses::CRobot2DPoseEstimator::reset | ( | ) |
Resets all internal state.
|
private |
Definition at line 86 of file CRobot2DPoseEstimator.h.
|
private |
Last pose as estimated by the localization/SLAM subsystem.
Definition at line 89 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 88 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 94 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 93 of file CRobot2DPoseEstimator.h.
|
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.
|
private |
Robot odometry-based velocity in a local frame of reference.
Definition at line 95 of file CRobot2DPoseEstimator.h.
TOptions mrpt::poses::CRobot2DPoseEstimator::params |
parameters of the filter.
Definition at line 83 of file CRobot2DPoseEstimator.h.
Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Sun Nov 26 00:44:48 UTC 2017 |