Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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  /** A trajectory in time and in 6D (CPose3D) that interpolates using splines the intervals between a set of given time-referenced poses.
28  * To insert new points into the sequence, use the "insert" method, and for getting an interpolated point, use "interpolate" method. For example:
29  * \code
30  * CPose3DInterpolator path;
31  *
32  * path.setInterpolationMethod( CPose3DInterpolator::imSplineSlerp );
33  *
34  * path.insert( t0, CPose3D(...) );
35  * path.insert( t1, CPose3D(...) );
36  * path.insert( t2, CPose3D(...) );
37  * path.insert( t3, CPose3D(...) );
38  *
39  * CPose3D p;
40  * bool valid;
41  *
42  * cout << "Pose at t: " << path.interpolate(t,p,valid) << endl;
43  * \endcode
44  *
45  * Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references.
46  *
47  * See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is "imLinearSlerp".
48  *
49  * \sa CPoseOrPoint
50  * \ingroup interpolation_grp poses_grp
51  */
53  {
54  // This must be added to any CSerializable derived class:
56 
57  private:
58  typedef mrpt::aligned_containers< mrpt::system::TTimeStamp, CPose3D >::map_t TPath;
59  TPath m_path; //!< The sequence of poses
60 
61  public:
62  typedef TPath::iterator iterator;
66 
67  /** Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod
68  * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
69  * - imLinear2Neig: Linear interpolation between the previous and next neightbour.
70  * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
71  * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
72  * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
73  * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
74  * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
75  */
77  {
78  imSpline = 0,
84  imSplineSlerp
85  };
86 
87  inline iterator begin() { return m_path.begin(); }
88  inline const_iterator begin() const { return m_path.begin(); }
89 
90  inline iterator end() { return m_path.end(); }
91  inline const_iterator end() const { return m_path.end(); }
92 
93  inline reverse_iterator rbegin() { return m_path.rbegin(); }
94  inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
95 
96  inline reverse_iterator rend() { return m_path.rend(); }
97  inline const_reverse_iterator rend() const { return m_path.rend(); }
98 
99  iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
100  const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
101 
102  iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
103  const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
104 
105  iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
106 
107  size_t size() const { return m_path.size(); }
108  bool empty() const { return m_path.empty(); }
109 
110  /** Creates an empty interpolator (with no points).
111  */
113 
114  /** Inserts a new pose in the sequence.
115  * It overwrites any previously existing pose at exactly the same time.
116  */
117  void insert( mrpt::system::TTimeStamp t, const CPose3D &p);
118 
119  /** Returns the pose at a given time, or interpolates using splines if there is not an exact match.
120  * \param t The time of the point to interpolate.
121  * \param out_interp The output interpolated pose.
122  * \param out_valid_interp Whether there was information enough to compute the interpolation.
123  * \return A reference to out_interp
124  */
125  CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const;
126 
127  /** Clears the current sequence of poses */
128  void clear();
129 
130  /** Set value of the maximum time to consider interpolation.
131  * If set to a negative value, the check is disabled (default behavior).
132  */
133  void setMaxTimeInterpolation( double time );
134 
135  /** Set value of the maximum time to consider interpolation */
136  double getMaxTimeInterpolation( );
137 
138  /** Get the previous CPose3D in the map with a minimum defined distance
139  * \return true if pose was found, false otherwise.
140  */
141  bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose );
142 
143  /** Saves the points in the interpolator to a text file, with this format:
144  * Each row contains these elements separated by spaces:
145  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
146  * - x y z: The 3D position in meters.
147  * - yaw pitch roll: The angles, in radians
148  * \sa loadFromTextFile
149  * \return true on success, false on any error.
150  */
151  bool saveToTextFile(const std::string &s) const;
152 
153  /** 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.
154  * \sa loadFromTextFile
155  * \return true on success, false on any error.
156  */
157  bool saveInterpolatedToTextFile(const std::string &s, double period) const;
158 
159  /** Loads from a text file, in the format described by saveToTextFile.
160  * \return true on success, false on any error.
161  * \exception std::exception On invalid file format
162  */
163  bool loadFromTextFile(const std::string &s);
164 
165  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
166  * \exception std::exception On empty path
167  */
168  void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const;
169 
170  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
171  * \exception std::exception On empty path
172  */
173  void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const;
174 
175  /** Change the method used to interpolate the robot path.
176  * The default method at construction is "imSpline".
177  * \sa getInterpolationMethod
178  */
179  void setInterpolationMethod( TInterpolatorMethod method);
180 
181  /** Returns the currently set interpolation method.
182  * \sa setInterpolationMethod
183  */
184  TInterpolatorMethod getInterpolationMethod() const;
185 
186  /** 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.
187  * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll).
188  * \param samples [IN] The width of the average filter.
189  */
190  void filter( unsigned int component, unsigned int samples );
191 
192 
193  private:
194  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.
195 
197 
198  }; // End of class def.
200 
201  } // End of namespace
202 
203  // Specializations MUST occur at the same namespace:
204  namespace utils
205  {
206  template <>
208  {
210  static void fill(bimap<enum_t,std::string> &m_map)
211  {
212  m_map.insert(poses::CPose3DInterpolator::imSpline, "imSpline");
213  m_map.insert(poses::CPose3DInterpolator::imLinear2Neig, "imLinear2Neig");
214  m_map.insert(poses::CPose3DInterpolator::imLinear4Neig, "imLinear4Neig");
215  m_map.insert(poses::CPose3DInterpolator::imSSLLLL, "imSSLLLL");
216  m_map.insert(poses::CPose3DInterpolator::imLinearSlerp, "imLinearSlerp");
217  m_map.insert(poses::CPose3DInterpolator::imSplineSlerp, "imSplineSlerp");
218  }
219  };
220  } // End of namespace
221 } // End of namespace
222 
223 #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 rend() 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
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...
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
const_iterator upper_bound(const mrpt::system::TTimeStamp &t) const
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
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
Helper types for STL containers with Eigen memory allocators.
std::pair< mrpt::system::TTimeStamp, mrpt::poses::CPose3D > TTimePosePair
class BASE_IMPEXP CSerializable
Definition: CStream.h:23
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...
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)
const_iterator lower_bound(const mrpt::system::TTimeStamp &t) const
A trajectory in time and in 6D (CPose3D) that interpolates using splines the intervals between a set ...
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
const_reverse_iterator rbegin() const
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.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016