MRPT  2.0.1
CPose3DInterpolator.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 CPose3DInterpolator::serializeGetVersion() const { return 1; }
23 {
24  out << m_path; // v1: change container element CPose3D->TPose3D
25 }
27  mrpt::serialization::CArchive& in, uint8_t version)
28 {
29  switch (version)
30  {
31  case 0:
32  {
33  std::map<mrpt::Clock::time_point, mrpt::poses::CPose3D> old_path;
34  in >> old_path;
35  m_path.clear();
36  for (const auto& p : old_path)
37  {
38  m_path[p.first] = p.second.asTPose();
39  }
40  }
41  break;
42  case 1:
43  {
44  in >> m_path;
45  }
46  break;
47  default:
49  };
50 }
51 
52 namespace mrpt::poses
53 {
54 // Specialization for DIM=3
55 template <>
57  const TTimePosePair& p1, const TTimePosePair& p2, const TTimePosePair& p3,
58  const TTimePosePair& p4, const TInterpolatorMethod method,
59  const mrpt::Clock::time_point& t, pose_t& out_interp) const
60 {
61  using mrpt::math::TPose3D;
64  using doubleDuration = std::chrono::duration<double>;
65  doubleDuration durationT = t.time_since_epoch();
66  double td = durationT.count();
67  ts[0] =
68  std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
69  .count();
70  ts[1] =
71  std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
72  .count();
73  ts[2] =
74  std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
75  .count();
76  ts[3] =
77  std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
78  .count();
79 
80  X[0] = p1.second.x;
81  Y[0] = p1.second.y;
82  Z[0] = p1.second.z;
83  X[1] = p2.second.x;
84  Y[1] = p2.second.y;
85  Z[1] = p2.second.z;
86  X[2] = p3.second.x;
87  Y[2] = p3.second.y;
88  Z[2] = p3.second.z;
89  X[3] = p4.second.x;
90  Y[3] = p4.second.y;
91  Z[3] = p4.second.z;
92 
93  yaw[0] = p1.second.yaw;
94  pitch[0] = p1.second.pitch;
95  roll[0] = p1.second.roll;
96  yaw[1] = p2.second.yaw;
97  pitch[1] = p2.second.pitch;
98  roll[1] = p2.second.roll;
99  yaw[2] = p3.second.yaw;
100  pitch[2] = p3.second.pitch;
101  roll[2] = p3.second.roll;
102  yaw[3] = p4.second.yaw;
103  pitch[3] = p4.second.pitch;
104  roll[3] = p4.second.roll;
105 
106  unwrap2PiSequence(yaw);
109 
110  // Target interpolated values:
111  switch (method)
112  {
113  case imSpline:
114  {
115  // ---------------------------------------
116  // SPLINE INTERPOLATION
117  // ---------------------------------------
118  out_interp.x = math::spline(td, ts, X);
119  out_interp.y = math::spline(td, ts, Y);
120  out_interp.z = math::spline(td, ts, Z);
121  out_interp.yaw = math::spline(td, ts, yaw, true); // Wrap 2pi
122  out_interp.pitch = math::spline(td, ts, pitch, true);
123  out_interp.roll = math::spline(td, ts, roll, true);
124  }
125  break;
126 
127  case imLinear2Neig:
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  out_interp.z =
134  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
135  out_interp.yaw = math::interpolate2points(
136  td, ts[1], yaw[1], ts[2], yaw[2], true); // Wrap 2pi
137  out_interp.pitch = math::interpolate2points(
138  td, ts[1], pitch[1], ts[2], pitch[2], true);
139  out_interp.roll = math::interpolate2points(
140  td, ts[1], roll[1], ts[2], roll[2], true);
141  }
142  break;
143 
144  case imLinear4Neig:
145  {
146  out_interp.x =
147  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
148  out_interp.y =
149  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
150  out_interp.z =
151  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
152  out_interp.yaw =
153  math::leastSquareLinearFit<double, decltype(ts), 4>(
154  td, ts, yaw, true); // Wrap 2pi
155  out_interp.pitch =
156  math::leastSquareLinearFit<double, decltype(ts), 4>(
157  td, ts, pitch, true);
158  out_interp.roll =
159  math::leastSquareLinearFit<double, decltype(ts), 4>(
160  td, ts, roll, true);
161  }
162  break;
163 
164  case imSSLLLL:
165  {
166  out_interp.x = math::spline(td, ts, X);
167  out_interp.y = math::spline(td, ts, Y);
168  out_interp.z =
169  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
170  out_interp.yaw =
171  math::leastSquareLinearFit<double, decltype(ts), 4>(
172  td, ts, yaw, true); // Wrap 2pi
173  out_interp.pitch =
174  math::leastSquareLinearFit<double, decltype(ts), 4>(
175  td, ts, pitch, true);
176  out_interp.roll =
177  math::leastSquareLinearFit<double, decltype(ts), 4>(
178  td, ts, roll, true);
179  }
180  break;
181 
182  case imSSLSLL:
183  {
184  out_interp.x = math::spline(td, ts, X);
185  out_interp.y = math::spline(td, ts, Y);
186  out_interp.z =
187  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
188  out_interp.yaw = math::spline(td, ts, yaw, true); // Wrap 2pi
189  out_interp.pitch =
190  math::leastSquareLinearFit<double, decltype(ts), 4>(
191  td, ts, pitch, true);
192  out_interp.roll =
193  math::leastSquareLinearFit<double, decltype(ts), 4>(
194  td, ts, roll, true);
195  }
196  break;
197 
198  case imLinearSlerp:
199  {
200  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
202  TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]),
203  TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
204 
205  out_interp.x =
206  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
207  out_interp.y =
208  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
209  out_interp.z =
210  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
211  }
212  break;
213 
214  case imSplineSlerp:
215  {
216  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
218  TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]),
219  TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
220 
221  out_interp.x = math::spline(td, ts, X);
222  out_interp.y = math::spline(td, ts, Y);
223  out_interp.z = math::spline(td, ts, Z);
224  }
225  break;
226 
227  default:
228  THROW_EXCEPTION("Unknown value for interpolation method!");
229  }; // end switch
230 }
231 
232 // Explicit instantations:
233 template class CPoseInterpolatorBase<3>;
234 } // namespace mrpt::poses
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
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.
#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.
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
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 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
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
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
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
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020