class mrpt::poses::CPose2DInterpolator
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses).
It can also interpolate SE(2) poses over time using linear, splines or SLERP interpolation, as set in CPose2DInterpolator::setInterpolationMethod() Usage:
Insert new poses into the sequence with CPose2DInterpolator::insert()
Query an exact/interpolated pose with CPose2DInterpolator::interpolate(). Example:
CPose2DInterpolator path; path.setInterpolationMethod( CPose2DInterpolator::imSplineSlerp ); path.insert( t0, mrpt::math::TPose2D(...) ); path.insert( t1, mrpt::math::TPose2D(...) ); mrpt::math::TPose2D p; bool valid; cout << "Pose at t: " << path.interpolate(t,p,valid).asString() << endl;
Time is represented with mrpt::Clock::time_point. See mrpt::system for methods and utilities to manage these time references.
See TInterpolatorMethod for the list of interpolation methods. The default method at constructor is “imLinearSlerp”.
See also:
#include <mrpt/poses/CPose2DInterpolator.h> class CPose2DInterpolator: public mrpt::serialization::CSerializable, public mrpt::poses::CPoseInterpolatorBase { public: // typedefs typedef typename Lie::SE<DIM>::light_type pose_t; typedef typename Lie::SE<DIM>::type cpose_t; typedef typename Lie::Euclidean<DIM>::light_type point_t; // methods void insert(const mrpt::Clock::time_point& t, const pose_t& p); void insert(const mrpt::Clock::time_point& t, const cpose_t& p); pose_t& interpolate(const mrpt::Clock::time_point& t, pose_t& out_interp, bool& out_valid_interp) const; void clear(); void setMaxTimeInterpolation(const mrpt::Clock::duration& time); mrpt::Clock::duration getMaxTimeInterpolation(); bool getPreviousPoseWithMinDistance(const mrpt::Clock::time_point& t, double distance, pose_t& out_pose); bool saveToTextFile(const std::string& s) const; bool saveToTextFile_TUM(const std::string& s) const; bool saveInterpolatedToTextFile(const std::string& s, const mrpt::Clock::duration& period) const; bool loadFromTextFile(const std::string& s); bool loadFromTextFile_TUM(const std::string& s); void getBoundingBox(point_t& minCorner, point_t& maxCorner) const; void setInterpolationMethod(TInterpolatorMethod method); TInterpolatorMethod getInterpolationMethod() const; void filter(unsigned int component, unsigned int samples); };
Inherited Members
public: // typedefs typedef std::pair<mrpt::Clock::time_point, pose_t> TTimePosePair; typedef std::map<mrpt::Clock::time_point, pose_t> TPath; typedef typename TPath::iterator iterator; typedef typename TPath::const_iterator const_iterator; typedef typename TPath::reverse_iterator reverse_iterator; typedef typename TPath::const_reverse_iterator const_reverse_iterator; // methods iterator begin(); const_iterator begin() const; const_iterator cbegin() const; iterator end(); const_iterator end() const; const_iterator cend() const; reverse_iterator rbegin(); const_reverse_iterator rbegin() const; reverse_iterator rend(); const_reverse_iterator rend() const; iterator lower_bound(const mrpt::Clock::time_point& t); const_iterator lower_bound(const mrpt::Clock::time_point& t) const; iterator upper_bound(const mrpt::Clock::time_point& t); const_iterator upper_bound(const mrpt::Clock::time_point& t) const; iterator erase(iterator element_to_erase); size_t size() const; bool empty() const; iterator find(const mrpt::Clock::time_point& t); const_iterator find(const mrpt::Clock::time_point& t) const; pose_t& at(const mrpt::Clock::time_point& t); const pose_t& at(const mrpt::Clock::time_point& t) const; cpose_t& interpolate(const mrpt::Clock::time_point& t, cpose_t& out_interp, bool& out_valid_interp) const; bool getPreviousPoseWithMinDistance(const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose);
Typedefs
typedef typename Lie::SE<DIM>::light_type pose_t
TPose2D or TPose3D.
typedef typename Lie::SE<DIM>::type cpose_t
CPose2D or CPose3D.
typedef typename Lie::Euclidean<DIM>::light_type point_t
TPoint2D or TPoint3D.
Methods
void insert(const mrpt::Clock::time_point& t, const pose_t& p)
Inserts a new pose in the sequence.
It overwrites any previously existing pose at exactly the same time.
void insert(const mrpt::Clock::time_point& t, const cpose_t& p)
Overload (slower)
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.
Parameters:
t |
The time of the point to interpolate. |
out_interp |
The output interpolated pose. |
out_valid_interp |
Whether there was information enough to compute the interpolation. |
Returns:
A reference to out_interp
void clear()
Clears the current sequence of poses.
void setMaxTimeInterpolation(const mrpt::Clock::duration& time)
Set value of the maximum time to consider interpolation.
If set to a negative value, the check is disabled (default behavior).
mrpt::Clock::duration getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
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.
Returns:
true if pose was found, false otherwise
bool saveToTextFile(const std::string& s) const
Saves the points in the interpolator to a text file, with this format: Each row contains these elements separated by spaces:
Timestamp: As a “double time_t” (see mrpt::system::timestampTotime_t).
x y z: The 3D position in meters.
yaw pitch roll: The angles, in radians
Returns:
true on success, false on any error.
See also:
bool saveToTextFile_TUM(const std::string& s) const
Saves the points in the interpolator to a text file in the “TUM” dataset format: each row contains these elements separated by spaces:
Timestamp: As a “double time_t” (see mrpt::system::timestampTotime_t).
x y z: The 3D position in meters.
q_x q_y q_z q_w: Quaternion
Returns:
true on success, false on any error.
See also:
loadFromTextFile, saveTextFile_TUM
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.
Returns:
true on success, false on any error.
See also:
bool loadFromTextFile(const std::string& s)
Loads from a text file, in the format described by saveToTextFile.
Parameters:
std::exception |
On invalid file format |
Returns:
true on success, false on any error.
bool loadFromTextFile_TUM(const std::string& s)
Loads from a text file, in the “TUM” dataset format.
Parameters:
std::exception |
On invalid file format |
Returns:
true on success, false on any error.
void getBoundingBox(point_t& minCorner, point_t& maxCorner) const
Computes the bounding box in all Euclidean coordinates of the whole path.
Parameters:
std::exception |
On empty path |
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
The default method at construction is “imSpline”.
See also:
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
See also:
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the pose data within the interpolator.
The width of the filter is set by the number of samples.
Parameters:
component |
[IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll) |
samples |
[IN] The width of the average filter. |