9 #ifndef CRobot2DPoseEstimator_H
10 #define CRobot2DPoseEstimator_H
47 bool hasVelocities =
false,
76 max_odometry_age ( 1.0 ),
77 max_localiz_age ( 4.0 )
106 const double delta_time,
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
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:
mrpt::system::TTimeStamp m_last_odo_time
mrpt::synch::CCriticalSection m_cs
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
virtual ~CRobot2DPoseEstimator()
Destructor.
bool getLatestRobotPose(CPose2D &pose) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
bool 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 o...
mrpt::system::TTimeStamp m_last_loc_time
mrpt::math::TPose2D m_last_loc
Last pose as estimated by the localization/SLAM subsystem.
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.
CRobot2DPoseEstimator()
Default constructor.
mrpt::math::CMatrixDouble33 m_last_loc_cov
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.
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,...
mrpt::math::TPose2D m_loc_odo_ref
The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subseq...
mrpt::math::TPose2D m_last_odo
TOptions params
parameters of the filter.
This class provides simple critical sections functionality.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double max_localiz_age
To consider data old, in seconds.
double max_odometry_age
To consider data old, in seconds.