MRPT  2.0.4
CPose2DInterpolator.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
14 #include <Eigen/Dense>
15 #include "CPoseInterpolatorBase.hpp" // templ impl
16 
17 using namespace mrpt::poses;
18 
20 
21 uint8_t CPose2DInterpolator::serializeGetVersion() const { return 0; }
23 {
24  out << m_path;
25 }
27  mrpt::serialization::CArchive& in, uint8_t version)
28 {
29  switch (version)
30  {
31  case 0:
32  {
33  in >> m_path;
34  }
35  break;
36  default:
38  };
39 }
40 
41 namespace mrpt::poses
42 {
43 // Specialization for DIM=2
44 template <>
46  const TTimePosePair& p1, const TTimePosePair& p2, const TTimePosePair& p3,
47  const TTimePosePair& p4, const TInterpolatorMethod method,
48  const mrpt::Clock::time_point& t, pose_t& out_interp) const
49 {
50  using mrpt::math::TPose2D;
51  using doubleDuration = std::chrono::duration<double>;
52  doubleDuration durationT(t.time_since_epoch());
53  double td = durationT.count();
55  ts[0] =
56  std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
57  .count();
58  ts[1] =
59  std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
60  .count();
61  ts[2] =
62  std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
63  .count();
64  ts[3] =
65  std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
66  .count();
67 
69  X[0] = p1.second.x;
70  Y[0] = p1.second.y;
71  yaw[0] = p1.second.phi;
72  X[1] = p2.second.x;
73  Y[1] = p2.second.y;
74  yaw[1] = p2.second.phi;
75  X[2] = p3.second.x;
76  Y[2] = p3.second.y;
77  yaw[2] = p3.second.phi;
78  X[3] = p4.second.x;
79  Y[3] = p4.second.y;
80  yaw[3] = p4.second.phi;
81 
82  unwrap2PiSequence(yaw);
83 
84  // Target interpolated values:
85  switch (method)
86  {
87  case imSpline:
88  {
89  // ---------------------------------------
90  // SPLINE INTERPOLATION
91  // ---------------------------------------
92  out_interp.x = math::spline(td, ts, X);
93  out_interp.y = math::spline(td, ts, Y);
94  out_interp.phi = math::spline(td, ts, yaw, true); // Wrap 2pi
95  }
96  break;
97 
98  case imLinear2Neig:
99  {
100  out_interp.x =
101  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
102  out_interp.y =
103  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
104  out_interp.phi = math::interpolate2points(
105  td, ts[1], yaw[1], ts[2], yaw[2], true); // Wrap 2pi
106  }
107  break;
108 
109  case imLinear4Neig:
110  {
111  out_interp.x =
112  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
113  out_interp.y =
114  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
115  out_interp.phi =
116  math::leastSquareLinearFit<double, decltype(ts), 4>(
117  td, ts, yaw, true); // Wrap 2pi
118  }
119  break;
120 
121  case imSSLLLL:
122  {
123  out_interp.x = math::spline(td, ts, X);
124  out_interp.y = math::spline(td, ts, Y);
125  out_interp.phi =
126  math::leastSquareLinearFit<double, decltype(ts), 4>(
127  td, ts, yaw, true); // Wrap 2pi
128  }
129  break;
130 
131  case imSSLSLL:
132  {
133  out_interp.x = math::spline(td, ts, X);
134  out_interp.y = math::spline(td, ts, Y);
135  out_interp.phi = math::spline(td, ts, yaw, true); // Wrap 2pi
136  }
137  break;
138 
139  case imLinearSlerp:
140  {
141  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
142  const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
143  out_interp.phi = yaw[1] + ratio * Aang;
144 
145  out_interp.x =
146  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
147  out_interp.y =
148  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
149  }
150  break;
151 
152  case imSplineSlerp:
153  {
154  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
155  const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
156  out_interp.phi = yaw[1] + ratio * Aang;
157 
158  out_interp.x = math::spline(td, ts, X);
159  out_interp.y = math::spline(td, ts, Y);
160  }
161  break;
162 
163  default:
164  THROW_EXCEPTION("Unknown value for interpolation method!");
165  }; // end switch
166 }
167 
168 // Explicit instantations:
169 template class CPoseInterpolatorBase<2>;
170 } // namespace mrpt::poses
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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:95
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
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...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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:71
mrpt::vision::TStereoCalibResults out
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:34
Lightweight 2D pose.
Definition: TPose2D.h:22
void impl_interpolation(const TTimePosePair &p1, const TTimePosePair &p2, const TTimePosePair &p3, const TTimePosePair &p4, const TInterpolatorMethod method, const mrpt::Clock::time_point &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).
Definition: math.cpp:438
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020