Go to the documentation of this file.
55 yaw[0] = p1.second.phi;
58 yaw[1] = p2.second.phi;
61 yaw[2] = p3.second.phi;
64 yaw[3] = p4.second.phi;
89 td, ts[1], yaw[1], ts[2], yaw[2],
true);
96 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
98 math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
100 math::leastSquareLinearFit<double, decltype(ts), 4>(
110 math::leastSquareLinearFit<double, decltype(ts), 4>(
125 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
127 out_interp.phi = yaw[1] + ratio * Aang;
138 const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
140 out_interp.phi = yaw[1] + ratio * Aang;
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
TPath m_path
The sequence of poses.
NUMTYPE spline(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi=false)
Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the tw...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
typename mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define THROW_EXCEPTION(msg)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Virtual base class for "archives": classes abstracting I/O streams.
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes.
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses).
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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
double interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
void unwrap2PiSequence(VECTOR &x)
Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolut...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |