Main MRPT website > C++ reference
MRPT logo
List of all members | Public Types | Public Member Functions | Static Public Attributes | Private Types | Private Attributes
mrpt::poses::CPose3DInterpolator Class Reference

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:

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

#include <mrpt/poses/CPose3DInterpolator.h>

Inheritance diagram for mrpt::poses::CPose3DInterpolator:
Inheritance graph
[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). More...
 
void insert (mrpt::system::TTimeStamp t, const CPose3D &p)
 Inserts a new pose in the sequence. More...
 
CPose3Dinterpolate (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. More...
 
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, CPose3D &out_pose)
 Get the previous CPose3D in the map with a minimum defined distance. More...
 
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 (CPoint3D &minCorner, CPoint3D &maxCorner) const
 Computes the bounding box in X,Y,Z of the whole vehicle path. More...
 
void getBoundingBox (mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const
 Computes the bounding box in X,Y,Z of the whole vehicle path. More...
 
void setInterpolationMethod (TInterpolatorMethod method)
 Change the method used to interpolate the robot path. 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 CPose3D data within the interpolator. More...
 
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). More...
 
CObject * clone () const
 Cloning interface for smart pointers. More...
 

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. More...
 
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. More...
 

Private Types

typedef std::map< mrpt::system::TTimeStamp, CPose3DTPath
 

Private 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
 

RTTI stuff

typedef CPose3DInterpolatorPtr SmartPtr
 
static mrpt::utils::CLASSINIT _init_CPose3DInterpolator
 
static mrpt::utils::TRuntimeClassId classCPose3DInterpolator
 
static const mrpt::utils::TRuntimeClassIdclassinfo
 
static const mrpt::utils::TRuntimeClassId_GetBaseClass ()
 
virtual const mrpt::utils::TRuntimeClassIdGetRuntimeClass () const
 Returns information about the class of an object in runtime. More...
 
virtual mrpt::utils::CObjectduplicate () const
 Returns a copy of the object, indepently of its class. More...
 
static mrpt::utils::CObjectCreateObject ()
 
static CPose3DInterpolatorPtr Create ()
 

Member Typedef Documentation

◆ const_iterator

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

Definition at line 63 of file CPose3DInterpolator.h.

◆ const_reverse_iterator

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

Definition at line 65 of file CPose3DInterpolator.h.

◆ iterator

Definition at line 62 of file CPose3DInterpolator.h.

◆ reverse_iterator

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

Definition at line 64 of file CPose3DInterpolator.h.

◆ SmartPtr

A typedef for the associated smart pointer

Definition at line 55 of file CPose3DInterpolator.h.

◆ TPath

Definition at line 58 of file CPose3DInterpolator.h.

Member Enumeration Documentation

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

Constructor & Destructor Documentation

◆ CPose3DInterpolator()

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

Creates an empty interpolator (with no points).

Member Function Documentation

◆ _GetBaseClass()

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

◆ begin() [1/2]

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

Definition at line 87 of file CPose3DInterpolator.h.

◆ begin() [2/2]

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

Definition at line 88 of file CPose3DInterpolator.h.

◆ clear()

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

Clears the current sequence of poses.

◆ clone()

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

Cloning interface for smart pointers.

Definition at line 135 of file CObject.h.

◆ Create()

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

◆ CreateObject()

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

◆ duplicate()

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

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

Implements mrpt::utils::CObject.

◆ duplicateGetSmartPtr()

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 132 of file CObject.h.

◆ empty()

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

Definition at line 108 of file CPose3DInterpolator.h.

◆ end() [1/2]

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

Definition at line 90 of file CPose3DInterpolator.h.

◆ end() [2/2]

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

Definition at line 91 of file CPose3DInterpolator.h.

◆ erase()

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

Definition at line 105 of file CPose3DInterpolator.h.

◆ filter()

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.

◆ getBoundingBox() [1/2]

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::exceptionOn empty path

◆ getBoundingBox() [2/2]

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::exceptionOn empty path

◆ getInterpolationMethod()

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

Returns the currently set interpolation method.

See also
setInterpolationMethod

◆ getMaxTimeInterpolation()

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

Set value of the maximum time to consider interpolation.

◆ getPreviousPoseWithMinDistance()

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.

◆ GetRuntimeClass()

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.

◆ insert()

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.

◆ interpolate()

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
tThe time of the point to interpolate.
out_interpThe output interpolated pose.
out_valid_interpWhether there was information enough to compute the interpolation.
Returns
A reference to out_interp

◆ loadFromTextFile()

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::exceptionOn invalid file format

◆ lower_bound() [1/2]

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

Definition at line 99 of file CPose3DInterpolator.h.

References t().

◆ lower_bound() [2/2]

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

Definition at line 100 of file CPose3DInterpolator.h.

References t().

◆ rbegin() [1/2]

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

Definition at line 93 of file CPose3DInterpolator.h.

◆ rbegin() [2/2]

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

Definition at line 94 of file CPose3DInterpolator.h.

◆ readFromStream()

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
inThe input binary stream where the object data must read from.
versionThe 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::exceptionOn any error, see CStream::ReadBuffer
See also
CStream

Implements mrpt::utils::CSerializable.

◆ rend() [1/2]

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

Definition at line 96 of file CPose3DInterpolator.h.

◆ rend() [2/2]

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

Definition at line 97 of file CPose3DInterpolator.h.

◆ saveInterpolatedToTextFile()

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.

◆ saveToTextFile()

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:

◆ setInterpolationMethod()

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

◆ setMaxTimeInterpolation()

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

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

Definition at line 107 of file CPose3DInterpolator.h.

◆ upper_bound() [1/2]

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

Definition at line 102 of file CPose3DInterpolator.h.

References t().

◆ upper_bound() [2/2]

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

Definition at line 103 of file CPose3DInterpolator.h.

References t().

◆ writeToStream()

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
outThe output binary stream where object must be dumped.
getVersionIf 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::exceptionOn any error, see CStream::WriteBuffer
See also
CStream

Implements mrpt::utils::CSerializable.

Member Data Documentation

◆ _init_CPose3DInterpolator

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

Definition at line 55 of file CPose3DInterpolator.h.

◆ classCObject

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

Definition at line 120 of file CObject.h.

◆ classCPose3DInterpolator

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

Definition at line 55 of file CPose3DInterpolator.h.

◆ classCSerializable

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

Definition at line 35 of file CSerializable.h.

◆ classinfo

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

Definition at line 55 of file CPose3DInterpolator.h.

◆ m_method

TInterpolatorMethod mrpt::poses::CPose3DInterpolator::m_method
private

Definition at line 196 of file CPose3DInterpolator.h.

◆ m_path

TPath mrpt::poses::CPose3DInterpolator::m_path
private

The sequence of poses.

Definition at line 59 of file CPose3DInterpolator.h.

◆ maxTimeInterpolation

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




Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo