MRPT  1.9.9
List of all members | Public Member Functions | Protected Member Functions | Protected Attributes
mrpt::poses::CPoseInterpolatorBase< DIM > Class Template Reference

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 50 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 (const mrpt::Clock::time_point &t, const pose_t &p)
 Inserts a new pose in the sequence. More...
 
void insert (const mrpt::Clock::time_point &t, const cpose_t &p)
 Overload (slower) More...
 
pose_tinterpolate (const mrpt::Clock::time_point &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_tinterpolate (const mrpt::Clock::time_point &t, cpose_t &out_interp, bool &out_valid_interp) const
 
void clear ()
 Clears the current sequence of poses. More...
 
void setMaxTimeInterpolation (const mrpt::Clock::duration &time)
 Set value of the maximum time to consider interpolation. More...
 
mrpt::Clock::duration getMaxTimeInterpolation ()
 Set value of the maximum time to consider interpolation. More...
 
bool getPreviousPoseWithMinDistance (const mrpt::Clock::time_point &t, double distance, pose_t &out_pose)
 Get the previous CPose3D in the map with a minimum defined distance. More...
 
bool getPreviousPoseWithMinDistance (const mrpt::Clock::time_point &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, const mrpt::Clock::duration &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. 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 TTimePosePair &p1, const TTimePosePair &p2, const TTimePosePair &p3, const TTimePosePair &p4, const TInterpolatorMethod method, const mrpt::Clock::time_point &td, pose_t &out_interp) const
 
template<>
void impl_interpolation (const TTimePosePair &p1, const TTimePosePair &p2, const TTimePosePair &p3, const TTimePosePair &p4, const TInterpolatorMethod method, const mrpt::Clock::time_point &t, pose_t &out_interp) const
 
template<>
void impl_interpolation (const TTimePosePair &p1, const TTimePosePair &p2, const TTimePosePair &p3, const TTimePosePair &p4, const TInterpolatorMethod method, const mrpt::Clock::time_point &t, pose_t &out_interp) const
 

Protected Attributes

TPath m_path
 The sequence of poses. More...
 
mrpt::Clock::duration maxTimeInterpolation
 Maximum time considered to interpolate. More...
 
TInterpolatorMethod m_method {mrpt::poses::imLinearSlerp}
 

Type definitions and STL-like container interface

using pose_t = typename Lie::SE< DIM >::light_type
 TPose2D or TPose3D. More...
 
using cpose_t = typename Lie::SE< DIM >::type
 CPose2D or CPose3D. More...
 
using point_t = typename Lie::Euclidean< DIM >::light_type
 TPoint2D or TPoint3D. More...
 
using TTimePosePair = std::pair< mrpt::Clock::time_point, pose_t >
 
using TPath = std::map< mrpt::Clock::time_point, pose_t >
 
using iterator = typename TPath::iterator
 
using const_iterator = typename TPath::const_iterator
 
using reverse_iterator = typename TPath::reverse_iterator
 
using const_reverse_iterator = typename TPath::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::Clock::time_point &t)
 
const_iterator lower_bound (const mrpt::Clock::time_point &t) const
 
iterator upper_bound (const mrpt::Clock::time_point &t)
 
const_iterator upper_bound (const mrpt::Clock::time_point &t) const
 
iterator erase (iterator element_to_erase)
 
size_t size () const
 
bool empty () const
 
iterator find (const mrpt::Clock::time_point &t)
 
const_iterator find (const mrpt::Clock::time_point &t) const
 
pose_tat (const mrpt::Clock::time_point &t)
 
const pose_tat (const mrpt::Clock::time_point &t) const
 

Member Typedef Documentation

◆ const_iterator

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::const_iterator = typename TPath::const_iterator

Definition at line 69 of file CPoseInterpolatorBase.h.

◆ const_reverse_iterator

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::const_reverse_iterator = typename TPath::const_reverse_iterator

Definition at line 71 of file CPoseInterpolatorBase.h.

◆ cpose_t

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::cpose_t = typename Lie::SE<DIM>::type

CPose2D or CPose3D.

Definition at line 62 of file CPoseInterpolatorBase.h.

◆ iterator

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::iterator = typename TPath::iterator

Definition at line 68 of file CPoseInterpolatorBase.h.

◆ point_t

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::point_t = typename Lie::Euclidean<DIM>::light_type

TPoint2D or TPoint3D.

Definition at line 64 of file CPoseInterpolatorBase.h.

◆ pose_t

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::pose_t = typename Lie::SE<DIM>::light_type

TPose2D or TPose3D.

Definition at line 60 of file CPoseInterpolatorBase.h.

◆ reverse_iterator

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::reverse_iterator = typename TPath::reverse_iterator

Definition at line 70 of file CPoseInterpolatorBase.h.

◆ TPath

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::TPath = std::map<mrpt::Clock::time_point, pose_t>

Definition at line 67 of file CPoseInterpolatorBase.h.

◆ TTimePosePair

template<int DIM>
using mrpt::poses::CPoseInterpolatorBase< DIM >::TTimePosePair = std::pair<mrpt::Clock::time_point, pose_t>

Definition at line 66 of file CPoseInterpolatorBase.h.

Constructor & Destructor Documentation

◆ CPoseInterpolatorBase()

Default ctor: empty sequence of poses.

Definition at line 28 of file CPoseInterpolatorBase.hpp.

Member Function Documentation

◆ at() [1/2]

template<int DIM>
pose_t& mrpt::poses::CPoseInterpolatorBase< DIM >::at ( const mrpt::Clock::time_point t)
inline

Definition at line 114 of file CPoseInterpolatorBase.h.

◆ at() [2/2]

template<int DIM>
const pose_t& mrpt::poses::CPoseInterpolatorBase< DIM >::at ( const mrpt::Clock::time_point t) const
inline

Definition at line 115 of file CPoseInterpolatorBase.h.

◆ begin() [1/2]

template<int DIM>
iterator mrpt::poses::CPoseInterpolatorBase< DIM >::begin ( )
inline

Definition at line 73 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

◆ begin() [2/2]

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

Definition at line 74 of file CPoseInterpolatorBase.h.

◆ cbegin()

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

Definition at line 75 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 35 of file CPoseInterpolatorBase.hpp.

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

Here is the caller graph for this function:

◆ empty()

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

Definition at line 108 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

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

Here is the caller graph for this function:

◆ 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 101 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

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

◆ find() [1/2]

template<int DIM>
iterator mrpt::poses::CPoseInterpolatorBase< DIM >::find ( const mrpt::Clock::time_point t)
inline

Definition at line 109 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

◆ find() [2/2]

template<int DIM>
const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::find ( const mrpt::Clock::time_point t) const
inline

Definition at line 110 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::exceptionOn empty path

Definition at line 337 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 368 of file CPoseInterpolatorBase.hpp.

◆ getMaxTimeInterpolation()

template<int DIM>
mrpt::Clock::duration mrpt::poses::CPoseInterpolatorBase< DIM >::getMaxTimeInterpolation ( )

Set value of the maximum time to consider interpolation.

Definition at line 230 of file CPoseInterpolatorBase.hpp.

◆ getPreviousPoseWithMinDistance() [1/2]

template<int DIM>
bool mrpt::poses::CPoseInterpolatorBase< DIM >::getPreviousPoseWithMinDistance ( const mrpt::Clock::time_point 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 191 of file CPoseInterpolatorBase.hpp.

◆ getPreviousPoseWithMinDistance() [2/2]

template<int DIM>
bool mrpt::poses::CPoseInterpolatorBase< DIM >::getPreviousPoseWithMinDistance ( const mrpt::Clock::time_point 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 181 of file CPoseInterpolatorBase.hpp.

◆ impl_interpolation() [1/3]

template<>
void mrpt::poses::CPoseInterpolatorBase< 2 >::impl_interpolation ( const TTimePosePair p1,
const TTimePosePair p2,
const TTimePosePair p3,
const TTimePosePair p4,
const TInterpolatorMethod  method,
const mrpt::Clock::time_point t,
pose_t out_interp 
) const
protected

Definition at line 45 of file CPose2DInterpolator.cpp.

◆ impl_interpolation() [2/3]

template<>
void mrpt::poses::CPoseInterpolatorBase< 3 >::impl_interpolation ( const TTimePosePair p1,
const TTimePosePair p2,
const TTimePosePair p3,
const TTimePosePair p4,
const TInterpolatorMethod  method,
const mrpt::Clock::time_point t,
pose_t out_interp 
) const
protected

Definition at line 56 of file CPose3DInterpolator.cpp.

◆ impl_interpolation() [3/3]

template<int DIM>
void mrpt::poses::CPoseInterpolatorBase< DIM >::impl_interpolation ( const TTimePosePair p1,
const TTimePosePair p2,
const TTimePosePair p3,
const TTimePosePair p4,
const TInterpolatorMethod  method,
const mrpt::Clock::time_point td,
pose_t out_interp 
) const
protected

◆ insert() [1/2]

template<int DIM>
void mrpt::poses::CPoseInterpolatorBase< DIM >::insert ( const mrpt::Clock::time_point 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 47 of file CPoseInterpolatorBase.hpp.

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

Here is the caller graph for this function:

◆ insert() [2/2]

template<int DIM>
void mrpt::poses::CPoseInterpolatorBase< DIM >::insert ( const mrpt::Clock::time_point t,
const cpose_t p 
)

Overload (slower)

Definition at line 41 of file CPoseInterpolatorBase.hpp.

◆ interpolate() [1/2]

template<int DIM>
CPoseInterpolatorBase< DIM >::pose_t & mrpt::poses::CPoseInterpolatorBase< DIM >::interpolate ( const mrpt::Clock::time_point 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
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

Definition at line 70 of file CPoseInterpolatorBase.hpp.

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

Here is the caller graph for this function:

◆ interpolate() [2/2]

template<int DIM>
CPoseInterpolatorBase< DIM >::cpose_t & mrpt::poses::CPoseInterpolatorBase< DIM >::interpolate ( const mrpt::Clock::time_point 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 58 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::exceptionOn invalid file format

Definition at line 303 of file CPoseInterpolatorBase.hpp.

◆ lower_bound() [1/2]

template<int DIM>
iterator mrpt::poses::CPoseInterpolatorBase< DIM >::lower_bound ( const mrpt::Clock::time_point t)
inline

Definition at line 83 of file CPoseInterpolatorBase.h.

◆ lower_bound() [2/2]

template<int DIM>
const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::lower_bound ( const mrpt::Clock::time_point t) const
inline

Definition at line 87 of file CPoseInterpolatorBase.h.

◆ rbegin() [1/2]

template<int DIM>
reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rbegin ( )
inline

Definition at line 79 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

◆ rbegin() [2/2]

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

Definition at line 80 of file CPoseInterpolatorBase.h.

◆ rend() [1/2]

template<int DIM>
reverse_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::rend ( )
inline

Definition at line 81 of file CPoseInterpolatorBase.h.

◆ rend() [2/2]

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

Definition at line 82 of file CPoseInterpolatorBase.h.

◆ saveInterpolatedToTextFile()

template<int DIM>
bool mrpt::poses::CPoseInterpolatorBase< DIM >::saveInterpolatedToTextFile ( const std::string s,
const mrpt::Clock::duration 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 265 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:

Definition at line 236 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 361 of file CPoseInterpolatorBase.hpp.

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

Here is the caller graph for this function:

◆ setMaxTimeInterpolation()

template<int DIM>
void mrpt::poses::CPoseInterpolatorBase< DIM >::setMaxTimeInterpolation ( const mrpt::Clock::duration 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 222 of file CPoseInterpolatorBase.hpp.

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

Here is the caller graph for this function:

◆ size()

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

Definition at line 107 of file CPoseInterpolatorBase.h.

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

Here is the caller graph for this function:

◆ upper_bound() [1/2]

template<int DIM>
iterator mrpt::poses::CPoseInterpolatorBase< DIM >::upper_bound ( const mrpt::Clock::time_point t)
inline

Definition at line 92 of file CPoseInterpolatorBase.h.

◆ upper_bound() [2/2]

template<int DIM>
const_iterator mrpt::poses::CPoseInterpolatorBase< DIM >::upper_bound ( const mrpt::Clock::time_point t) const
inline

Definition at line 96 of file CPoseInterpolatorBase.h.

Member Data Documentation

◆ m_method

Definition at line 212 of file CPoseInterpolatorBase.h.

◆ m_path

template<int DIM>
TPath mrpt::poses::CPoseInterpolatorBase< DIM >::m_path
protected

◆ maxTimeInterpolation

template<int DIM>
mrpt::Clock::duration 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 211 of file CPoseInterpolatorBase.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019