Main MRPT website > C++ reference for MRPT 1.9.9
CPose2DInterpolator.cpp
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 
10 #include "poses-precomp.h" // Precompiled headers
11 
13 #include "CPoseInterpolatorBase.hpp" // templ impl
15 
16 using namespace mrpt::poses;
17 
19 
20 uint8_t CPose2DInterpolator::serializeGetVersion() const { return 0; }
22 {
23  out << m_path;
24 }
27 {
28  switch (version)
29  {
30  case 0:
31  {
32  in >> m_path;
33  }
34  break;
35  default:
37  };
38 }
39 
40 namespace mrpt
41 {
42 namespace poses
43 {
44 // Specialization for DIM=2
45 template <>
47  const mrpt::math::CArrayDouble<4>& ts, const TTimePosePair p1,
48  const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4,
49  const TInterpolatorMethod method, double td, pose_t& out_interp) const
50 {
51  using mrpt::math::TPose2D;
53  X[0] = p1.second.x;
54  Y[0] = p1.second.y;
55  yaw[0] = p1.second.phi;
56  X[1] = p2.second.x;
57  Y[1] = p2.second.y;
58  yaw[1] = p2.second.phi;
59  X[2] = p3.second.x;
60  Y[2] = p3.second.y;
61  yaw[2] = p3.second.phi;
62  X[3] = p4.second.x;
63  Y[3] = p4.second.y;
64  yaw[3] = p4.second.phi;
65 
66  unwrap2PiSequence(yaw);
67 
68  // Target interpolated values:
69  switch (method)
70  {
71  case imSpline:
72  {
73  // ---------------------------------------
74  // SPLINE INTERPOLATION
75  // ---------------------------------------
76  out_interp.x = math::spline(td, ts, X);
77  out_interp.y = math::spline(td, ts, Y);
78  out_interp.phi = math::spline(td, ts, yaw, true); // Wrap 2pi
79  }
80  break;
81 
82  case imLinear2Neig:
83  {
84  out_interp.x =
85  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
86  out_interp.y =
87  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
88  out_interp.phi = math::interpolate2points(
89  td, ts[1], yaw[1], ts[2], yaw[2], true); // Wrap 2pi
90  }
91  break;
92 
93  case imLinear4Neig:
94  {
95  out_interp.x =
96  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
97  out_interp.y =
98  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
99  out_interp.phi =
100  math::leastSquareLinearFit<double, decltype(ts), 4>(
101  td, ts, yaw, true); // Wrap 2pi
102  }
103  break;
104 
105  case imSSLLLL:
106  {
107  out_interp.x = math::spline(td, ts, X);
108  out_interp.y = math::spline(td, ts, Y);
109  out_interp.phi =
110  math::leastSquareLinearFit<double, decltype(ts), 4>(
111  td, ts, yaw, true); // Wrap 2pi
112  }
113  break;
114 
115  case imSSLSLL:
116  {
117  out_interp.x = math::spline(td, ts, X);
118  out_interp.y = math::spline(td, ts, Y);
119  out_interp.phi = math::spline(td, ts, yaw, true); // Wrap 2pi
120  }
121  break;
122 
123  case imLinearSlerp:
124  {
125  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
126  const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
127  out_interp.phi = yaw[1] + ratio * Aang;
128 
129  out_interp.x =
130  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
131  out_interp.y =
132  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
133  }
134  break;
135 
136  case imSplineSlerp:
137  {
138  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
139  const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
140  out_interp.phi = yaw[1] + ratio * Aang;
141 
142  out_interp.x = math::spline(td, ts, X);
143  out_interp.y = math::spline(td, ts, Y);
144  }
145  break;
146 
147  default:
148  THROW_EXCEPTION("Unknown value for interpolation method!");
149  }; // end switch
150 }
151 
152 // Explicit instantations:
153 template class CPoseInterpolatorBase<2>;
154 }
155 }
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:98
typename mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void unwrap2PiSequence(VECTOR &x)
Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolut...
Definition: wrap2pi.h:74
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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...
Definition: interp_fit.hpp:36
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
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).
Definition: math.cpp:436
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...



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