Main MRPT website > C++ reference
MRPT logo
CPose3DInterpolator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPose3DInterpolator_H
29 #define CPose3DInterpolator_H
30 
31 #include <mrpt/poses/CPose.h>
32 #include <mrpt/poses/CPose3D.h>
33 #include <mrpt/poses/CPoint3D.h>
34 #include <mrpt/system/os.h>
35 #include <mrpt/utils/stl_extensions.h>
36 #include <mrpt/utils/TEnumType.h>
37 
38 namespace mrpt
39 {
40  namespace poses
41  {
42  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DInterpolator, mrpt::utils::CSerializable )
43 
44  typedef std::pair<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> TTimePosePair;
45 
46  /** A trajectory in time and in 6D (CPose3D) that interpolates using splines the intervals between a set of given time-referenced poses.
47  * To insert new points into the sequence, use the "insert" method, and for getting an interpolated point, use "interpolate" method. For example:
48  * \code
49  * CPose3DInterpolator path;
50  *
51  * path.setInterpolationMethod( CPose3DInterpolator::imSplineSlerp );
52  *
53  * path.insert( t0, CPose3D(...) );
54  * path.insert( t1, CPose3D(...) );
55  * path.insert( t2, CPose3D(...) );
56  * path.insert( t3, CPose3D(...) );
57  *
58  * CPose3D p;
59  * bool valid;
60  *
61  * cout << "Pose at t: " << path.interpolate(t,p,valid) << endl;
62  * \endcode
63  *
64  * Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references.
65  *
66  * See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is "imLinearSlerp".
67  *
68  * \sa CPoseOrPoint
69  * \ingroup interpolation_grp poses_grp
70  */
71  class BASE_IMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable
72  {
73  // This must be added to any CSerializable derived class:
74  DEFINE_SERIALIZABLE( CPose3DInterpolator )
75 
76  private:
77  typedef std::map< mrpt::system::TTimeStamp, CPose3D > TPath;
78  TPath m_path; //!< The sequence of poses
79 
80  public:
81  typedef TPath::iterator iterator;
82  typedef TPath::const_iterator const_iterator;
83  typedef TPath::reverse_iterator reverse_iterator;
84  typedef TPath::const_reverse_iterator const_reverse_iterator;
85 
86  /** Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod
87  * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
88  * - imLinear2Neig: Linear interpolation between the previous and next neightbour.
89  * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
90  * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
91  * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
92  * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
93  * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
94  */
95  enum TInterpolatorMethod
96  {
97  imSpline = 0,
98  imLinear2Neig,
99  imLinear4Neig,
100  imSSLLLL,
101  imSSLSLL,
102  imLinearSlerp,
103  imSplineSlerp
104  };
105 
106  inline iterator begin() { return m_path.begin(); }
107  inline const_iterator begin() const { return m_path.begin(); }
108 
109  inline iterator end() { return m_path.end(); }
110  inline const_iterator end() const { return m_path.end(); }
111 
112  inline reverse_iterator rbegin() { return m_path.rbegin(); }
113  inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
114 
115  inline reverse_iterator rend() { return m_path.rend(); }
116  inline const_reverse_iterator rend() const { return m_path.rend(); }
117 
118  iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
119  const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
120 
121  iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
122  const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
123 
124  iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
125 
126  size_t size() const { return m_path.size(); }
127  bool empty() const { return m_path.empty(); }
128 
129  /** Creates an empty interpolator (with no points).
130  */
131  CPose3DInterpolator();
132 
133  /** Inserts a new pose in the sequence.
134  * It overwrites any previously existing pose at exactly the same time.
135  */
136  void insert( mrpt::system::TTimeStamp t, const CPose3D &p);
137 
138  /** Returns the pose at a given time, or interpolates using splines if there is not an exact match.
139  * \param t The time of the point to interpolate.
140  * \param out_interp The output interpolated pose.
141  * \param out_valid_interp Whether there was information enough to compute the interpolation.
142  * \return A reference to out_interp
143  */
144  CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const;
145 
146  /** Clears the current sequence of poses */
147  void clear();
148 
149  /** Set value of the maximum time to consider interpolation.
150  * If set to a negative value, the check is disabled (default behavior).
151  */
152  void setMaxTimeInterpolation( double time );
153 
154  /** Set value of the maximum time to consider interpolation */
155  double getMaxTimeInterpolation( );
156 
157  /** Get the previous CPose3D in the map with a minimum defined distance
158  * \return true if pose was found, false otherwise.
159  */
160  bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose );
161 
162  /** Saves the points in the interpolator to a text file, with this format:
163  * Each row contains these elements separated by spaces:
164  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
165  * - x y z: The 3D position in meters.
166  * - yaw pitch roll: The angles, in radians
167  * \sa loadFromTextFile
168  * \return true on success, false on any error.
169  */
170  bool saveToTextFile(const std::string &s) const;
171 
172  /** 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.
173  * \sa loadFromTextFile
174  * \return true on success, false on any error.
175  */
176  bool saveInterpolatedToTextFile(const std::string &s, double period) const;
177 
178  /** Loads from a text file, in the format described by saveToTextFile.
179  * \return true on success, false on any error.
180  * \exception std::exception On invalid file format
181  */
182  bool loadFromTextFile(const std::string &s);
183 
184  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
185  * \exception std::exception On empty path
186  */
187  void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const;
188 
189  /** Computes the bounding box in X,Y,Z of the whole vehicle path.
190  * \exception std::exception On empty path
191  */
192  void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const;
193 
194  /** Change the method used to interpolate the robot path.
195  * The default method at construction is "imSpline".
196  * \sa getInterpolationMethod
197  */
198  void setInterpolationMethod( TInterpolatorMethod method);
199 
200  /** Returns the currently set interpolation method.
201  * \sa setInterpolationMethod
202  */
203  TInterpolatorMethod getInterpolationMethod() const;
204 
205  /** 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.
206  * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll).
207  * \param samples [IN] The width of the average filter.
208  */
209  void filter( unsigned int component, unsigned int samples );
210 
211 
212  private:
213  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.
214 
215  TInterpolatorMethod m_method;
216 
217  }; // End of class def.
218 
219  } // End of namespace
220 
221  // Specializations MUST occur at the same namespace:
222  namespace utils
223  {
224  template <>
225  struct TEnumTypeFiller<poses::CPose3DInterpolator::TInterpolatorMethod>
226  {
227  typedef poses::CPose3DInterpolator::TInterpolatorMethod enum_t;
228  static void fill(bimap<enum_t,std::string> &m_map)
229  {
230  m_map.insert(poses::CPose3DInterpolator::imSpline, "imSpline");
231  m_map.insert(poses::CPose3DInterpolator::imLinear2Neig, "imLinear2Neig");
232  m_map.insert(poses::CPose3DInterpolator::imLinear4Neig, "imLinear4Neig");
233  m_map.insert(poses::CPose3DInterpolator::imSSLLLL, "imSSLLLL");
234  m_map.insert(poses::CPose3DInterpolator::imLinearSlerp, "imLinearSlerp");
235  m_map.insert(poses::CPose3DInterpolator::imSplineSlerp, "imSplineSlerp");
236  }
237  };
238  } // End of namespace
239 } // End of namespace
240 
241 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013