MRPT  2.0.4
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
mrpt::poses::CPoseInterpolatorBase< 3 >::point_t
typename Lie::Euclidean< DIM >::light_type point_t
TPoint2D or TPoint3D.
Definition: CPoseInterpolatorBase.h:64
mrpt::poses::CPoseInterpolatorBase::size
size_t size() const
Definition: CPoseInterpolatorBase.h:107
mrpt::poses::CPoseInterpolatorBase::cbegin
const_iterator cbegin() const
Definition: CPoseInterpolatorBase.h:75
mrpt::poses::CPoseInterpolatorBase::find
iterator find(const mrpt::Clock::time_point &t)
Definition: CPoseInterpolatorBase.h:109
mrpt::poses::CPoseInterpolatorBase::loadFromTextFile
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by saveToTextFile.
Definition: CPoseInterpolatorBase.hpp:303
mrpt::poses::CPoseInterpolatorBase< 3 >::reverse_iterator
typename TPath::reverse_iterator reverse_iterator
Definition: CPoseInterpolatorBase.h:70
mrpt::Clock::duration
std::chrono::duration< rep, period > duration
Definition: Clock.h:24
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
SE.h
mrpt::poses::CPoseInterpolatorBase::end
iterator end()
Definition: CPoseInterpolatorBase.h:76
mrpt::poses::CPoseInterpolatorBase< 3 >::pose_t
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
Definition: CPoseInterpolatorBase.h:60
mrpt::poses::CPoseInterpolatorBase
Base class for SE(2)/SE(3) interpolators.
Definition: CPoseInterpolatorBase.h:50
mrpt::poses::CPoseInterpolatorBase::end
const_iterator end() const
Definition: CPoseInterpolatorBase.h:77
mrpt::poses::CPoseInterpolatorBase::impl_interpolation
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
mrpt::poses::CPoseInterpolatorBase::m_path
TPath m_path
The sequence of poses.
Definition: CPoseInterpolatorBase.h:207
mrpt::poses::imSplineSlerp
@ imSplineSlerp
Definition: CPoseInterpolatorBase.h:43
mrpt::poses::CPoseInterpolatorBase< 3 >::cpose_t
typename Lie::SE< DIM >::type cpose_t
CPose2D or CPose3D.
Definition: CPoseInterpolatorBase.h:62
mrpt::poses::CPoseInterpolatorBase::getPreviousPoseWithMinDistance
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.
Definition: CPoseInterpolatorBase.hpp:191
mrpt::poses::CPoseInterpolatorBase::CPoseInterpolatorBase
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
Definition: CPoseInterpolatorBase.hpp:28
mrpt::poses::CPoseInterpolatorBase::at
const pose_t & at(const mrpt::Clock::time_point &t) const
Definition: CPoseInterpolatorBase.h:115
mrpt::poses::CPoseInterpolatorBase::erase
iterator erase(iterator element_to_erase)
Definition: CPoseInterpolatorBase.h:101
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
mrpt::poses::CPoseInterpolatorBase::m_method
TInterpolatorMethod m_method
Definition: CPoseInterpolatorBase.h:212
mrpt::poses::CPoseInterpolatorBase< 3 >::iterator
typename TPath::iterator iterator
Definition: CPoseInterpolatorBase.h:68
mrpt::poses::CPoseInterpolatorBase::interpolate
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.
Definition: CPoseInterpolatorBase.hpp:70
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSpline)
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::imLinearSlerp
@ imLinearSlerp
Definition: CPoseInterpolatorBase.h:42
mrpt::poses::imLinear4Neig
@ imLinear4Neig
Definition: CPoseInterpolatorBase.h:39
mrpt::poses::CPoseInterpolatorBase::setInterpolationMethod
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
Definition: CPoseInterpolatorBase.hpp:361
mrpt::poses::CPoseInterpolatorBase::begin
const_iterator begin() const
Definition: CPoseInterpolatorBase.h:74
mrpt::poses::CPoseInterpolatorBase::rbegin
reverse_iterator rbegin()
Definition: CPoseInterpolatorBase.h:79
mrpt::poses::CPoseInterpolatorBase::saveToTextFile
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...
Definition: CPoseInterpolatorBase.hpp:236
mrpt::poses::CPoseInterpolatorBase::insert
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
Definition: CPoseInterpolatorBase.hpp:47
mrpt::poses::TInterpolatorMethod
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes.
Definition: CPoseInterpolatorBase.h:35
mrpt::poses::imLinear2Neig
@ imLinear2Neig
Definition: CPoseInterpolatorBase.h:38
mrpt::poses::CPoseInterpolatorBase::rend
reverse_iterator rend()
Definition: CPoseInterpolatorBase.h:81
mrpt::poses::CPoseInterpolatorBase::find
const_iterator find(const mrpt::Clock::time_point &t) const
Definition: CPoseInterpolatorBase.h:110
TEnumType.h
mrpt::poses::CPoseInterpolatorBase::clear
void clear()
Clears the current sequence of poses.
Definition: CPoseInterpolatorBase.hpp:35
Euclidean.h
mrpt::poses::CPoseInterpolatorBase< 3 >::const_iterator
typename TPath::const_iterator const_iterator
Definition: CPoseInterpolatorBase.h:69
mrpt::poses::CPoseInterpolatorBase::getInterpolationMethod
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
Definition: CPoseInterpolatorBase.hpp:368
mrpt::Clock::time_point
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
mrpt::poses::CPoseInterpolatorBase::lower_bound
iterator lower_bound(const mrpt::Clock::time_point &t)
Definition: CPoseInterpolatorBase.h:83
mrpt::poses::Lie::SE
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
mrpt::poses::imSpline
@ imSpline
Definition: CPoseInterpolatorBase.h:37
mrpt::poses::CPoseInterpolatorBase::cend
const_iterator cend() const
Definition: CPoseInterpolatorBase.h:78
mrpt::poses::CPoseInterpolatorBase::begin
iterator begin()
Definition: CPoseInterpolatorBase.h:73
mrpt::poses::CPoseInterpolatorBase::getBoundingBox
void getBoundingBox(point_t &minCorner, point_t &maxCorner) const
Computes the bounding box in all Euclidean coordinates of the whole path.
Definition: CPoseInterpolatorBase.hpp:337
mrpt::poses::imSSLLLL
@ imSSLLLL
Definition: CPoseInterpolatorBase.h:40
mrpt::poses::CPoseInterpolatorBase::lower_bound
const_iterator lower_bound(const mrpt::Clock::time_point &t) const
Definition: CPoseInterpolatorBase.h:87
mrpt::poses::CPoseInterpolatorBase::empty
bool empty() const
Definition: CPoseInterpolatorBase.h:108
poses_frwds.h
mrpt::poses::CPoseInterpolatorBase::upper_bound
iterator upper_bound(const mrpt::Clock::time_point &t)
Definition: CPoseInterpolatorBase.h:92
mrpt::poses::Lie::Euclidean
Traits for Euclidean R^N space.
Definition: Euclidean.h:22
mrpt::poses::imSSLSLL
@ imSSLSLL
Definition: CPoseInterpolatorBase.h:41
mrpt::poses::CPoseInterpolatorBase::rbegin
const_reverse_iterator rbegin() const
Definition: CPoseInterpolatorBase.h:80
Clock.h
mrpt::poses::CPoseInterpolatorBase::filter
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the pose data within the interpolator.
Definition: CPoseInterpolatorBase.hpp:374
mrpt::poses::CPoseInterpolatorBase::maxTimeInterpolation
mrpt::Clock::duration maxTimeInterpolation
Maximum time considered to interpolate.
Definition: CPoseInterpolatorBase.h:211
mrpt::poses::CPoseInterpolatorBase::getMaxTimeInterpolation
mrpt::Clock::duration getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
Definition: CPoseInterpolatorBase.hpp:230
mrpt::poses::CPoseInterpolatorBase::rend
const_reverse_iterator rend() const
Definition: CPoseInterpolatorBase.h:82
mrpt::poses::CPoseInterpolatorBase< 3 >::const_reverse_iterator
typename TPath::const_reverse_iterator const_reverse_iterator
Definition: CPoseInterpolatorBase.h:71
mrpt::poses::CPoseInterpolatorBase::saveInterpolatedToTextFile
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,...
Definition: CPoseInterpolatorBase.hpp:265
mrpt::poses::CPoseInterpolatorBase< 3 >::TPath
std::map< mrpt::Clock::time_point, pose_t > TPath
Definition: CPoseInterpolatorBase.h:67
mrpt::poses::CPoseInterpolatorBase::upper_bound
const_iterator upper_bound(const mrpt::Clock::time_point &t) const
Definition: CPoseInterpolatorBase.h:96
mrpt::poses::CPoseInterpolatorBase< 3 >::TTimePosePair
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
Definition: CPoseInterpolatorBase.h:66
mrpt::poses::CPoseInterpolatorBase::at
pose_t & at(const mrpt::Clock::time_point &t)
Definition: CPoseInterpolatorBase.h:114
mrpt::poses::CPoseInterpolatorBase::setMaxTimeInterpolation
void setMaxTimeInterpolation(const mrpt::Clock::duration &time)
Set value of the maximum time to consider interpolation.
Definition: CPoseInterpolatorBase.hpp:222



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020