Main MRPT website > C++ reference for MRPT 1.4.0
CPose3DInterpolator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CPose3DInterpolator_H
10 #define CPose3DInterpolator_H
11 
13 #include <mrpt/system/datetime.h>
14 #include <mrpt/utils/TEnumType.h>
16 #include <mrpt/math/math_frwds.h>
17 #include <mrpt/poses/CPose3D.h>
18 
19 namespace mrpt
20 {
21  namespace poses
22  {
24 
25  typedef std::pair<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> TTimePosePair;
26 
27  /** This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
28  * It can also interpolate SE(3) poses over time using linear, splines or SLERP interpolation, as set in CPose3DInterpolator::setInterpolationMethod()
29  * Usage:
30  * - Insert new poses into the sequence with CPose3DInterpolator::insert()
31  * - Query an exact/interpolated pose with CPose3DInterpolator::interpolate().
32  * Example:
33  * \code
34  * CPose3DInterpolator path;
35  *
36  * path.setInterpolationMethod( CPose3DInterpolator::imSplineSlerp );
37  *
38  * path.insert( t0, CPose3D(...) );
39  * path.insert( t1, CPose3D(...) );
40  * path.insert( t2, CPose3D(...) );
41  * path.insert( t3, CPose3D(...) );
42  *
43  * CPose3D p;
44  * bool valid;
45  *
46  * cout << "Pose at t: " << path.interpolate(t,p,valid) << endl;
47  * \endcode
48  *
49  * Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references.
50  *
51  * See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is "imLinearSlerp".
52  *
53  * \sa CPoseOrPoint
54  * \ingroup interpolation_grp poses_grp
55  */
56  class BASE_IMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable
57  {
58  // This must be added to any CSerializable derived class:
60 
61  private:
63  TPath m_path; //!< The sequence of poses
64 
65  public:
68  typedef TPath::reverse_iterator reverse_iterator;
69  typedef TPath::const_reverse_iterator const_reverse_iterator;
70 
71  /** Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod
72  * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
73  * - imLinear2Neig: Linear interpolation between the previous and next neightbour.
74  * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
75  * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
76  * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
77  * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
78  * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
79  */
81  {
82  imSpline = 0,
88  imSplineSlerp
89  };
90 
91  inline iterator begin() { return m_path.begin(); }
92  inline const_iterator begin() const { return m_path.begin(); }
93 
94  inline iterator end() { return m_path.end(); }
95  inline const_iterator end() const { return m_path.end(); }
96 
97  inline reverse_iterator rbegin() { return m_path.rbegin(); }
98  inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
99 
100  inline reverse_iterator rend() { return m_path.rend(); }
101  inline const_reverse_iterator rend() const { return m_path.rend(); }
102 
103  iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
104  const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
105 
106  iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
107  const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
108 
109  iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
110 
111  size_t size() const { return m_path.size(); }
112  bool empty() const { return m_path.empty(); }
113 
114  /** Creates an empty interpolator (with no points).
115  */
117 
118  /** Inserts a new pose in the sequence.
119  * It overwrites any previously existing pose at exactly the same time.
120  */
122 
123  /** Returns the pose at a given time, or interpolates using splines if there is not an exact match.
124  * \param t The time of the point to interpolate.
125  * \param out_interp The output interpolated pose.
126  * \param out_valid_interp Whether there was information enough to compute the interpolation.
127  * \return A reference to out_interp
128  */
129  CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const;
130 
131  /** Clears the current sequence of poses */
132  void clear();
133 
134  /** Set value of the maximum time to consider interpolation.
135  * If set to a negative value, the check is disabled (default behavior).
136  */
137  void setMaxTimeInterpolation( double time );
138 
139  /** Set value of the maximum time to consider interpolation */
141 
142  /** Get the previous CPose3D in the map with a minimum defined distance
143  * \return true if pose was found, false otherwise.
144  */
146 
147  /** Saves the points in the interpolator to a text file, with this format:
148  * Each row contains these elements separated by spaces:
149  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
150  * - x y z: The 3D position in meters.
151  * - yaw pitch roll: The angles, in radians
152  * \sa loadFromTextFile
153  * \return true on success, false on any error.
154  */
155  bool saveToTextFile(const std::string &s) const;
156 
157  /** 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.
158  * \sa loadFromTextFile
159  * \return true on success, false on any error.
160  */
161  bool saveInterpolatedToTextFile(const std::string &s, double period) const;
162 
163  /** Loads from a text file, in the format described by saveToTextFile.
164  * \return true on success, false on any error.
165  * \exception std::exception On invalid file format
166  */
167  bool loadFromTextFile(const std::string &s);
168 
169  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
170  * \exception std::exception On empty path
171  */
172  void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const;
173 
174  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
175  * \exception std::exception On empty path
176  */
177  void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const;
178 
179  /** Change the method used to interpolate the robot path.
180  * The default method at construction is "imSpline".
181  * \sa getInterpolationMethod
182  */
184 
185  /** Returns the currently set interpolation method.
186  * \sa setInterpolationMethod
187  */
189 
190  /** 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.
191  * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll).
192  * \param samples [IN] The width of the average filter.
193  */
194  void filter( unsigned int component, unsigned int samples );
195 
196 
197  private:
198  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.
199 
201 
202  }; // End of class def.
204 
205  } // End of namespace
206 
207  // Specializations MUST occur at the same namespace:
208  namespace utils
209  {
210  template <>
211  struct TEnumTypeFiller<poses::CPose3DInterpolator::TInterpolatorMethod>
212  {
214  static void fill(bimap<enum_t,std::string> &m_map)
215  {
216  m_map.insert(poses::CPose3DInterpolator::imSpline, "imSpline");
217  m_map.insert(poses::CPose3DInterpolator::imLinear2Neig, "imLinear2Neig");
218  m_map.insert(poses::CPose3DInterpolator::imLinear4Neig, "imLinear4Neig");
219  m_map.insert(poses::CPose3DInterpolator::imSSLLLL, "imSSLLLL");
220  m_map.insert(poses::CPose3DInterpolator::imLinearSlerp, "imLinearSlerp");
221  m_map.insert(poses::CPose3DInterpolator::imSplineSlerp, "imSplineSlerp");
222  }
223  };
224  } // End of namespace
225 } // End of namespace
226 
227 #endif
mrpt::poses::CPose3DInterpolator::begin
iterator begin()
Definition: CPose3DInterpolator.h:91
mrpt::poses::CPose3DInterpolator::empty
bool empty() const
Definition: CPose3DInterpolator.h:112
mrpt::poses::CPose3DInterpolator::imLinear2Neig
@ imLinear2Neig
Definition: CPose3DInterpolator.h:83
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::poses::CPose3DInterpolator::setInterpolationMethod
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
mrpt::poses::CPose3DInterpolator::imLinear4Neig
@ imLinear4Neig
Definition: CPose3DInterpolator.h:84
mrpt::poses::CPose3DInterpolator::clear
void clear()
Clears the current sequence of poses.
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::poses::CPose3DInterpolator::setMaxTimeInterpolation
void setMaxTimeInterpolation(double time)
Set value of the maximum time to consider interpolation.
mrpt::poses::CPose3DInterpolator::insert
void insert(mrpt::system::TTimeStamp t, const CPose3D &p)
Inserts a new pose in the sequence.
mrpt::poses::CPose3DInterpolator::rend
reverse_iterator rend()
Definition: CPose3DInterpolator.h:100
mrpt::poses::CPose3DInterpolator::size
size_t size() const
Definition: CPose3DInterpolator.h:111
mrpt::poses::CPose3DInterpolator::rbegin
reverse_iterator rbegin()
Definition: CPose3DInterpolator.h:97
mrpt::poses::CPose3DInterpolator::upper_bound
const_iterator upper_bound(const mrpt::system::TTimeStamp &t) const
Definition: CPose3DInterpolator.h:107
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
Definition: CSerializable.h:182
mrpt::math::distance
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
mrpt::utils::bimap
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
mrpt::poses::CPose3DInterpolator::imLinearSlerp
@ imLinearSlerp
Definition: CPose3DInterpolator.h:87
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::poses::CPose3DInterpolator::filter
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the CPose3D data within the interpolator.
mrpt::poses::TTimePosePair
std::pair< mrpt::system::TTimeStamp, mrpt::poses::CPose3D > TTimePosePair
Definition: CPose3DInterpolator.h:25
mrpt::utils::TEnumTypeFiller< poses::CPose3DInterpolator::TInterpolatorMethod >::enum_t
poses::CPose3DInterpolator::TInterpolatorMethod enum_t
Definition: CPose3DInterpolator.h:213
mrpt::poses::CPose3DInterpolator::const_reverse_iterator
TPath::const_reverse_iterator const_reverse_iterator
Definition: CPose3DInterpolator.h:69
mrpt::poses::CPose3DInterpolator
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
Definition: CPose3DInterpolator.h:57
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
mrpt::poses::CPose3DInterpolator::TPath
mrpt::aligned_containers< mrpt::system::TTimeStamp, CPose3D >::map_t TPath
Definition: CPose3DInterpolator.h:62
t
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
Definition: eigen_plugins.h:470
mrpt::poses::CPose3DInterpolator::maxTimeInterpolation
double maxTimeInterpolation
Maximum time considered to interpolate. If the difference between the desired timestamp where to inte...
Definition: CPose3DInterpolator.h:198
mrpt::poses::CPose3DInterpolator::rbegin
const_reverse_iterator rbegin() const
Definition: CPose3DInterpolator.h:98
mrpt::poses::CPose3DInterpolator::imSpline
@ imSpline
Definition: CPose3DInterpolator.h:82
mrpt::poses::CPose3DInterpolator::imSSLSLL
@ imSSLSLL
Definition: CPose3DInterpolator.h:86
mrpt::poses::CPose3DInterpolator::end
iterator end()
Definition: CPose3DInterpolator.h:94
mrpt::utils::bimap::insert
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
mrpt::poses::CPose3DInterpolator::getMaxTimeInterpolation
double getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
mrpt::aligned_containers::map_t
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
Definition: aligned_containers.h:31
mrpt::poses::CPose3DInterpolator::CPose3DInterpolator
CPose3DInterpolator()
Creates an empty interpolator (with no points).
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
mrpt::poses::CPose3DInterpolator::const_iterator
TPath::const_iterator const_iterator
Definition: CPose3DInterpolator.h:67
TEnumType.h
math_frwds.h
mrpt::poses::CPose3DInterpolator::TInterpolatorMethod
TInterpolatorMethod
Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod.
Definition: CPose3DInterpolator.h:81
mrpt::poses::CPose3DInterpolator::reverse_iterator
TPath::reverse_iterator reverse_iterator
Definition: CPose3DInterpolator.h:68
mrpt::poses::CPose3DInterpolator::m_method
TInterpolatorMethod m_method
Definition: CPose3DInterpolator.h:200
mrpt::utils::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:229
CPose3D.h
mrpt::poses::CPose3DInterpolator::begin
const_iterator begin() const
Definition: CPose3DInterpolator.h:92
mrpt::poses::CPose3DInterpolator::getInterpolationMethod
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
mrpt::poses::CPose3DInterpolator::loadFromTextFile
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by saveToTextFile.
mrpt::poses::CPose3DInterpolator::saveToTextFile
bool saveToTextFile(const std::string &s) const
Saves the points in the interpolator to a text file, with this format: Each row contains these elemen...
mrpt::poses::CPose3DInterpolator::upper_bound
iterator upper_bound(const mrpt::system::TTimeStamp &t)
Definition: CPose3DInterpolator.h:106
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:147
mrpt::poses::CPose3DInterpolator::interpolate
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.
mrpt::poses::CPose3DInterpolator::imSSLLLL
@ imSSLLLL
Definition: CPose3DInterpolator.h:85
mrpt::poses::CPose3DInterpolator::lower_bound
const_iterator lower_bound(const mrpt::system::TTimeStamp &t) const
Definition: CPose3DInterpolator.h:104
mrpt::poses::CPose3DInterpolator::m_path
TPath m_path
The sequence of poses.
Definition: CPose3DInterpolator.h:63
mrpt::poses::CPose3DInterpolator::iterator
TPath::iterator iterator
Definition: CPose3DInterpolator.h:66
mrpt::poses::CPose3DInterpolator::saveInterpolatedToTextFile
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,...
mrpt::poses::CPose3DInterpolator::imSplineSlerp
@ imSplineSlerp
Definition: CPose3DInterpolator.h:88
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
CSerializable.h
mrpt::poses::CPose3DInterpolator::getBoundingBox
void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const
Computes the bounding box in X,Y,Z of the whole vehicle path.
mrpt::poses::CPose3DInterpolator::getPreviousPoseWithMinDistance
bool getPreviousPoseWithMinDistance(const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose)
Get the previous CPose3D in the map with a minimum defined distance.
mrpt::utils::TEnumTypeFiller< poses::CPose3DInterpolator::TInterpolatorMethod >::fill
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CPose3DInterpolator.h:214
mrpt::poses::CPose3DInterpolator::getBoundingBox
void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const
Computes the bounding box in X,Y,Z of the whole vehicle path.
mrpt::poses::CPose3DInterpolator::erase
iterator erase(iterator element_to_erase)
Definition: CPose3DInterpolator.h:109
aligned_containers.h
mrpt::poses::CPose3DInterpolator::end
const_iterator end() const
Definition: CPose3DInterpolator.h:95
mrpt::utils::TEnumTypeFiller
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24
mrpt::poses::CPose3DInterpolator::lower_bound
iterator lower_bound(const mrpt::system::TTimeStamp &t)
Definition: CPose3DInterpolator.h:103
datetime.h
mrpt::poses::CPose3DInterpolator::rend
const_reverse_iterator rend() const
Definition: CPose3DInterpolator.h:101
DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Definition: CSerializable.h:178



Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020