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 30 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 () |
void | processUpdateNewPoseLocalization (const mrpt::math::TPose2D &newPose, const mrpt::math::CMatrixDouble33 &newPoseCov, mrpt::system::TTimeStamp cur_tim) |
Updates the filter so the pose is tracked to the current time. More... | |
void | processUpdateNewOdometry (const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, float v=0, float w=0) |
Updates the filter so the pose is tracked to the current time. More... | |
bool | getCurrentEstimate (mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const |
Get the current estimate, obtained as: More... | |
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 float v, const float w, 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::CMatrixDouble33 | m_last_loc_cov |
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 |
float | m_robot_v |
float | m_robot_w |
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, |
float & | v, | ||
float & | w, | ||
mrpt::system::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 | ( | mrpt::poses::CPose2D & | pose, |
float & | v, | ||
float & | w, | ||
mrpt::system::TTimeStamp | tim_query = mrpt::system::now() |
||
) | const |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
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 , |
||
float | v = 0 , |
||
float | w = 0 |
||
) |
Updates the filter so the pose is tracked to the current time.
void mrpt::poses::CRobot2DPoseEstimator::processUpdateNewPoseLocalization | ( | const mrpt::math::TPose2D & | newPose, |
const mrpt::math::CMatrixDouble33 & | newPoseCov, | ||
mrpt::system::TTimeStamp | cur_tim | ||
) |
Updates the filter so the pose is tracked to the current time.
void mrpt::poses::CRobot2DPoseEstimator::reset | ( | ) |
|
private |
Definition at line 87 of file CRobot2DPoseEstimator.h.
|
private |
Last pose as estimated by the localization/SLAM subsystem.
Definition at line 90 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 91 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 89 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 96 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 95 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 93 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 97 of file CRobot2DPoseEstimator.h.
|
private |
Definition at line 98 of file CRobot2DPoseEstimator.h.
TOptions mrpt::poses::CRobot2DPoseEstimator::params |
parameters of the filter.
Definition at line 84 of file CRobot2DPoseEstimator.h.
Page generated by Doxygen 1.8.13 for MRPT 1.4.0 SVN: at Fri Mar 17 07:27:15 UTC 2017 |