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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019