MRPT  2.0.1
CPoseInterpolatorBase.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/Clock.h>
13 #include <mrpt/poses/Lie/SE.h>
14 #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 */
60  using pose_t = typename Lie::SE<DIM>::light_type;
61  /** CPose2D or CPose3D */
62  using cpose_t = typename Lie::SE<DIM>::type;
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;
69  using const_iterator = typename TPath::const_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  pose_t& at(const mrpt::Clock::time_point& t) { return m_path.at(t); }
115  const pose_t& at(const mrpt::Clock::time_point& t) const
116  {
117  return m_path.at(t);
118  }
119  /** @} */
120 
121  /** Inserts a new pose in the sequence.
122  * It overwrites any previously existing pose at exactly the same time.
123  */
124  void insert(const mrpt::Clock::time_point& t, const pose_t& p);
125  /** Overload (slower) */
126  void insert(const mrpt::Clock::time_point& t, const cpose_t& p);
127 
128  /** Returns the pose at a given time, or interpolates using splines if there
129  * is not an exact match.
130  * \param t The time of the point to interpolate.
131  * \param out_interp The output interpolated pose.
132  * \param out_valid_interp Whether there was information enough to compute
133  * the interpolation.
134  * \return A reference to out_interp
135  */
137  const mrpt::Clock::time_point& t, pose_t& out_interp,
138  bool& out_valid_interp) const;
139  /** \overload (slower) */
141  const mrpt::Clock::time_point& t, cpose_t& out_interp,
142  bool& out_valid_interp) const;
143 
144  /** Clears the current sequence of poses */
145  void clear();
146 
147  /** Set value of the maximum time to consider interpolation.
148  * If set to a negative value, the check is disabled (default behavior). */
150  /** Set value of the maximum time to consider interpolation */
152 
153  /** Get the previous CPose3D in the map with a minimum defined distance.
154  * \return true if pose was found, false otherwise */
156  const mrpt::Clock::time_point& t, double distance, pose_t& out_pose);
157  /** \overload (slower) */
159  const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose);
160 
161  /** Saves the points in the interpolator to a text file, with this format:
162  * Each row contains these elements separated by spaces:
163  * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
164  * - x y z: The 3D position in meters.
165  * - yaw pitch roll: The angles, in radians
166  * \sa loadFromTextFile
167  * \return true on success, false on any error.
168  */
169  bool saveToTextFile(const std::string& s) const;
170 
171  /** Saves the points in the interpolator to a text file, with the same
172  * format that saveToTextFile, but interpolating the path with the given
173  * period in seconds.
174  * \sa loadFromTextFile
175  * \return true on success, false on any error.
176  */
178  const std::string& s, const mrpt::Clock::duration& period) const;
179 
180  /** Loads from a text file, in the format described by saveToTextFile.
181  * \return true on success, false on any error.
182  * \exception std::exception On invalid file format
183  */
184  bool loadFromTextFile(const std::string& s);
185 
186  /** Computes the bounding box in all Euclidean coordinates of the whole
187  * path. \exception std::exception On empty path */
188  void getBoundingBox(point_t& minCorner, point_t& maxCorner) const;
189 
190  /** Change the method used to interpolate the robot path. The default method
191  * at construction is "imSpline". \sa getInterpolationMethod() */
193  /** Returns the currently set interpolation method. \sa
194  * setInterpolationMethod() */
196 
197  /** Filters by averaging one of the components of the pose data within the
198  * interpolator. The width of the filter is set by the number of samples.
199  * \param component [IN] The index of the component to filter: 0 (x),
200  * 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll)
201  * \param samples [IN] The width of the average filter.
202  */
203  void filter(unsigned int component, unsigned int samples);
204 
205  protected:
206  /** The sequence of poses */
208  /** Maximum time considered to interpolate. If the difference between the
209  * desired timestamp where to interpolate and the next timestamp stored in
210  * the map is bigger than this value, the interpolation will not be done. */
213 
214  void impl_interpolation(
215  const TTimePosePair& p1, const TTimePosePair& p2,
216  const TTimePosePair& p3, const TTimePosePair& p4,
217  const TInterpolatorMethod method, const mrpt::Clock::time_point& td,
218  pose_t& out_interp) const;
219 
220 }; // End of class def.
221 } // namespace mrpt::poses
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:24
mrpt::Clock::duration maxTimeInterpolation
Maximum time considered to interpolate.
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:25
typename Lie::Euclidean< DIM >::light_type point_t
TPoint2D or TPoint3D.
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...
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 TPath::const_reverse_iterator const_reverse_iterator
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSpline)
const_reverse_iterator rend() const
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
typename Lie::SE< DIM >::type cpose_t
CPose2D or CPose3D.
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.
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
Traits for Euclidean R^N space.
Definition: Euclidean.h:22
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)
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
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 TPath::const_iterator const_iterator
const pose_t & at(const mrpt::Clock::time_point &t) const
pose_t & at(const mrpt::Clock::time_point &t)
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
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:1807



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020