Main MRPT website > C++ reference for MRPT 1.9.9
CPose3DInterpolator.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 CPose3DInterpolator::serializeGetVersion() const { return 1; }
22 {
23  out << m_path; // v1: change container element CPose3D->TPose3D
24 }
27 {
28  switch (version)
29  {
30  case 0:
31  {
32  std::map<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> old_path;
33  in >> old_path;
34  m_path.clear();
35  for (const auto& p : old_path)
36  {
37  m_path[p.first] = p.second.asTPose();
38  }
39  }
40  break;
41  case 1:
42  {
43  in >> m_path;
44  }
45  break;
46  default:
48  };
49 }
50 
51 namespace mrpt
52 {
53 namespace poses
54 {
55 // Specialization for DIM=3
56 template <>
58  const mrpt::math::CArrayDouble<4>& ts, const TTimePosePair p1,
59  const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4,
60  const TInterpolatorMethod method, double td, pose_t& out_interp) const
61 {
62  using mrpt::math::TPose3D;
63  mrpt::math::CArrayDouble<4> X, Y, Z, yaw, pitch, roll;
64  X[0] = p1.second.x;
65  Y[0] = p1.second.y;
66  Z[0] = p1.second.z;
67  X[1] = p2.second.x;
68  Y[1] = p2.second.y;
69  Z[1] = p2.second.z;
70  X[2] = p3.second.x;
71  Y[2] = p3.second.y;
72  Z[2] = p3.second.z;
73  X[3] = p4.second.x;
74  Y[3] = p4.second.y;
75  Z[3] = p4.second.z;
76 
77  yaw[0] = p1.second.yaw;
78  pitch[0] = p1.second.pitch;
79  roll[0] = p1.second.roll;
80  yaw[1] = p2.second.yaw;
81  pitch[1] = p2.second.pitch;
82  roll[1] = p2.second.roll;
83  yaw[2] = p3.second.yaw;
84  pitch[2] = p3.second.pitch;
85  roll[2] = p3.second.roll;
86  yaw[3] = p4.second.yaw;
87  pitch[3] = p4.second.pitch;
88  roll[3] = p4.second.roll;
89 
90  unwrap2PiSequence(yaw);
93 
94  // Target interpolated values:
95  switch (method)
96  {
97  case imSpline:
98  {
99  // ---------------------------------------
100  // SPLINE INTERPOLATION
101  // ---------------------------------------
102  out_interp.x = math::spline(td, ts, X);
103  out_interp.y = math::spline(td, ts, Y);
104  out_interp.z = math::spline(td, ts, Z);
105  out_interp.yaw = math::spline(td, ts, yaw, true); // Wrap 2pi
106  out_interp.pitch = math::spline(td, ts, pitch, true);
107  out_interp.roll = math::spline(td, ts, roll, true);
108  }
109  break;
110 
111  case imLinear2Neig:
112  {
113  out_interp.x =
114  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
115  out_interp.y =
116  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
117  out_interp.z =
118  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
119  out_interp.yaw = math::interpolate2points(
120  td, ts[1], yaw[1], ts[2], yaw[2], true); // Wrap 2pi
121  out_interp.pitch = math::interpolate2points(
122  td, ts[1], pitch[1], ts[2], pitch[2], true);
123  out_interp.roll = math::interpolate2points(
124  td, ts[1], roll[1], ts[2], roll[2], true);
125  }
126  break;
127 
128  case imLinear4Neig:
129  {
130  out_interp.x =
131  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
132  out_interp.y =
133  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
134  out_interp.z =
135  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
136  out_interp.yaw =
137  math::leastSquareLinearFit<double, decltype(ts), 4>(
138  td, ts, yaw, true); // Wrap 2pi
139  out_interp.pitch =
140  math::leastSquareLinearFit<double, decltype(ts), 4>(
141  td, ts, pitch, true);
142  out_interp.roll =
143  math::leastSquareLinearFit<double, decltype(ts), 4>(
144  td, ts, roll, true);
145  }
146  break;
147 
148  case imSSLLLL:
149  {
150  out_interp.x = math::spline(td, ts, X);
151  out_interp.y = math::spline(td, ts, Y);
152  out_interp.z =
153  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
154  out_interp.yaw =
155  math::leastSquareLinearFit<double, decltype(ts), 4>(
156  td, ts, yaw, true); // Wrap 2pi
157  out_interp.pitch =
158  math::leastSquareLinearFit<double, decltype(ts), 4>(
159  td, ts, pitch, true);
160  out_interp.roll =
161  math::leastSquareLinearFit<double, decltype(ts), 4>(
162  td, ts, roll, true);
163  }
164  break;
165 
166  case imSSLSLL:
167  {
168  out_interp.x = math::spline(td, ts, X);
169  out_interp.y = math::spline(td, ts, Y);
170  out_interp.z =
171  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
172  out_interp.yaw = math::spline(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 imLinearSlerp:
183  {
184  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
186  TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]),
187  TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
188 
189  out_interp.x =
190  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
191  out_interp.y =
192  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
193  out_interp.z =
194  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
195  }
196  break;
197 
198  case imSplineSlerp:
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 = math::spline(td, ts, X);
206  out_interp.y = math::spline(td, ts, Y);
207  out_interp.z = math::spline(td, ts, Z);
208  }
209  break;
210 
211  default:
212  THROW_EXCEPTION("Unknown value for interpolation method!");
213  }; // end switch
214 }
215 
216 // Explicit instantations:
217 template class CPoseInterpolatorBase<3>;
218 }
219 }
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
#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
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
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
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
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
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:436
GLfloat GLfloat p
Definition: glext.h:6305
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 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019