MRPT  2.0.3
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
poses-precomp.h
mrpt::poses::CPoseInterpolatorBase< 3 >::pose_t
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
Definition: CPoseInterpolatorBase.h:60
mrpt::poses::CPoseInterpolatorBase< 3 >
mrpt::poses::CPoseInterpolatorBase::impl_interpolation
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
mrpt::poses::CPoseInterpolatorBase< 3 >::m_path
TPath m_path
The sequence of poses.
Definition: CPoseInterpolatorBase.h:207
mrpt::math::spline
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
mrpt::poses::imSplineSlerp
@ imSplineSlerp
Definition: CPoseInterpolatorBase.h:43
stl_serialization.h
mrpt::obs::gnss::roll
double roll
Definition: gnss_messages_novatel.h:306
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::imLinearSlerp
@ imLinearSlerp
Definition: CPoseInterpolatorBase.h:42
CPose3DInterpolator.h
mrpt::poses::imLinear4Neig
@ imLinear4Neig
Definition: CPoseInterpolatorBase.h:39
mrpt::poses::CPose3DInterpolator
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
Definition: CPose3DInterpolator.h:47
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::poses::TInterpolatorMethod
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes.
Definition: CPoseInterpolatorBase.h:35
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::poses::CPose3DInterpolator::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose3DInterpolator.cpp:22
mrpt::poses::imLinear2Neig
@ imLinear2Neig
Definition: CPoseInterpolatorBase.h:38
mrpt::poses::CPose3DInterpolator::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose3DInterpolator.cpp:26
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::Clock::time_point
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:306
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::poses::imSpline
@ imSpline
Definition: CPoseInterpolatorBase.h:37
CPoseInterpolatorBase.hpp
mrpt::poses::imSSLLLL
@ imSSLLLL
Definition: CPoseInterpolatorBase.h:40
mrpt::math::interpolate2points
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
mrpt::poses::imSSLSLL
@ imSSLSLL
Definition: CPoseInterpolatorBase.h:41
mrpt::math::slerp_ypr
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
mrpt::math::unwrap2PiSequence
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_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::poses::CPoseInterpolatorBase< 3 >::TTimePosePair
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
Definition: CPoseInterpolatorBase.h:66



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020