Detailed Description

template<int DIM>
class mrpt::poses::CPoseInterpolatorBase< DIM >

Base class for SE(2)/SE(3) interpolators.

See docs for derived classes.

Definition at line 47 of file CPoseInterpolatorBase.h.

#include <mrpt/poses/CPoseInterpolatorBase.h>

Inheritance diagram for mrpt::poses::CPoseInterpolatorBase< DIM >:

Public Member Functions
	CPoseInterpolatorBase ()
	Default ctor: empty sequence of poses. More...

void	insert (mrpt::system::TTimeStamp t, const pose_t &p)
	Inserts a new pose in the sequence. More...

void	insert (mrpt::system::TTimeStamp t, const cpose_t &p)
	Overload (slower) More...

pose_t &	interpolate (mrpt::system::TTimeStamp t, pose_t &out_interp, bool &out_valid_interp) const
	Returns the pose at a given time, or interpolates using splines if there is not an exact match. More...

cpose_t &	interpolate (mrpt::system::TTimeStamp t, cpose_t &out_interp, bool &out_valid_interp) const

void	clear ()
	Clears the current sequence of poses. More...

void	setMaxTimeInterpolation (double time)
	Set value of the maximum time to consider interpolation. More...

double	getMaxTimeInterpolation ()
	Set value of the maximum time to consider interpolation. More...

bool	getPreviousPoseWithMinDistance (const mrpt::system::TTimeStamp &t, double distance, pose_t &out_pose)
	Get the previous CPose3D in the map with a minimum defined distance. More...

bool	getPreviousPoseWithMinDistance (const mrpt::system::TTimeStamp &t, double distance, cpose_t &out_pose)

bool	saveToTextFile (const std::string &s) const
	Saves the points in the interpolator to a text file, with this format: Each row contains these elements separated by spaces: More...

bool	saveInterpolatedToTextFile (const std::string &s, double period) const
	Saves the points in the interpolator to a text file, with the same format that saveToTextFile, but interpolating the path with the given period in seconds. More...

bool	loadFromTextFile (const std::string &s)
	Loads from a text file, in the format described by saveToTextFile. More...

void	getBoundingBox (point_t &minCorner, point_t &maxCorner) const
	Computes the bounding box in all Euclidean coordinates of the whole path. More...

void	setInterpolationMethod (TInterpolatorMethod method)
	Change the method used to interpolate the robot path. The default method at construction is "imSpline". More...

TInterpolatorMethod	getInterpolationMethod () const
	Returns the currently set interpolation method. More...

void	filter (unsigned int component, unsigned int samples)
	Filters by averaging one of the components of the pose data within the interpolator. More...

Protected Member Functions
void	impl_interpolation (const mrpt::math::CArrayDouble< 4 > &ts, const TTimePosePair p1, const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4, const TInterpolatorMethod method, double td, pose_t &out_interp) const

template<>
void	impl_interpolation (const mrpt::math::CArrayDouble< 4 > &ts, const TTimePosePair p1, const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4, const TInterpolatorMethod method, double td, pose_t &out_interp) const

template<>
void	impl_interpolation (const mrpt::math::CArrayDouble< 4 > &ts, const TTimePosePair p1, const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4, const TInterpolatorMethod method, double td, pose_t &out_interp) const

Protected Attributes
TPath	m_path
	The sequence of poses. More...

double	maxTimeInterpolation
	Maximum time considered to interpolate. If the difference between the desired timestamp where to interpolate and the next timestamp stored in the map is bigger than this value, the interpolation will not be done. More...

TInterpolatorMethod	m_method

Type definitions and STL-like container interface
typedef mrpt::poses::SE_traits< DIM >::lightweight_pose_t	pose_t
	TPose2D or TPose3D. More...

typedef mrpt::poses::SE_traits< DIM >::pose_t	cpose_t
	CPose2D or CPose3D. More...

typedef mrpt::poses::SE_traits< DIM >::point_t	point_t
	TPoint2D or TPoint3D. More...

typedef std::pair< mrpt::system::TTimeStamp, pose_t >	TTimePosePair

typedef std::map< mrpt::system::TTimeStamp, pose_t >	TPath

typedef TPath::iterator	iterator

typedef TPath::const_iterator	const_iterator

typedef TPath::reverse_iterator	reverse_iterator

typedef TPath::const_reverse_iterator	const_reverse_iterator

iterator	begin ()

const_iterator	begin () const

const_iterator	cbegin () const

iterator	end ()

const_iterator	end () const

const_iterator	cend () const

reverse_iterator	rbegin ()

const_reverse_iterator	rbegin () const

reverse_iterator	rend ()

const_reverse_iterator	rend () const

iterator	lower_bound (const mrpt::system::TTimeStamp &t)

const_iterator	lower_bound (const mrpt::system::TTimeStamp &t) const

iterator	upper_bound (const mrpt::system::TTimeStamp &t)

const_iterator	upper_bound (const mrpt::system::TTimeStamp &t) const

iterator	erase (iterator element_to_erase)

size_t	size () const

bool	empty () const

iterator	find (const mrpt::system::TTimeStamp &t)

const_iterator	find (const mrpt::system::TTimeStamp &t) const

Member Typedef Documentation

◆ const_iterator

template<int DIM>

typedef TPath::const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::const_iterator

Definition at line 62 of file CPoseInterpolatorBase.h.

◆ const_reverse_iterator

template<int DIM>

typedef TPath::const_reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::const_reverse_iterator

Definition at line 64 of file CPoseInterpolatorBase.h.

◆ cpose_t

template<int DIM>

typedef mrpt::poses::SE_traits<DIM>::pose_t mrpt::poses::CPoseInterpolatorBase< DIM >::cpose_t

CPose2D or CPose3D.

Definition at line 56 of file CPoseInterpolatorBase.h.

◆ iterator

template<int DIM>

typedef TPath::iterator mrpt::poses::CPoseInterpolatorBase< DIM >::iterator

Definition at line 61 of file CPoseInterpolatorBase.h.

◆ point_t

template<int DIM>

typedef mrpt::poses::SE_traits<DIM>::point_t mrpt::poses::CPoseInterpolatorBase< DIM >::point_t

TPoint2D or TPoint3D.

Definition at line 57 of file CPoseInterpolatorBase.h.

◆ pose_t

template<int DIM>

typedef mrpt::poses::SE_traits<DIM>::lightweight_pose_t mrpt::poses::CPoseInterpolatorBase< DIM >::pose_t

TPose2D or TPose3D.

Definition at line 55 of file CPoseInterpolatorBase.h.

◆ reverse_iterator

template<int DIM>

typedef TPath::reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::reverse_iterator

Definition at line 63 of file CPoseInterpolatorBase.h.

◆ TPath

template<int DIM>

typedef std::map<mrpt::system::TTimeStamp,pose_t> mrpt::poses::CPoseInterpolatorBase< DIM >::TPath

Definition at line 60 of file CPoseInterpolatorBase.h.

◆ TTimePosePair

template<int DIM>

typedef std::pair<mrpt::system::TTimeStamp,pose_t> mrpt::poses::CPoseInterpolatorBase< DIM >::TTimePosePair

Definition at line 59 of file CPoseInterpolatorBase.h.

Constructor & Destructor Documentation

◆ CPoseInterpolatorBase()

template<int DIM>

mrpt::poses::CPoseInterpolatorBase< DIM >::CPoseInterpolatorBase ( )

Default ctor: empty sequence of poses.

Definition at line 24 of file CPoseInterpolatorBase.hpp.

Member Function Documentation

◆ begin() [1/2]

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::begin ( )

inline

Definition at line 66 of file CPoseInterpolatorBase.h.

Referenced by mrpt::topography::path_from_rtk_gps(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ begin() [2/2]

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::begin ( ) const

inline

Definition at line 67 of file CPoseInterpolatorBase.h.

◆ cbegin()

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::cbegin ( ) const

inline

Definition at line 68 of file CPoseInterpolatorBase.h.

◆ cend()

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::cend ( ) const

inline

Definition at line 78 of file CPoseInterpolatorBase.h.

◆ clear()

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::clear ( )

Clears the current sequence of poses.

Definition at line 30 of file CPoseInterpolatorBase.hpp.

Referenced by mrpt::nav::CAbstractNavigator::internal_onStartNewNavigation(), mrpt::topography::path_from_rtk_gps(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ empty()

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::empty ( ) const

inline

Definition at line 101 of file CPoseInterpolatorBase.h.

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating().

◆ end() [1/2]

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::end ( )

inline

Definition at line 76 of file CPoseInterpolatorBase.h.

Referenced by mrpt::topography::path_from_rtk_gps().

◆ end() [2/2]

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::end ( ) const

inline

Definition at line 77 of file CPoseInterpolatorBase.h.

◆ erase()

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::erase ( iterator element_to_erase )

inline

Definition at line 98 of file CPoseInterpolatorBase.h.

Referenced by mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ filter()

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::filter	(	unsigned int	component,
		unsigned int	samples
	)

Filters by averaging one of the components of the pose data within the interpolator.

The width of the filter is set by the number of samples.

Parameters

component	[IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll)
samples	[IN] The width of the average filter.

Definition at line 356 of file CPoseInterpolatorBase.hpp.

◆ find() [1/2]

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::find ( const mrpt::system::TTimeStamp & t )

inline

Definition at line 103 of file CPoseInterpolatorBase.h.

Referenced by mrpt::poses::CPoseInterpolatorBase< 3 >::getPreviousPoseWithMinDistance().

◆ find() [2/2]

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::find ( const mrpt::system::TTimeStamp & t ) const

inline

Definition at line 104 of file CPoseInterpolatorBase.h.

◆ getBoundingBox()

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::getBoundingBox	(	point_t &	minCorner,
		point_t &	maxCorner
	)		const

Computes the bounding box in all Euclidean coordinates of the whole path.

Exceptions

std::exception On empty path

Definition at line 323 of file CPoseInterpolatorBase.hpp.

◆ getInterpolationMethod()

template<int DIM>

TInterpolatorMethod mrpt::poses::CPoseInterpolatorBase< DIM >::getInterpolationMethod ( ) const

Returns the currently set interpolation method.

See also: setInterpolationMethod()

Definition at line 350 of file CPoseInterpolatorBase.hpp.

◆ getMaxTimeInterpolation()

template<int DIM>

double mrpt::poses::CPoseInterpolatorBase< DIM >::getMaxTimeInterpolation ( )

Set value of the maximum time to consider interpolation.

Definition at line 219 of file CPoseInterpolatorBase.hpp.

◆ getPreviousPoseWithMinDistance() [1/2]

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::getPreviousPoseWithMinDistance	(	const mrpt::system::TTimeStamp &	t,
		double	distance,
		pose_t &	out_pose
	)

Get the previous CPose3D in the map with a minimum defined distance.

Returns: true if pose was found, false otherwise

Definition at line 181 of file CPoseInterpolatorBase.hpp.

◆ getPreviousPoseWithMinDistance() [2/2]

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::getPreviousPoseWithMinDistance	(	const mrpt::system::TTimeStamp &	t,
		double	distance,
		cpose_t &	out_pose
	)

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 172 of file CPoseInterpolatorBase.hpp.

◆ impl_interpolation() [1/3]

template<>

void mrpt::poses::CPoseInterpolatorBase< 2 >::impl_interpolation	(	const mrpt::math::CArrayDouble< 4 > &	ts,
		const TTimePosePair	p1,
		const TTimePosePair	p2,
		const TTimePosePair	p3,
		const TTimePosePair	p4,
		const TInterpolatorMethod	method,
		double	td,
		pose_t &	out_interp
	)		const

protected

Definition at line 50 of file CPose2DInterpolator.cpp.

◆ impl_interpolation() [2/3]

template<>

void mrpt::poses::CPoseInterpolatorBase< 3 >::impl_interpolation	(	const mrpt::math::CArrayDouble< 4 > &	ts,
		const TTimePosePair	p1,
		const TTimePosePair	p2,
		const TTimePosePair	p3,
		const TTimePosePair	p4,
		const TInterpolatorMethod	method,
		double	td,
		pose_t &	out_interp
	)		const

protected

Definition at line 60 of file CPose3DInterpolator.cpp.

◆ impl_interpolation() [3/3]

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::impl_interpolation	(	const mrpt::math::CArrayDouble< 4 > &	ts,
		const TTimePosePair	p1,
		const TTimePosePair	p2,
		const TTimePosePair	p3,
		const TTimePosePair	p4,
		const TInterpolatorMethod	method,
		double	td,
		pose_t &	out_interp
	)		const

protected

◆ insert() [1/2]

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::insert	(	mrpt::system::TTimeStamp	t,
		const pose_t &	p
	)

Inserts a new pose in the sequence.

It overwrites any previously existing pose at exactly the same time.

Definition at line 41 of file CPoseInterpolatorBase.hpp.

Referenced by mrpt::topography::path_from_rtk_gps(), TEST(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ insert() [2/2]

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::insert	(	mrpt::system::TTimeStamp	t,
		const cpose_t &	p
	)

Overload (slower)

Definition at line 36 of file CPoseInterpolatorBase.hpp.

◆ interpolate() [1/2]

template<int DIM>

CPoseInterpolatorBase< DIM >::pose_t & mrpt::poses::CPoseInterpolatorBase< DIM >::interpolate	(	mrpt::system::TTimeStamp	t,
		pose_t &	out_interp,
		bool &	out_valid_interp
	)		const

Returns the pose at a given time, or interpolates using splines if there is not an exact match.

Parameters

t	The time of the point to interpolate.
out_interp	The output interpolated pose.
out_valid_interp	Whether there was information enough to compute the interpolation.

Returns: A reference to out_interp

Definition at line 59 of file CPoseInterpolatorBase.hpp.

Referenced by mrpt::obs::CObservationVelodyneScan::generatePointCloudAlongSE3Trajectory(), mrpt::nav::CAbstractPTGBasedReactive::performNavigationStep(), and TEST().

◆ interpolate() [2/2]

template<int DIM>

CPoseInterpolatorBase< DIM >::cpose_t & mrpt::poses::CPoseInterpolatorBase< DIM >::interpolate	(	mrpt::system::TTimeStamp	t,
		cpose_t &	out_interp,
		bool &	out_valid_interp
	)		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 50 of file CPoseInterpolatorBase.hpp.

◆ loadFromTextFile()

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::loadFromTextFile ( const std::string & s )

Loads from a text file, in the format described by saveToTextFile.

Returns: true on success, false on any error.

Exceptions

std::exception On invalid file format

Definition at line 289 of file CPoseInterpolatorBase.hpp.

◆ lower_bound() [1/2]

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::lower_bound ( const mrpt::system::TTimeStamp & t )

inline

Definition at line 92 of file CPoseInterpolatorBase.h.

◆ lower_bound() [2/2]

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::lower_bound ( const mrpt::system::TTimeStamp & t ) const

inline

Definition at line 93 of file CPoseInterpolatorBase.h.

◆ rbegin() [1/2]

template<int DIM>

reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin ( )

inline

Definition at line 86 of file CPoseInterpolatorBase.h.

Referenced by mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ rbegin() [2/2]

template<int DIM>

const_reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin ( ) const

inline

Definition at line 87 of file CPoseInterpolatorBase.h.

◆ rend() [1/2]

template<int DIM>

reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rend ( )

inline

Definition at line 89 of file CPoseInterpolatorBase.h.

◆ rend() [2/2]

template<int DIM>

const_reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rend ( ) const

inline

Definition at line 90 of file CPoseInterpolatorBase.h.

◆ saveInterpolatedToTextFile()

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::saveInterpolatedToTextFile	(	const std::string &	s,
		double	period
	)		const

Saves the points in the interpolator to a text file, with the same format that saveToTextFile, but interpolating the path with the given period in seconds.

See also: loadFromTextFile

Returns: true on success, false on any error.

Definition at line 252 of file CPoseInterpolatorBase.hpp.

◆ saveToTextFile()

template<int DIM>

bool mrpt::poses::CPoseInterpolatorBase< DIM >::saveToTextFile ( const std::string & s ) const

Saves the points in the interpolator to a text file, with this format: Each row contains these elements separated by spaces:

Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
x y z: The 3D position in meters.
yaw pitch roll: The angles, in radians
See also
loadFromTextFile

Returns
true on success, false on any error.

Definition at line 225 of file CPoseInterpolatorBase.hpp.

◆ setInterpolationMethod()

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::setInterpolationMethod ( TInterpolatorMethod method )

Change the method used to interpolate the robot path. The default method at construction is "imSpline".

See also: getInterpolationMethod()

Definition at line 344 of file CPoseInterpolatorBase.hpp.

Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), and mrpt::topography::path_from_rtk_gps().

◆ setMaxTimeInterpolation()

template<int DIM>

void mrpt::poses::CPoseInterpolatorBase< DIM >::setMaxTimeInterpolation ( double time )

Set value of the maximum time to consider interpolation.

If set to a negative value, the check is disabled (default behavior).

Definition at line 212 of file CPoseInterpolatorBase.hpp.

Referenced by mrpt::topography::path_from_rtk_gps().

◆ size()

template<int DIM>

size_t mrpt::poses::CPoseInterpolatorBase< DIM >::size ( ) const

inline

Definition at line 100 of file CPoseInterpolatorBase.h.

Referenced by mrpt::topography::path_from_rtk_gps(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), TEST(), and mrpt::nav::CAbstractNavigator::updateCurrentPoseAndSpeeds().

◆ upper_bound() [1/2]

template<int DIM>

iterator mrpt::poses::CPoseInterpolatorBase< DIM >::upper_bound ( const mrpt::system::TTimeStamp & t )

inline

Definition at line 95 of file CPoseInterpolatorBase.h.

◆ upper_bound() [2/2]

template<int DIM>

const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::upper_bound ( const mrpt::system::TTimeStamp & t ) const

inline

Definition at line 96 of file CPoseInterpolatorBase.h.

Member Data Documentation

◆ m_method

template<int DIM>

TInterpolatorMethod mrpt::poses::CPoseInterpolatorBase< DIM >::m_method

protected

Definition at line 172 of file CPoseInterpolatorBase.h.

◆ m_path

template<int DIM>

TPath mrpt::poses::CPoseInterpolatorBase< DIM >::m_path

protected

The sequence of poses.

Definition at line 170 of file CPoseInterpolatorBase.h.

◆ maxTimeInterpolation

template<int DIM>

double mrpt::poses::CPoseInterpolatorBase< DIM >::maxTimeInterpolation

protected

Maximum time considered to interpolate. If the difference between the desired timestamp where to interpolate and the next timestamp stored in the map is bigger than this value, the interpolation will not be done.

Definition at line 171 of file CPoseInterpolatorBase.h.

Detailed Description

template<int DIM> class mrpt::poses::CPoseInterpolatorBase< DIM >

Public Member Functions

Protected Member Functions

Protected Attributes

Type definitions and STL-like container interface

Member Typedef Documentation

◆ const_iterator

◆ const_reverse_iterator

◆ cpose_t

◆ iterator

◆ point_t

◆ pose_t

◆ reverse_iterator

◆ TPath

◆ TTimePosePair

Constructor & Destructor Documentation

◆ CPoseInterpolatorBase()

Member Function Documentation

◆ begin() [1/2]

◆ begin() [2/2]

◆ cbegin()

◆ cend()

◆ clear()

◆ empty()

◆ end() [1/2]

◆ end() [2/2]

◆ erase()

◆ filter()

◆ find() [1/2]

◆ find() [2/2]

◆ getBoundingBox()

◆ getInterpolationMethod()

◆ getMaxTimeInterpolation()

◆ getPreviousPoseWithMinDistance() [1/2]

◆ getPreviousPoseWithMinDistance() [2/2]

◆ impl_interpolation() [1/3]

◆ impl_interpolation() [2/3]

◆ impl_interpolation() [3/3]

◆ insert() [1/2]

◆ insert() [2/2]

◆ interpolate() [1/2]

◆ interpolate() [2/2]

◆ loadFromTextFile()

◆ lower_bound() [1/2]

◆ lower_bound() [2/2]

◆ rbegin() [1/2]

◆ rbegin() [2/2]

◆ rend() [1/2]

◆ rend() [2/2]

◆ saveInterpolatedToTextFile()

◆ saveToTextFile()

◆ setInterpolationMethod()

◆ setMaxTimeInterpolation()

◆ size()

◆ upper_bound() [1/2]

◆ upper_bound() [2/2]

Member Data Documentation

◆ m_method

◆ m_path

◆ maxTimeInterpolation

template<int DIM>
class mrpt::poses::CPoseInterpolatorBase< DIM >