Main MRPT website > C++ reference for MRPT 1.5.7
CPoseInterpolatorBase.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-2017, 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 #pragma once
10 
11 #include <mrpt/system/datetime.h>
12 #include <mrpt/utils/TEnumType.h>
13 #include <mrpt/poses/SE_traits.h>
15 #include <mrpt/poses/poses_frwds.h>
16 #include <mrpt/base/link_pragmas.h>
17 
18 namespace mrpt {
19 namespace poses {
20 
21  /** Type to select the interpolation method in CPoseInterpolatorBase derived classes.
22  * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
23  * - imLinear2Neig: Linear interpolation between the previous and next neightbour.
24  * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
25  * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
26  * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
27  * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
28  * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
29  * \ingroup interpolation_grp poses_grp
30  */
32  {
33  imSpline = 0,
40  };
41 
42 
43  /** Base class for SE(2)/SE(3) interpolators. See docs for derived classes.
44  * \ingroup interpolation_grp poses_grp
45  */
46  template <int DIM>
48  {
49  public:
50  CPoseInterpolatorBase(); //!< Default ctor: empty sequence of poses
51 
52  /** @name Type definitions and STL-like container interface
53  * @{ */
54 
55  typedef typename mrpt::poses::SE_traits<DIM>::lightweight_pose_t pose_t; //!< TPose2D or TPose3D
56  typedef typename mrpt::poses::SE_traits<DIM>::pose_t cpose_t; //!< CPose2D or CPose3D
57  typedef typename mrpt::poses::SE_traits<DIM>::point_t point_t; //!< TPoint2D or TPoint3D
58 
59  typedef std::pair<mrpt::system::TTimeStamp,pose_t> TTimePosePair;
60  typedef std::map<mrpt::system::TTimeStamp,pose_t> TPath;
61  typedef typename TPath::iterator iterator;
63  typedef typename TPath::reverse_iterator reverse_iterator;
64  typedef typename TPath::const_reverse_iterator const_reverse_iterator;
65 
66  inline iterator begin() { return m_path.begin(); }
67  inline const_iterator begin() const { return m_path.begin(); }
68  inline const_iterator cbegin() const {
69 #if MRPT_HAS_CXX11
70  return m_path.cbegin();
71 #else
72  return m_path.begin();
73 #endif
74  }
75 
76  inline iterator end() { return m_path.end(); }
77  inline const_iterator end() const { return m_path.end(); }
78  inline const_iterator cend() const {
79 #if MRPT_HAS_CXX11
80  return m_path.cend();
81 #else
82  return m_path.end();
83 #endif
84  }
85 
86  inline reverse_iterator rbegin() { return m_path.rbegin(); }
87  inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
88 
89  inline reverse_iterator rend() { return m_path.rend(); }
90  inline const_reverse_iterator rend() const { return m_path.rend(); }
91 
92  iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
93  const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
94 
95  iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
96  const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
97 
98  iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
99 
100  size_t size() const { return m_path.size(); }
101  bool empty() const { return m_path.empty(); }
102 
103  iterator find(const mrpt::system::TTimeStamp & t) { return m_path.find(t); }
104  const_iterator find(const mrpt::system::TTimeStamp & t) const { return m_path.find(t); }
105  /** @} */
106 
107  /** Inserts a new pose in the sequence.
108  * It overwrites any previously existing pose at exactly the same time.
109  */
110  void insert( mrpt::system::TTimeStamp t, const pose_t &p);
111  void insert( mrpt::system::TTimeStamp t, const cpose_t &p); //!< Overload (slower)
112 
113  /** Returns the pose at a given time, or interpolates using splines if there is not an exact match.
114  * \param t The time of the point to interpolate.
115  * \param out_interp The output interpolated pose.
116  * \param out_valid_interp Whether there was information enough to compute the interpolation.
117  * \return A reference to out_interp
118  */
119  pose_t &interpolate(mrpt::system::TTimeStamp t, pose_t &out_interp, bool &out_valid_interp ) const;
120  cpose_t &interpolate(mrpt::system::TTimeStamp t, cpose_t &out_interp, bool &out_valid_interp) const; //!< \overload (slower)
121 
122  void clear(); //!< Clears the current sequence of poses
123 
124  /** Set value of the maximum time to consider interpolation.
125  * If set to a negative value, the check is disabled (default behavior). */
126  void setMaxTimeInterpolation( double time );
127  double getMaxTimeInterpolation(); //!< Set value of the maximum time to consider interpolation
128 
129  /** Get the previous CPose3D in the map with a minimum defined distance.
130  * \return true if pose was found, false otherwise */
131  bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, pose_t &out_pose );
132  bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, cpose_t &out_pose ); //!< \overload (slower)
133 
134  /** Saves the points in the interpolator to a text file, with this format:
135  * Each row contains these elements separated by spaces:
136  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
137  * - x y z: The 3D position in meters.
138  * - yaw pitch roll: The angles, in radians
139  * \sa loadFromTextFile
140  * \return true on success, false on any error.
141  */
142  bool saveToTextFile(const std::string &s) const;
143 
144  /** 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.
145  * \sa loadFromTextFile
146  * \return true on success, false on any error.
147  */
148  bool saveInterpolatedToTextFile(const std::string &s, double period) const;
149 
150  /** Loads from a text file, in the format described by saveToTextFile.
151  * \return true on success, false on any error.
152  * \exception std::exception On invalid file format
153  */
154  bool loadFromTextFile(const std::string &s);
155 
156  /** Computes the bounding box in all Euclidean coordinates of the whole path. \exception std::exception On empty path */
157  void getBoundingBox(point_t &minCorner, point_t &maxCorner) const;
158 
159  void setInterpolationMethod( TInterpolatorMethod method); //!< Change the method used to interpolate the robot path. The default method at construction is "imSpline". \sa getInterpolationMethod()
160  TInterpolatorMethod getInterpolationMethod() const; //!< Returns the currently set interpolation method. \sa setInterpolationMethod()
161 
162  /** 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.
163  * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll)
164  * \param samples [IN] The width of the average filter.
165  */
166  void filter( unsigned int component, unsigned int samples );
167 
168 
169  protected:
170  TPath m_path; //!< The sequence of poses
171  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.
173 
174  void impl_interpolation(
175  const mrpt::math::CArrayDouble<4> &ts,
176  const TTimePosePair p1,const TTimePosePair p2,const TTimePosePair p3,const TTimePosePair p4,
177  const TInterpolatorMethod method,double td,pose_t &out_interp) const;
178 
179  }; // End of class def.
180 
181 } // End of namespace
182 
183 // Specializations MUST occur at the same namespace:
184 namespace utils
185 {
186  template <>
188  {
190  static void fill(bimap<enum_t,std::string> &m_map)
191  {
192  m_map.insert(poses::imSpline, "imSpline");
193  m_map.insert(poses::imLinear2Neig, "imLinear2Neig");
194  m_map.insert(poses::imLinear4Neig, "imLinear4Neig");
195  m_map.insert(poses::imSSLLLL, "imSSLLLL");
196  m_map.insert(poses::imLinearSlerp, "imLinearSlerp");
197  m_map.insert(poses::imSplineSlerp, "imSplineSlerp");
198  }
199  };
200 } // End of namespace
201 } // End of namespace
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
GLdouble GLdouble t
Definition: glext.h:3610
Base class for SE(2)/SE(3) interpolators.
std::map< mrpt::system::TTimeStamp, pose_t > TPath
const_reverse_iterator rbegin() const
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.hpp:17
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...
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
TPath m_path
The sequence of poses.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLdouble s
Definition: glext.h:3602
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
const_reverse_iterator rend() const
GLsizei samples
Definition: glext.h:6928
iterator upper_bound(const mrpt::system::TTimeStamp &t)
iterator find(const mrpt::system::TTimeStamp &t)
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
const_iterator upper_bound(const mrpt::system::TTimeStamp &t) const
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
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.
TPath::const_reverse_iterator const_reverse_iterator
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::poses::SE_traits< DIM >::point_t point_t
TPoint2D or TPoint3D.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
iterator erase(iterator element_to_erase)
mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
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...
const_iterator find(const mrpt::system::TTimeStamp &t) const
static void fill(bimap< enum_t, std::string > &m_map)
GLfloat GLfloat p
Definition: glext.h:5587
TPath::reverse_iterator reverse_iterator
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
GLenum filter
Definition: glext.h:4617
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
const_iterator lower_bound(const mrpt::system::TTimeStamp &t) const



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019