A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.
The implemented model is a state vector:
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.
Definition at line 49 of file CRobot2DPoseEstimator.h.
#include <mrpt/poses/CRobot2DPoseEstimator.h>
Classes | |
struct | TOptions |
Public Member Functions | |
CRobot2DPoseEstimator () | |
Default constructor. | |
virtual | ~CRobot2DPoseEstimator () |
Destructor. | |
void | reset () |
void | processUpdateNewPoseLocalization (const TPose2D &newPose, const CMatrixDouble33 &newPoseCov, TTimeStamp cur_tim) |
Updates the filter so the pose is tracked to the current time. | |
void | processUpdateNewOdometry (const TPose2D &newGlobalOdometry, TTimeStamp cur_tim, bool hasVelocities=false, float v=0, float w=0) |
Updates the filter so the pose is tracked to the current time. | |
bool | getCurrentEstimate (TPose2D &pose, float &v, float &w, TTimeStamp tim_query=mrpt::system::now()) const |
Get the current estimate, obtained as: | |
bool | getCurrentEstimate (CPose2D &pose, float &v, float &w, TTimeStamp tim_query=mrpt::system::now()) const |
Get the current estimate, obtained as: | |
bool | getLatestRobotPose (TPose2D &pose) const |
Get the latest known robot pose, either from odometry or localization. | |
bool | getLatestRobotPose (CPose2D &pose) const |
Get the latest known robot pose, either from odometry or localization. | |
Public Attributes | |
TOptions | params |
parameters of the filter. | |
Static Private Member Functions | |
static void | extrapolateRobotPose (const TPose2D &p, const float v, const float w, const double delta_time, 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". | |
Private Attributes | |
mrpt::synch::CCriticalSection | m_cs |
TTimeStamp | m_last_loc_time |
TPose2D | m_last_loc |
Last pose as estimated by the localization/SLAM subsystem. | |
CMatrixDouble33 | m_last_loc_cov |
TPose2D | m_loc_odo_ref |
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings) | |
TTimeStamp | m_last_odo_time |
TPose2D | m_last_odo |
float | m_robot_v |
float | m_robot_w |
mrpt::poses::CRobot2DPoseEstimator::CRobot2DPoseEstimator | ( | ) |
Default constructor.
virtual mrpt::poses::CRobot2DPoseEstimator::~CRobot2DPoseEstimator | ( | ) | [virtual] |
Destructor.
static void mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose | ( | const TPose2D & | p, |
const float | v, | ||
const float | w, | ||
const double | delta_time, | ||
TPose2D & | new_p | ||
) | [static, private] |
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 | ( | TPose2D & | pose, |
float & | v, | ||
float & | w, | ||
TTimeStamp | tim_query = mrpt::system::now() |
||
) | const |
Get the current estimate, obtained as:
last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate | ( | CPose2D & | pose, |
float & | v, | ||
float & | w, | ||
TTimeStamp | tim_query = mrpt::system::now() |
||
) | const [inline] |
Get the current estimate, obtained as:
last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw
Definition at line 86 of file CRobot2DPoseEstimator.h.
References mrpt::poses::CPose2D.
bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose | ( | CPose2D & | pose | ) | const [inline] |
Get the latest known robot pose, either from odometry or localization.
Definition at line 105 of file CRobot2DPoseEstimator.h.
References mrpt::math::TPose2D::phi, mrpt::poses::CPose2D::phi(), mrpt::math::TPose2D::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::math::TPose2D::y, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().
bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose | ( | 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.
void mrpt::poses::CRobot2DPoseEstimator::processUpdateNewOdometry | ( | const TPose2D & | newGlobalOdometry, |
TTimeStamp | cur_tim, | ||
bool | hasVelocities = false , |
||
float | v = 0 , |
||
float | w = 0 |
||
) |
Updates the filter so the pose is tracked to the current time.
void mrpt::poses::CRobot2DPoseEstimator::processUpdateNewPoseLocalization | ( | const TPose2D & | newPose, |
const CMatrixDouble33 & | newPoseCov, | ||
TTimeStamp | cur_tim | ||
) |
Updates the filter so the pose is tracked to the current time.
void mrpt::poses::CRobot2DPoseEstimator::reset | ( | ) |
Definition at line 133 of file CRobot2DPoseEstimator.h.
Last pose as estimated by the localization/SLAM subsystem.
Definition at line 136 of file CRobot2DPoseEstimator.h.
Definition at line 137 of file CRobot2DPoseEstimator.h.
Definition at line 135 of file CRobot2DPoseEstimator.h.
Definition at line 142 of file CRobot2DPoseEstimator.h.
Definition at line 141 of file CRobot2DPoseEstimator.h.
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)
Definition at line 139 of file CRobot2DPoseEstimator.h.
float mrpt::poses::CRobot2DPoseEstimator::m_robot_v [private] |
Definition at line 143 of file CRobot2DPoseEstimator.h.
float mrpt::poses::CRobot2DPoseEstimator::m_robot_w [private] |
Definition at line 144 of file CRobot2DPoseEstimator.h.
parameters of the filter.
Definition at line 130 of file CRobot2DPoseEstimator.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |