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  */
121  void insert( mrpt::system::TTimeStamp t, const CPose3D &p);
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 */
140  double getMaxTimeInterpolation( );
141 
142  /** Get the previous CPose3D in the map with a minimum defined distance
143  * \return true if pose was found, false otherwise.
144  */
145  bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose );
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  */
183  void setInterpolationMethod( TInterpolatorMethod method);
184 
185  /** Returns the currently set interpolation method.
186  * \sa setInterpolationMethod
187  */
188  TInterpolatorMethod getInterpolationMethod() const;
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
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
TPath::const_iterator const_iterator
TPath::reverse_iterator reverse_iterator
const_reverse_iterator rbegin() const
TInterpolatorMethod
Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
Scalar * iterator
Definition: eigen_plugins.h:23
T interpolate(const T &x, const VECTOR &ys, const T &x0, const T &x1)
Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...
Definition: interp_fit.h:30
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
STL namespace.
iterator upper_bound(const mrpt::system::TTimeStamp &t)
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
const_iterator lower_bound(const mrpt::system::TTimeStamp &t) const
std::pair< mrpt::system::TTimeStamp, mrpt::poses::CPose3D > TTimePosePair
const_reverse_iterator rend() const
TPath m_path
The sequence of poses.
iterator erase(iterator element_to_erase)
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A class used to store a 3D point.
Definition: CPoint3D.h:32
iterator lower_bound(const mrpt::system::TTimeStamp &t)
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
const_iterator upper_bound(const mrpt::system::TTimeStamp &t) const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
double maxTimeInterpolation
Maximum time considered to interpolate. If the difference between the desired timestamp where to inte...
Lightweight 3D point.
mrpt::aligned_containers< mrpt::system::TTimeStamp, CPose3D >::map_t TPath
TPath::const_reverse_iterator const_reverse_iterator
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.



Page generated by Doxygen 1.8.14 for MRPT 1.4.0 SVN: at Mon Mar 5 22:51:34 UTC 2018