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:

TPose2D (x,y,phi) + TTwist2D (vx,vy,omega) 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 29 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::Clock::time_point 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::Clock::time_point 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::Clock::time_point tim_query=mrpt::Clock::now()) const
	Get the estimate for a given timestamp (defaults to `now()`), obtained as: More...

bool	getLatestRobotPose (mrpt::math::TPose2D &pose) const
	Get the latest known robot pose, either from odometry or localization. More...

bool	getLatestRobotPose (mrpt::poses::CPose2D &pose) const

Static Public 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)
	Auxiliary static method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time". More...

Public Attributes
TOptions	params
	parameters of the filter. More...

Private Attributes
std::mutex	m_cs

std::optional< mrpt::Clock::time_point >	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...

std::optional< mrpt::Clock::time_point >	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 29 of file CRobot2DPoseEstimator.cpp.

◆ ~CRobot2DPoseEstimator()

CRobot2DPoseEstimator::~CRobot2DPoseEstimator ( )

virtualdefault

Destructor.

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
	)

static

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

Definition at line 170 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_data< T >::x, mrpt::math::TPose2D::x, mrpt::math::TPoint2D_data< T >::y, and mrpt::math::TPose2D::y.

Referenced by TEST().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getCurrentEstimate()

bool CRobot2DPoseEstimator::getCurrentEstimate	(	mrpt::math::TPose2D &	pose,
		mrpt::math::TTwist2D &	velLocal,
		mrpt::math::TTwist2D &	velGlobal,
		mrpt::Clock::time_point	tim_query = `mrpt::Clock::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 109 of file CRobot2DPoseEstimator.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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 142 of file CRobot2DPoseEstimator.cpp.

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

Here is the caller graph for this function:

◆ getLatestRobotPose() [2/2]

bool CRobot2DPoseEstimator::getLatestRobotPose ( mrpt::poses::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 206 of file CRobot2DPoseEstimator.cpp.

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

Here is the call graph for this function:

◆ processUpdateNewOdometry()

void CRobot2DPoseEstimator::processUpdateNewOdometry	(	const mrpt::math::TPose2D &	newGlobalOdometry,
		mrpt::Clock::time_point	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 70 of file CRobot2DPoseEstimator.cpp.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ processUpdateNewPoseLocalization()

void CRobot2DPoseEstimator::processUpdateNewPoseLocalization	(	const mrpt::math::TPose2D &	newPose,
		mrpt::Clock::time_point	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

tim	The timestamp of the sensor readings used to evaluate localization / SLAM.

Definition at line 52 of file CRobot2DPoseEstimator.cpp.

References mrpt::system::timeDifference().

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reset()

void CRobot2DPoseEstimator::reset ( )

Resets all internal state.

Definition at line 37 of file CRobot2DPoseEstimator.cpp.

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

Here is the caller graph for this function:

Member Data Documentation

◆ m_cs

std::mutex mrpt::poses::CRobot2DPoseEstimator::m_cs

private

Definition at line 98 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 102 of file CRobot2DPoseEstimator.h.

◆ m_last_loc_time

std::optional<mrpt::Clock::time_point> mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time

private

Definition at line 100 of file CRobot2DPoseEstimator.h.

◆ m_last_odo

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

private

Definition at line 109 of file CRobot2DPoseEstimator.h.

◆ m_last_odo_time

std::optional<mrpt::Clock::time_point> mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time

private

Definition at line 108 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 106 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 111 of file CRobot2DPoseEstimator.h.

◆ params

TOptions mrpt::poses::CRobot2DPoseEstimator::params

parameters of the filter.

Definition at line 88 of file CRobot2DPoseEstimator.h.

Referenced by TEST().

Detailed Description

Classes

Public Member Functions

Static Public Member Functions

Public Attributes

Private Attributes

Constructor & Destructor Documentation

◆ CRobot2DPoseEstimator()

◆ ~CRobot2DPoseEstimator()

Member Function Documentation

◆ extrapolateRobotPose()

◆ getCurrentEstimate()

◆ getLatestRobotPose() [1/2]

◆ getLatestRobotPose() [2/2]

◆ processUpdateNewOdometry()

◆ processUpdateNewPoseLocalization()

◆ reset()

Member Data Documentation

◆ m_cs

◆ m_last_loc

◆ m_last_loc_time

◆ m_last_odo

◆ m_last_odo_time

◆ m_loc_odo_ref

◆ m_robot_vel_local

◆ params