This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
It can also interpolate SE(3) poses over time using linear, splines or SLERP interpolation, as set in CPose3DInterpolator::setInterpolationMethod() Usage:
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".
Definition at line 56 of file CPose3DInterpolator.h.
#include <mrpt/poses/CPose3DInterpolator.h>
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 | |
void * | operator new (size_t size) |
void * | operator new[] (size_t size) |
void | operator delete (void *ptr) throw () |
void | operator delete[] (void *ptr) throw () |
void | operator delete (void *memory, void *ptr) throw () |
void * | operator new (size_t size, const std::nothrow_t &) throw () |
void | operator delete (void *ptr, const std::nothrow_t &) throw () |
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... | |
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. 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... | |
virtual mxArray * | writeToMatlab () const |
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. 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 Member Functions | |
static void * | operator new (size_t size, void *ptr) |
Static Public Attributes | |
static const mrpt::utils::TRuntimeClassId | classCObject |
RTTI stuff <br> | |
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 mrpt::aligned_containers< mrpt::system::TTimeStamp, CPose3D >::map_t | TPath |
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 <br> | |
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. More... | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. More... | |
static mrpt::utils::CObject * | CreateObject () |
static CPose3DInterpolatorPtr | Create () |
typedef TPath::const_iterator mrpt::poses::CPose3DInterpolator::const_iterator |
Definition at line 67 of file CPose3DInterpolator.h.
typedef TPath::const_reverse_iterator mrpt::poses::CPose3DInterpolator::const_reverse_iterator |
Definition at line 69 of file CPose3DInterpolator.h.
typedef TPath::iterator mrpt::poses::CPose3DInterpolator::iterator |
Definition at line 66 of file CPose3DInterpolator.h.
typedef TPath::reverse_iterator mrpt::poses::CPose3DInterpolator::reverse_iterator |
Definition at line 68 of file CPose3DInterpolator.h.
typedef CPose3DInterpolatorPtr mrpt::poses::CPose3DInterpolator::SmartPtr |
A typedef for the associated smart pointer
Definition at line 59 of file CPose3DInterpolator.h.
|
private |
Definition at line 62 of file CPose3DInterpolator.h.
Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod.
Enumerator | |
---|---|
imSpline | |
imLinear2Neig | |
imLinear4Neig | |
imSSLLLL | |
imSSLSLL | |
imLinearSlerp | |
imSplineSlerp |
Definition at line 80 of file CPose3DInterpolator.h.
mrpt::poses::CPose3DInterpolator::CPose3DInterpolator | ( | ) |
Creates an empty interpolator (with no points).
|
staticprotected |
|
inline |
Definition at line 91 of file CPose3DInterpolator.h.
|
inline |
Definition at line 92 of file CPose3DInterpolator.h.
void mrpt::poses::CPose3DInterpolator::clear | ( | ) |
Clears the current sequence of poses.
|
inlineinherited |
|
static |
|
static |
|
virtual |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
|
inlineinherited |
|
inline |
Definition at line 112 of file CPose3DInterpolator.h.
|
inline |
Definition at line 94 of file CPose3DInterpolator.h.
|
inline |
Definition at line 95 of file CPose3DInterpolator.h.
Definition at line 109 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.
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.
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.
std::exception | On empty path |
TInterpolatorMethod mrpt::poses::CPose3DInterpolator::getInterpolationMethod | ( | ) | const |
Returns the currently set interpolation method.
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.
|
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.
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. |
bool mrpt::poses::CPose3DInterpolator::loadFromTextFile | ( | const std::string & | s | ) |
Loads from a text file, in the format described by saveToTextFile.
std::exception | On invalid file format |
|
inline |
Definition at line 103 of file CPose3DInterpolator.h.
References t().
|
inline |
Definition at line 104 of file CPose3DInterpolator.h.
References t().
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inlinestatic |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 59 of file CPose3DInterpolator.h.
|
inline |
Definition at line 97 of file CPose3DInterpolator.h.
|
inline |
Definition at line 98 of file CPose3DInterpolator.h.
|
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.
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. |
std::exception | On any error, see CStream::ReadBuffer |
Implements mrpt::utils::CSerializable.
|
inline |
Definition at line 100 of file CPose3DInterpolator.h.
|
inline |
Definition at line 101 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.
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:
void mrpt::poses::CPose3DInterpolator::setInterpolationMethod | ( | TInterpolatorMethod | method | ) |
Change the method used to interpolate the robot path.
The default method at construction is "imSpline".
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).
|
inline |
Definition at line 111 of file CPose3DInterpolator.h.
|
inline |
Definition at line 106 of file CPose3DInterpolator.h.
References t().
|
inline |
Definition at line 107 of file CPose3DInterpolator.h.
References t().
|
inlinevirtualinherited |
Introduces a pure virtual method responsible for writing to a mxArray
Matlab object, typically a MATLAB struct
whose contents are documented in each derived class.
mxArray
(caller is responsible of memory freeing) or NULL is class does not support conversion to MATLAB. Definition at line 79 of file CSerializable.h.
|
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.
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. |
std::exception | On any error, see CStream::WriteBuffer |
Implements mrpt::utils::CSerializable.
|
staticprotected |
Definition at line 59 of file CPose3DInterpolator.h.
|
staticinherited |
|
static |
Definition at line 59 of file CPose3DInterpolator.h.
|
staticinherited |
Definition at line 42 of file CSerializable.h.
|
static |
Definition at line 59 of file CPose3DInterpolator.h.
|
private |
Definition at line 200 of file CPose3DInterpolator.h.
|
private |
The sequence of poses.
Definition at line 63 of file CPose3DInterpolator.h.
|
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 198 of file CPose3DInterpolator.h.
Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:34:44 UTC 2022 |