Main MRPT website > C++ reference for MRPT 1.4.0
CPose3DInterpolator.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-2016, 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#ifndef CPose3DInterpolator_H
10#define CPose3DInterpolator_H
11
17#include <mrpt/poses/CPose3D.h>
18
19namespace mrpt
20{
21 namespace poses
22 {
24
25 typedef std::pair<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> TTimePosePair;
26
27 /** This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
28 * It can also interpolate SE(3) poses over time using linear, splines or SLERP interpolation, as set in CPose3DInterpolator::setInterpolationMethod()
29 * Usage:
30 * - Insert new poses into the sequence with CPose3DInterpolator::insert()
31 * - Query an exact/interpolated pose with CPose3DInterpolator::interpolate().
32 * Example:
33 * \code
34 * CPose3DInterpolator path;
35 *
36 * path.setInterpolationMethod( CPose3DInterpolator::imSplineSlerp );
37 *
38 * path.insert( t0, CPose3D(...) );
39 * path.insert( t1, CPose3D(...) );
40 * path.insert( t2, CPose3D(...) );
41 * path.insert( t3, CPose3D(...) );
42 *
43 * CPose3D p;
44 * bool valid;
45 *
46 * cout << "Pose at t: " << path.interpolate(t,p,valid) << endl;
47 * \endcode
48 *
49 * Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references.
50 *
51 * See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is "imLinearSlerp".
52 *
53 * \sa CPoseOrPoint
54 * \ingroup interpolation_grp poses_grp
55 */
56 class BASE_IMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable
57 {
58 // This must be added to any CSerializable derived class:
60
61 private:
63 TPath m_path; //!< The sequence of poses
64
65 public:
66 typedef TPath::iterator iterator;
67 typedef TPath::const_iterator const_iterator;
68 typedef TPath::reverse_iterator reverse_iterator;
69 typedef TPath::const_reverse_iterator const_reverse_iterator;
70
71 /** Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod
72 * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point).
73 * - imLinear2Neig: Linear interpolation between the previous and next neightbour.
74 * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point).
75 * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll.
76 * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll.
77 * - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
78 * - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
79 */
81 {
82 imSpline = 0,
88 imSplineSlerp
89 };
90
91 inline iterator begin() { return m_path.begin(); }
92 inline const_iterator begin() const { return m_path.begin(); }
93
94 inline iterator end() { return m_path.end(); }
95 inline const_iterator end() const { return m_path.end(); }
96
97 inline reverse_iterator rbegin() { return m_path.rbegin(); }
98 inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
99
100 inline reverse_iterator rend() { return m_path.rend(); }
101 inline const_reverse_iterator rend() const { return m_path.rend(); }
102
103 iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
104 const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
105
106 iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
107 const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
108
109 iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
110
111 size_t size() const { return m_path.size(); }
112 bool empty() const { return m_path.empty(); }
113
114 /** Creates an empty interpolator (with no points).
115 */
117
118 /** Inserts a new pose in the sequence.
119 * It overwrites any previously existing pose at exactly the same time.
120 */
122
123 /** Returns the pose at a given time, or interpolates using splines if there is not an exact match.
124 * \param t The time of the point to interpolate.
125 * \param out_interp The output interpolated pose.
126 * \param out_valid_interp Whether there was information enough to compute the interpolation.
127 * \return A reference to out_interp
128 */
129 CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const;
130
131 /** Clears the current sequence of poses */
132 void clear();
133
134 /** Set value of the maximum time to consider interpolation.
135 * If set to a negative value, the check is disabled (default behavior).
136 */
137 void setMaxTimeInterpolation( double time );
138
139 /** Set value of the maximum time to consider interpolation */
141
142 /** Get the previous CPose3D in the map with a minimum defined distance
143 * \return true if pose was found, false otherwise.
144 */
145 bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose );
146
147 /** Saves the points in the interpolator to a text file, with this format:
148 * Each row contains these elements separated by spaces:
149 * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
150 * - x y z: The 3D position in meters.
151 * - yaw pitch roll: The angles, in radians
152 * \sa loadFromTextFile
153 * \return true on success, false on any error.
154 */
155 bool saveToTextFile(const std::string &s) const;
156
157 /** 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.
158 * \sa loadFromTextFile
159 * \return true on success, false on any error.
160 */
161 bool saveInterpolatedToTextFile(const std::string &s, double period) const;
162
163 /** Loads from a text file, in the format described by saveToTextFile.
164 * \return true on success, false on any error.
165 * \exception std::exception On invalid file format
166 */
167 bool loadFromTextFile(const std::string &s);
168
169 /** Computes the bounding box in X,Y,Z of the whole vehicle path.
170 * \exception std::exception On empty path
171 */
172 void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const;
173
174 /** Computes the bounding box in X,Y,Z of the whole vehicle path.
175 * \exception std::exception On empty path
176 */
177 void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const;
178
179 /** Change the method used to interpolate the robot path.
180 * The default method at construction is "imSpline".
181 * \sa getInterpolationMethod
182 */
184
185 /** Returns the currently set interpolation method.
186 * \sa setInterpolationMethod
187 */
189
190 /** Filters by averaging one of the components of the CPose3D data within the interpolator. The width of the filter is set by the number of samples.
191 * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll).
192 * \param samples [IN] The width of the average filter.
193 */
194 void filter( unsigned int component, unsigned int samples );
195
196
197 private:
198 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.
199
201
202 }; // End of class def.
203 DEFINE_SERIALIZABLE_POST_CUSTOM_BASE( CPose3DInterpolator, mrpt::utils::CSerializable )
204
205 } // End of namespace
206
207 // Specializations MUST occur at the same namespace:
208 namespace utils
209 {
210 template <>
211 struct TEnumTypeFiller<poses::CPose3DInterpolator::TInterpolatorMethod>
212 {
214 static void fill(bimap<enum_t,std::string> &m_map)
215 {
222 }
223 };
224 } // End of namespace
225} // End of namespace
226
227#endif
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
bool getPreviousPoseWithMinDistance(const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose)
Get the previous CPose3D in the map with a minimum defined distance.
double getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
void setMaxTimeInterpolation(double time)
Set value of the maximum time to consider interpolation.
const_iterator lower_bound(const mrpt::system::TTimeStamp &t) const
iterator upper_bound(const mrpt::system::TTimeStamp &t)
CPose3D & interpolate(mrpt::system::TTimeStamp t, CPose3D &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.
void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const
Computes the bounding box in X,Y,Z of the whole vehicle path.
TInterpolatorMethod
Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod.
const_reverse_iterator rbegin() const
TPath::const_iterator const_iterator
mrpt::aligned_containers< mrpt::system::TTimeStamp, CPose3D >::map_t TPath
double maxTimeInterpolation
Maximum time considered to interpolate. If the difference between the desired timestamp where to inte...
void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const
Computes the bounding box in X,Y,Z of the whole vehicle path.
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
TPath::reverse_iterator reverse_iterator
void clear()
Clears the current sequence of poses.
TPath::const_reverse_iterator const_reverse_iterator
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,...
iterator erase(iterator element_to_erase)
iterator lower_bound(const mrpt::system::TTimeStamp &t)
void insert(mrpt::system::TTimeStamp t, const CPose3D &p)
Inserts a new pose in the sequence.
CPose3DInterpolator()
Creates an empty interpolator (with no points).
const_reverse_iterator rend() const
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the CPose3D data within the interpolator.
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by 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...
const_iterator upper_bound(const mrpt::system::TTimeStamp &t) const
TPath m_path
The sequence of poses.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:30
std::pair< mrpt::system::TTimeStamp, mrpt::poses::CPose3D > TTimePosePair
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
Lightweight 3D point.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:59 UTC 2022