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".
Definition at line 70 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
| |
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. | |
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 () |
Definition at line 81 of file CPose3DInterpolator.h.
typedef TPath::const_reverse_iterator mrpt::poses::CPose3DInterpolator::const_reverse_iterator |
Definition at line 83 of file CPose3DInterpolator.h.
Definition at line 80 of file CPose3DInterpolator.h.
typedef TPath::reverse_iterator mrpt::poses::CPose3DInterpolator::reverse_iterator |
Definition at line 82 of file CPose3DInterpolator.h.
A typedef for the associated smart pointer
Definition at line 73 of file CPose3DInterpolator.h.
typedef std::map< mrpt::system::TTimeStamp, CPose3D > mrpt::poses::CPose3DInterpolator::TPath [private] |
Definition at line 76 of file CPose3DInterpolator.h.
Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod
Definition at line 94 of file CPose3DInterpolator.h.
mrpt::poses::CPose3DInterpolator::CPose3DInterpolator | ( | ) |
Creates an empty interpolator (with no points).
static const mrpt::utils::TRuntimeClassId* mrpt::poses::CPose3DInterpolator::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::utils::CSerializable.
iterator mrpt::poses::CPose3DInterpolator::begin | ( | ) | [inline] |
Definition at line 105 of file CPose3DInterpolator.h.
const_iterator mrpt::poses::CPose3DInterpolator::begin | ( | ) | const [inline] |
Definition at line 106 of file CPose3DInterpolator.h.
void mrpt::poses::CPose3DInterpolator::clear | ( | ) |
Clears the current sequence of poses.
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.
bool mrpt::poses::CPose3DInterpolator::empty | ( | ) | const [inline] |
Definition at line 126 of file CPose3DInterpolator.h.
iterator mrpt::poses::CPose3DInterpolator::end | ( | ) | [inline] |
Definition at line 108 of file CPose3DInterpolator.h.
const_iterator mrpt::poses::CPose3DInterpolator::end | ( | ) | const [inline] |
Definition at line 109 of file CPose3DInterpolator.h.
Definition at line 123 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 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.
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 |
iterator mrpt::poses::CPose3DInterpolator::lower_bound | ( | const mrpt::system::TTimeStamp & | t ) | [inline] |
Definition at line 117 of file CPose3DInterpolator.h.
const_iterator mrpt::poses::CPose3DInterpolator::lower_bound | ( | const mrpt::system::TTimeStamp & | t ) | const [inline] |
Definition at line 118 of file CPose3DInterpolator.h.
const_reverse_iterator mrpt::poses::CPose3DInterpolator::rbegin | ( | ) | const [inline] |
Definition at line 112 of file CPose3DInterpolator.h.
reverse_iterator mrpt::poses::CPose3DInterpolator::rbegin | ( | ) | [inline] |
Definition at line 111 of file CPose3DInterpolator.h.
const_reverse_iterator mrpt::poses::CPose3DInterpolator::rend | ( | ) | const [inline] |
Definition at line 115 of file CPose3DInterpolator.h.
reverse_iterator mrpt::poses::CPose3DInterpolator::rend | ( | ) | [inline] |
Definition at line 114 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).
size_t mrpt::poses::CPose3DInterpolator::size | ( | ) | const [inline] |
Definition at line 125 of file CPose3DInterpolator.h.
iterator mrpt::poses::CPose3DInterpolator::upper_bound | ( | const mrpt::system::TTimeStamp & | t ) | [inline] |
Definition at line 120 of file CPose3DInterpolator.h.
const_iterator mrpt::poses::CPose3DInterpolator::upper_bound | ( | const mrpt::system::TTimeStamp & | t ) | const [inline] |
Definition at line 121 of file CPose3DInterpolator.h.
mrpt::utils::CLASSINIT mrpt::poses::CPose3DInterpolator::_init_CPose3DInterpolator [static, protected] |
Definition at line 73 of file CPose3DInterpolator.h.
Definition at line 73 of file CPose3DInterpolator.h.
Definition at line 73 of file CPose3DInterpolator.h.
Definition at line 214 of file CPose3DInterpolator.h.
The sequence of poses.
Definition at line 77 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 212 of file CPose3DInterpolator.h.
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |