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-2017, 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 "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose3DInterpolator.h>
13 #include "CPoseInterpolatorBase.hpp" // templ impl
14 #include <mrpt/utils/stl_serialization.h>
15 
16 using namespace mrpt::utils;
17 using namespace mrpt::poses;
18 
19 IMPLEMENTS_SERIALIZABLE(CPose3DInterpolator, CSerializable, mrpt::poses)
20 
21 void CPose3DInterpolator::writeToStream(
22  mrpt::utils::CStream& out, int* version) const
23 {
24  if (version)
25  *version = 1;
26  else
27  {
28  out << m_path; // v1: change container element CPose3D->TPose3D
29  }
30 }
31 
32 void CPose3DInterpolator::readFromStream(mrpt::utils::CStream& in, int version)
33 {
34  switch (version)
35  {
36  case 0:
37  {
38  std::map<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> old_path;
39  in >> old_path;
40  m_path.clear();
41  for (const auto& p : old_path)
42  {
43  m_path[p.first] = mrpt::math::TPose3D(p.second);
44  }
45  }
46  break;
47  case 1:
48  {
49  in >> m_path;
50  }
51  break;
52  default:
53  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
54  };
55 }
56 
57 namespace mrpt
58 {
59 namespace poses
60 {
61 // Specialization for DIM=3
62 template <>
63 void CPoseInterpolatorBase<3>::impl_interpolation(
64  const mrpt::math::CArrayDouble<4>& ts, const TTimePosePair p1,
65  const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4,
66  const TInterpolatorMethod method, double td, pose_t& out_interp) const
67 {
68  using mrpt::math::TPose3D;
69  mrpt::math::CArrayDouble<4> X, Y, Z, yaw, pitch, roll;
70  X[0] = p1.second.x;
71  Y[0] = p1.second.y;
72  Z[0] = p1.second.z;
73  X[1] = p2.second.x;
74  Y[1] = p2.second.y;
75  Z[1] = p2.second.z;
76  X[2] = p3.second.x;
77  Y[2] = p3.second.y;
78  Z[2] = p3.second.z;
79  X[3] = p4.second.x;
80  Y[3] = p4.second.y;
81  Z[3] = p4.second.z;
82 
83  yaw[0] = p1.second.yaw;
84  pitch[0] = p1.second.pitch;
85  roll[0] = p1.second.roll;
86  yaw[1] = p2.second.yaw;
87  pitch[1] = p2.second.pitch;
88  roll[1] = p2.second.roll;
89  yaw[2] = p3.second.yaw;
90  pitch[2] = p3.second.pitch;
91  roll[2] = p3.second.roll;
92  yaw[3] = p4.second.yaw;
93  pitch[3] = p4.second.pitch;
94  roll[3] = p4.second.roll;
95 
96  unwrap2PiSequence(yaw);
97  unwrap2PiSequence(pitch);
98  unwrap2PiSequence(roll);
99 
100  // Target interpolated values:
101  switch (method)
102  {
103  case imSpline:
104  {
105  // ---------------------------------------
106  // SPLINE INTERPOLATION
107  // ---------------------------------------
108  out_interp.x = math::spline(td, ts, X);
109  out_interp.y = math::spline(td, ts, Y);
110  out_interp.z = math::spline(td, ts, Z);
111  out_interp.yaw = math::spline(td, ts, yaw, true); // Wrap 2pi
112  out_interp.pitch = math::spline(td, ts, pitch, true);
113  out_interp.roll = math::spline(td, ts, roll, true);
114  }
115  break;
116 
117  case imLinear2Neig:
118  {
119  out_interp.x =
120  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
121  out_interp.y =
122  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
123  out_interp.z =
124  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
125  out_interp.yaw = math::interpolate2points(
126  td, ts[1], yaw[1], ts[2], yaw[2], true); // Wrap 2pi
127  out_interp.pitch = math::interpolate2points(
128  td, ts[1], pitch[1], ts[2], pitch[2], true);
129  out_interp.roll = math::interpolate2points(
130  td, ts[1], roll[1], ts[2], roll[2], true);
131  }
132  break;
133 
134  case imLinear4Neig:
135  {
136  out_interp.x =
137  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
138  out_interp.y =
139  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
140  out_interp.z =
141  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
142  out_interp.yaw =
143  math::leastSquareLinearFit<double, decltype(ts), 4>(
144  td, ts, yaw, true); // Wrap 2pi
145  out_interp.pitch =
146  math::leastSquareLinearFit<double, decltype(ts), 4>(
147  td, ts, pitch, true);
148  out_interp.roll =
149  math::leastSquareLinearFit<double, decltype(ts), 4>(
150  td, ts, roll, true);
151  }
152  break;
153 
154  case imSSLLLL:
155  {
156  out_interp.x = math::spline(td, ts, X);
157  out_interp.y = math::spline(td, ts, Y);
158  out_interp.z =
159  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
160  out_interp.yaw =
161  math::leastSquareLinearFit<double, decltype(ts), 4>(
162  td, ts, yaw, true); // Wrap 2pi
163  out_interp.pitch =
164  math::leastSquareLinearFit<double, decltype(ts), 4>(
165  td, ts, pitch, true);
166  out_interp.roll =
167  math::leastSquareLinearFit<double, decltype(ts), 4>(
168  td, ts, roll, true);
169  }
170  break;
171 
172  case imSSLSLL:
173  {
174  out_interp.x = math::spline(td, ts, X);
175  out_interp.y = math::spline(td, ts, Y);
176  out_interp.z =
177  math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
178  out_interp.yaw = math::spline(td, ts, yaw, true); // Wrap 2pi
179  out_interp.pitch =
180  math::leastSquareLinearFit<double, decltype(ts), 4>(
181  td, ts, pitch, true);
182  out_interp.roll =
183  math::leastSquareLinearFit<double, decltype(ts), 4>(
184  td, ts, roll, true);
185  }
186  break;
187 
188  case imLinearSlerp:
189  {
190  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
191  mrpt::math::slerp_ypr(
192  TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]),
193  TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
194 
195  out_interp.x =
196  math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
197  out_interp.y =
198  math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
199  out_interp.z =
200  math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
201  }
202  break;
203 
204  case imSplineSlerp:
205  {
206  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
207  mrpt::math::slerp_ypr(
208  TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]),
209  TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
210 
211  out_interp.x = math::spline(td, ts, X);
212  out_interp.y = math::spline(td, ts, Y);
213  out_interp.z = math::spline(td, ts, Z);
214  }
215  break;
216 
217  default:
218  THROW_EXCEPTION("Unknown value for interpolation method!");
219  }; // end switch
220 }
221 
222 // Explicit instantations:
223 template class CPoseInterpolatorBase<3>;
224 }
225 }
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:169
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:88
mrpt::poses::CPose3DInterpolator
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
Definition: CPose3DInterpolator.h:51
mrpt::poses::CPoseInterpolatorBase
Base class for SE(2)/SE(3) interpolators.
Definition: CPoseInterpolatorBase.h:53
mrpt::poses::CPoseInterpolatorBase::pose_t
mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
Definition: CPoseInterpolatorBase.h:62
mrpt::poses::CPoseInterpolatorBase::TTimePosePair
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
Definition: CPoseInterpolatorBase.h:68
mrpt::poses::CPoseInterpolatorBase::clear
void clear()
Clears the current sequence of poses.
Definition: CPoseInterpolatorBase.hpp:30
mrpt::utils::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:45
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:42
void
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
in
GLuint in
Definition: glext.h:7274
p
GLfloat GLfloat p
Definition: glext.h:6305
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:74
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:46
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:37
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:1975
mrpt::poses::TInterpolatorMethod
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes.
Definition: CPoseInterpolatorBase.h:38
mrpt::poses::imSpline
@ imSpline
Definition: CPoseInterpolatorBase.h:39
mrpt::poses::imLinear2Neig
@ imLinear2Neig
Definition: CPoseInterpolatorBase.h:40
mrpt::poses::imSplineSlerp
@ imSplineSlerp
Definition: CPoseInterpolatorBase.h:45
mrpt::poses::imSSLLLL
@ imSSLLLL
Definition: CPoseInterpolatorBase.h:42
mrpt::poses::imSSLSLL
@ imSSLSLL
Definition: CPoseInterpolatorBase.h:43
mrpt::poses::imLinearSlerp
@ imLinearSlerp
Definition: CPoseInterpolatorBase.h:44
mrpt::poses::imLinear4Neig
@ imLinear4Neig
Definition: CPoseInterpolatorBase.h:41
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: mrpt_macros.h:181
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:111
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: CParticleFilter.h:224
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:18
stl_serialization.h
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:617


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST