Detailed Description

A trajectory in time and in 6D (CPose3D) that interpolates using splines the intervals between a set of given time-referenced poses.

To insert new points into the sequence, use the "insert" method, and for getting an interpolated point, use "interpolate" method. For example:

CPose3DInterpolator            path;
path.setInterpolationMethod( CPose3DInterpolator::imSplineSlerp );
path.insert( t0, CPose3D(...) );
path.insert( t1, CPose3D(...) );
path.insert( t2, CPose3D(...) );
path.insert( t3, CPose3D(...) );
CPose3D p;
bool valid;
cout << "Pose at t: " << path.interpolate(t,p,valid) << endl;

Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references.

See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is "imLinearSlerp".

See Also: CPoseOrPoint

Definition at line 71 of file CPose3DInterpolator.h.

#include <mrpt/poses/CPose3DInterpolator.h>

Inheritance diagram for mrpt::poses::CPose3DInterpolator:

[legend]

Public Types
enum	TInterpolatorMethod { imSpline = 0, imLinear2Neig, imLinear4Neig, imSSLLLL, imSSLSLL, imLinearSlerp, imSplineSlerp }
	Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod. More...

typedef TPath::iterator	iterator

typedef TPath::const_iterator	const_iterator

typedef TPath::reverse_iterator	reverse_iterator

typedef TPath::const_reverse_iterator	const_reverse_iterator

Public Member Functions
iterator	begin ()

const_iterator	begin () const

iterator	end ()

const_iterator	end () 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

	CPose3DInterpolator ()
	Creates an empty interpolator (with no points).

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

CPose3D &	interpolate (mrpt::system::TTimeStamp t, CPose3D &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.

void	clear ()
	Clears the current sequence of poses.

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

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

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

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:

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.

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

void	getBoundingBox (CPoint3D &minCorner, CPoint3D &maxCorner) const
	Computes the bounding box in X,Y,Z of the whole vehicle path.

void	getBoundingBox (mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const
	Computes the bounding box in X,Y,Z of the whole vehicle path.

void	setInterpolationMethod (TInterpolatorMethod method)
	Change the method used to interpolate the robot path.

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

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

mrpt::utils::CObjectPtr	duplicateGetSmartPtr () const
	Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer).

CObject *	clone () const
	Cloning interface for smart pointers.

Static Public Attributes
static const mrpt::utils::TRuntimeClassId	classCObject

RTTI stuff
static const mrpt::utils::TRuntimeClassId	classCSerializable

Protected Member Functions
CSerializable virtual methods
void	writeToStream (mrpt::utils::CStream &out, int *getVersion) const
	Introduces a pure virtual method responsible for writing to a CStream.

void	readFromStream (mrpt::utils::CStream &in, int version)
	Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori.

Private Types
typedef std::map < mrpt::system::TTimeStamp, CPose3D >	TPath

Private Attributes
TPath	m_path
	The sequence of poses.

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.

TInterpolatorMethod	m_method

RTTI stuff
typedef CPose3DInterpolatorPtr	SmartPtr

static mrpt::utils::CLASSINIT	_init_CPose3DInterpolator

static mrpt::utils::TRuntimeClassId	classCPose3DInterpolator

static const mrpt::utils::TRuntimeClassId *	classinfo

static const mrpt::utils::TRuntimeClassId *	_GetBaseClass ()

virtual const mrpt::utils::TRuntimeClassId *	GetRuntimeClass () const
	Returns information about the class of an object in runtime.

virtual mrpt::utils::CObject *	duplicate () const
	Returns a copy of the object, indepently of its class.

static mrpt::utils::CObject *	CreateObject ()

static CPose3DInterpolatorPtr	Create ()

Member Typedef Documentation

typedef TPath::const_iterator mrpt::poses::CPose3DInterpolator::const_iterator

Definition at line 82 of file CPose3DInterpolator.h.

typedef TPath::const_reverse_iterator mrpt::poses::CPose3DInterpolator::const_reverse_iterator

Definition at line 84 of file CPose3DInterpolator.h.

typedef TPath::iterator mrpt::poses::CPose3DInterpolator::iterator

Definition at line 81 of file CPose3DInterpolator.h.

typedef TPath::reverse_iterator mrpt::poses::CPose3DInterpolator::reverse_iterator

Definition at line 83 of file CPose3DInterpolator.h.

typedef CPose3DInterpolatorPtr mrpt::poses::CPose3DInterpolator::SmartPtr

A typedef for the associated smart pointer

Definition at line 74 of file CPose3DInterpolator.h.

typedef std::map< mrpt::system::TTimeStamp, CPose3D > mrpt::poses::CPose3DInterpolator::TPath

private

Definition at line 77 of file CPose3DInterpolator.h.

Member Enumeration Documentation

enum mrpt::poses::CPose3DInterpolator::TInterpolatorMethod

Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod.

imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
imLinear2Neig: Linear interpolation between the previous and next neightbour.
imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.

Enumerator
imSpline
imLinear2Neig
imLinear4Neig
imSSLLLL
imSSLSLL
imLinearSlerp
imSplineSlerp

Definition at line 95 of file CPose3DInterpolator.h.

Constructor & Destructor Documentation

mrpt::poses::CPose3DInterpolator::CPose3DInterpolator ( )

Creates an empty interpolator (with no points).

Member Function Documentation

static const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3DInterpolator::_GetBaseClass ( )

staticprotected

iterator mrpt::poses::CPose3DInterpolator::begin ( )

inline

Definition at line 106 of file CPose3DInterpolator.h.

const_iterator mrpt::poses::CPose3DInterpolator::begin ( ) const

inline

Definition at line 107 of file CPose3DInterpolator.h.

void mrpt::poses::CPose3DInterpolator::clear ( )

Clears the current sequence of poses.

CObject* mrpt::utils::CObject::clone ( ) const

inlineinherited

Cloning interface for smart pointers.

Definition at line 154 of file CObject.h.

static CPose3DInterpolatorPtr mrpt::poses::CPose3DInterpolator::Create ( )

static

static mrpt::utils::CObject* mrpt::poses::CPose3DInterpolator::CreateObject ( )

static

virtual mrpt::utils::CObject* mrpt::poses::CPose3DInterpolator::duplicate ( ) const

virtual

Returns a copy of the object, indepently of its class.

Implements mrpt::utils::CObject.

mrpt::utils::CObjectPtr mrpt::utils::CObject::duplicateGetSmartPtr ( ) const

inlineinherited

Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer).

Definition at line 151 of file CObject.h.

bool mrpt::poses::CPose3DInterpolator::empty ( ) const

inline

Definition at line 127 of file CPose3DInterpolator.h.

iterator mrpt::poses::CPose3DInterpolator::end ( )

inline

Definition at line 109 of file CPose3DInterpolator.h.

const_iterator mrpt::poses::CPose3DInterpolator::end ( ) const

inline

Definition at line 110 of file CPose3DInterpolator.h.

iterator mrpt::poses::CPose3DInterpolator::erase ( iterator element_to_erase )

inline

Definition at line 124 of file CPose3DInterpolator.h.

void mrpt::poses::CPose3DInterpolator::filter	(	unsigned int	component,
		unsigned int	samples
	)

Filters by averaging one of the components of the CPose3D 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.

void mrpt::poses::CPose3DInterpolator::getBoundingBox	(	CPoint3D &	minCorner,
		CPoint3D &	maxCorner
	)		const

Computes the bounding box in X,Y,Z of the whole vehicle path.

Exceptions

std::exception On empty path

void mrpt::poses::CPose3DInterpolator::getBoundingBox	(	mrpt::math::TPoint3D &	minCorner,
		mrpt::math::TPoint3D &	maxCorner
	)		const

Computes the bounding box in X,Y,Z of the whole vehicle path.

Exceptions

std::exception On empty path

TInterpolatorMethod mrpt::poses::CPose3DInterpolator::getInterpolationMethod ( ) const

Returns the currently set interpolation method.

See Also: setInterpolationMethod

double mrpt::poses::CPose3DInterpolator::getMaxTimeInterpolation ( )

Set value of the maximum time to consider interpolation.

bool mrpt::poses::CPose3DInterpolator::getPreviousPoseWithMinDistance	(	const mrpt::system::TTimeStamp &	t,
		double	distance,
		CPose3D &	out_pose
	)

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

Returns: true if pose was found, false otherwise.

virtual const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3DInterpolator::GetRuntimeClass ( ) const

virtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::utils::CSerializable.

void mrpt::poses::CPose3DInterpolator::insert	(	mrpt::system::TTimeStamp	t,
		const CPose3D &	p
	)

Inserts a new pose in the sequence.

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

CPose3D& mrpt::poses::CPose3DInterpolator::interpolate	(	mrpt::system::TTimeStamp	t,
		CPose3D &	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

bool mrpt::poses::CPose3DInterpolator::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

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

inline

Definition at line 118 of file CPose3DInterpolator.h.

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

inline

Definition at line 119 of file CPose3DInterpolator.h.

reverse_iterator mrpt::poses::CPose3DInterpolator::rbegin ( )

inline

Definition at line 112 of file CPose3DInterpolator.h.

const_reverse_iterator mrpt::poses::CPose3DInterpolator::rbegin ( ) const

inline

Definition at line 113 of file CPose3DInterpolator.h.

void mrpt::poses::CPose3DInterpolator::readFromStream	(	mrpt::utils::CStream &	in,
		int	version
	)

protectedvirtual

Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori.

Parameters

in	The input binary stream where the object data must read from.
version	The version of the object stored in the stream: use this version number in your code to know how to read the incoming data.

Exceptions

std::exception On any error, see CStream::ReadBuffer

See Also: CStream

Implements mrpt::utils::CSerializable.

reverse_iterator mrpt::poses::CPose3DInterpolator::rend ( )

inline

Definition at line 115 of file CPose3DInterpolator.h.

const_reverse_iterator mrpt::poses::CPose3DInterpolator::rend ( ) const

inline

Definition at line 116 of file CPose3DInterpolator.h.

bool mrpt::poses::CPose3DInterpolator::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.

bool mrpt::poses::CPose3DInterpolator::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.

void mrpt::poses::CPose3DInterpolator::setInterpolationMethod ( TInterpolatorMethod method )

Change the method used to interpolate the robot path.

The default method at construction is "imSpline".

See Also: getInterpolationMethod

void mrpt::poses::CPose3DInterpolator::setMaxTimeInterpolation ( double time )

Set value of the maximum time to consider interpolation.

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

size_t mrpt::poses::CPose3DInterpolator::size ( ) const

inline

Definition at line 126 of file CPose3DInterpolator.h.

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

inline

Definition at line 121 of file CPose3DInterpolator.h.

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

inline

Definition at line 122 of file CPose3DInterpolator.h.

void mrpt::poses::CPose3DInterpolator::writeToStream	(	mrpt::utils::CStream &	out,
		int *	getVersion
	)		const

protectedvirtual

Introduces a pure virtual method responsible for writing to a CStream.

This can not be used directly be users, instead use "stream << object;" for writing it to a stream.

Parameters

out	The output binary stream where object must be dumped.
getVersion	If NULL, the object must be dumped. If not, only the version of the object dump must be returned in this pointer. This enables the versioning of objects dumping and backward compatibility with previously stored data.

Exceptions

std::exception On any error, see CStream::WriteBuffer

See Also: CStream

Implements mrpt::utils::CSerializable.

Member Data Documentation

mrpt::utils::CLASSINIT mrpt::poses::CPose3DInterpolator::_init_CPose3DInterpolator

staticprotected

Definition at line 74 of file CPose3DInterpolator.h.

const mrpt::utils::TRuntimeClassId mrpt::utils::CObject::classCObject

staticinherited

Definition at line 139 of file CObject.h.

mrpt::utils::TRuntimeClassId mrpt::poses::CPose3DInterpolator::classCPose3DInterpolator

static

Definition at line 74 of file CPose3DInterpolator.h.

const mrpt::utils::TRuntimeClassId mrpt::utils::CSerializable::classCSerializable

staticinherited

Definition at line 56 of file CSerializable.h.

const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3DInterpolator::classinfo

static

Definition at line 74 of file CPose3DInterpolator.h.

TInterpolatorMethod mrpt::poses::CPose3DInterpolator::m_method

private

Definition at line 215 of file CPose3DInterpolator.h.

TPath mrpt::poses::CPose3DInterpolator::m_path

private

The sequence of poses.

Definition at line 78 of file CPose3DInterpolator.h.

double mrpt::poses::CPose3DInterpolator::maxTimeInterpolation

private

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 213 of file CPose3DInterpolator.h.

Detailed Description

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Private Types

Private Attributes

RTTI stuff

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation