MRPT  1.9.9
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-2018, 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/core/Clock.h>
13 #include <mrpt/poses/SE_traits.h>
15 #include <mrpt/poses/poses_frwds.h>
16 
17 namespace mrpt::poses
18 {
19 /** Type to select the interpolation method in CPoseInterpolatorBase derived
20  * classes.
21  * - imSpline: Spline interpolation using 4 points (2 before + 2 after the
22  * query point).
23  * - imLinear2Neig: Linear interpolation between the previous and next
24  * neightbour.
25  * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer
26  * points (2 before + 2 after the query point).
27  * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw,
28  * pitch and roll.
29  * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z,
30  * pitch and roll.
31  * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
32  * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
33  * \ingroup interpolation_grp poses_grp
34  */
36 {
37  imSpline = 0,
44 };
45 
46 /** Base class for SE(2)/SE(3) interpolators. See docs for derived classes.
47 * \ingroup interpolation_grp poses_grp
48 */
49 template <int DIM>
51 {
52  public:
53  /** Default ctor: empty sequence of poses */
55 
56  /** @name Type definitions and STL-like container interface
57  * @{ */
58 
59  /** TPose2D or TPose3D */
61  /** CPose2D or CPose3D */
63  /** TPoint2D or TPoint3D */
65 
66  using TTimePosePair = std::pair<mrpt::Clock::time_point, pose_t>;
67  using TPath = std::map<mrpt::Clock::time_point, pose_t>;
68  using iterator = typename TPath::iterator;
70  using reverse_iterator = typename TPath::reverse_iterator;
71  using const_reverse_iterator = typename TPath::const_reverse_iterator;
72 
73  inline iterator begin() { return m_path.begin(); }
74  inline const_iterator begin() const { return m_path.begin(); }
75  inline const_iterator cbegin() const { return m_path.cbegin(); }
76  inline iterator end() { return m_path.end(); }
77  inline const_iterator end() const { return m_path.end(); }
78  inline const_iterator cend() const { return m_path.cend(); }
79  inline reverse_iterator rbegin() { return m_path.rbegin(); }
80  inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
81  inline reverse_iterator rend() { return m_path.rend(); }
82  inline const_reverse_iterator rend() const { return m_path.rend(); }
84  {
85  return m_path.lower_bound(t);
86  }
88  {
89  return m_path.lower_bound(t);
90  }
91 
93  {
94  return m_path.upper_bound(t);
95  }
97  {
98  return m_path.upper_bound(t);
99  }
100 
101  iterator erase(iterator element_to_erase)
102  {
103  m_path.erase(element_to_erase++);
104  return element_to_erase;
105  }
106 
107  size_t size() const { return m_path.size(); }
108  bool empty() const { return m_path.empty(); }
109  iterator find(const mrpt::Clock::time_point& t) { return m_path.find(t); }
111  {
112  return m_path.find(t);
113  }
114  /** @} */
115 
116  /** Inserts a new pose in the sequence.
117  * It overwrites any previously existing pose at exactly the same time.
118  */
119  void insert(const mrpt::Clock::time_point &t, const pose_t& p);
120  /** Overload (slower) */
121  void insert(const mrpt::Clock::time_point &t, const cpose_t& p);
122 
123  /** Returns the pose at a given time, or interpolates using splines if there
124  * is not an exact match.
125  * \param t The time of the point to interpolate.
126  * \param out_interp The output interpolated pose.
127  * \param out_valid_interp Whether there was information enough to compute
128  * the interpolation.
129  * \return A reference to out_interp
130  */
132  const mrpt::Clock::time_point &t, pose_t& out_interp,
133  bool& out_valid_interp) const;
134  /** \overload (slower) */
136  const mrpt::Clock::time_point &t, cpose_t& out_interp,
137  bool& out_valid_interp) const;
138 
139  /** Clears the current sequence of poses */
140  void clear();
141 
142  /** Set value of the maximum time to consider interpolation.
143  * If set to a negative value, the check is disabled (default behavior). */
145  /** Set value of the maximum time to consider interpolation */
147 
148  /** Get the previous CPose3D in the map with a minimum defined distance.
149  * \return true if pose was found, false otherwise */
151  const mrpt::Clock::time_point& t, double distance, pose_t& out_pose);
152  /** \overload (slower) */
154  const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose);
155 
156  /** Saves the points in the interpolator to a text file, with this format:
157  * Each row contains these elements separated by spaces:
158  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
159  * - x y z: The 3D position in meters.
160  * - yaw pitch roll: The angles, in radians
161  * \sa loadFromTextFile
162  * \return true on success, false on any error.
163  */
164  bool saveToTextFile(const std::string& s) const;
165 
166  /** Saves the points in the interpolator to a text file, with the same
167  * format that saveToTextFile, but interpolating the path with the given
168  * period in seconds.
169  * \sa loadFromTextFile
170  * \return true on success, false on any error.
171  */
172  bool saveInterpolatedToTextFile(const std::string& s, const mrpt::Clock::duration &period) const;
173 
174  /** Loads from a text file, in the format described by saveToTextFile.
175  * \return true on success, false on any error.
176  * \exception std::exception On invalid file format
177  */
178  bool loadFromTextFile(const std::string& s);
179 
180  /** Computes the bounding box in all Euclidean coordinates of the whole
181  * path. \exception std::exception On empty path */
182  void getBoundingBox(point_t& minCorner, point_t& maxCorner) const;
183 
184  /** Change the method used to interpolate the robot path. The default method
185  * at construction is "imSpline". \sa getInterpolationMethod() */
187  /** Returns the currently set interpolation method. \sa
188  * setInterpolationMethod() */
190 
191  /** Filters by averaging one of the components of the pose data within the
192  * interpolator. The width of the filter is set by the number of samples.
193  * \param component [IN] The index of the component to filter: 0 (x),
194  * 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll)
195  * \param samples [IN] The width of the average filter.
196  */
197  void filter(unsigned int component, unsigned int samples);
198 
199  protected:
200  /** The sequence of poses */
202  /** Maximum time considered to interpolate. If the difference between the
203  * desired timestamp where to interpolate and the next timestamp stored in
204  * the map is bigger than this value, the interpolation will not be done. */
207 
208  void impl_interpolation(const TTimePosePair &p1, const TTimePosePair &p2,
209  const TTimePosePair &p3, const TTimePosePair &p4,
210  const TInterpolatorMethod method, const mrpt::Clock::time_point &td,
211  pose_t& out_interp) const;
212 
213 }; // End of class def.
214 }
223 
224 
const_iterator find(const mrpt::Clock::time_point &t) const
mrpt::Clock::duration getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
std::chrono::duration< rep, period > duration
Definition: Clock.h:25
mrpt::Clock::duration maxTimeInterpolation
Maximum time considered to interpolate.
Scalar * iterator
Definition: eigen_plugins.h:26
GLdouble GLdouble t
Definition: glext.h:3689
const_iterator lower_bound(const mrpt::Clock::time_point &t) const
Base class for SE(2)/SE(3) interpolators.
std::chrono::time_point< Clock > time_point
Definition: Clock.h:26
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
const_reverse_iterator rbegin() const
TPath m_path
The sequence of poses.
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...
GLdouble s
Definition: glext.h:3676
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the pose data within the interpolator.
typename mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
typename TPath::const_reverse_iterator const_reverse_iterator
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSpline)
const_reverse_iterator rend() const
GLsizei samples
Definition: glext.h:8068
iterator lower_bound(const mrpt::Clock::time_point &t)
typename TPath::reverse_iterator reverse_iterator
std::map< mrpt::Clock::time_point, pose_t > TPath
void getBoundingBox(point_t &minCorner, point_t &maxCorner) const
Computes the bounding box in all Euclidean coordinates of the whole path.
typename mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by saveToTextFile.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
iterator upper_bound(const mrpt::Clock::time_point &t)
void setMaxTimeInterpolation(const mrpt::Clock::duration &time)
Set value of the maximum time to consider interpolation.
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &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...
const_iterator upper_bound(const mrpt::Clock::time_point &t) const
void clear()
Clears the current sequence of poses.
typename mrpt::poses::SE_traits< DIM >::point_t point_t
TPoint2D or TPoint3D.
typename TPath::const_iterator const_iterator
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
Definition: SE_traits.h:27
void impl_interpolation(const TTimePosePair &p1, const TTimePosePair &p2, const TTimePosePair &p3, const TTimePosePair &p4, const TInterpolatorMethod method, const mrpt::Clock::time_point &td, pose_t &out_interp) const
iterator erase(iterator element_to_erase)
iterator find(const mrpt::Clock::time_point &t)
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
bool getPreviousPoseWithMinDistance(const mrpt::Clock::time_point &t, double distance, pose_t &out_pose)
Get the previous CPose3D in the map with a minimum defined distance.
bool saveInterpolatedToTextFile(const std::string &s, const mrpt::Clock::duration &period) const
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.
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020