Main MRPT website > C++ reference for MRPT 1.5.7
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 
13 #include "CPoseInterpolatorBase.hpp" // templ impl
15 
16 using namespace mrpt::utils;
17 using namespace mrpt::poses;
18 
20 
21 void CPose3DInterpolator::writeToStream(mrpt::utils::CStream &out,int *version) const
22 {
23  if (version)
24  *version = 1;
25  else
26  {
27  out << m_path; // v1: change container element CPose3D->TPose3D
28  }
29 }
30 
31 void CPose3DInterpolator::readFromStream(mrpt::utils::CStream &in,int version)
32 {
33  switch(version)
34  {
35  case 0:
36  {
37  std::map<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> old_path;
38  in >> old_path;
39  m_path.clear();
40  for (const auto &p: old_path) {
41  m_path[p.first] = mrpt::math::TPose3D(p.second);
42  }
43  }
44  break;
45  case 1:
46  {
47  in >> m_path;
48  }
49  break;
50  default:
52  };
53 }
54 
55 namespace mrpt {
56 namespace poses {
57 
58 // Specialization for DIM=3
59 template <>
62  const TTimePosePair p1,const TTimePosePair p2,const TTimePosePair p3,const TTimePosePair p4,
63  const TInterpolatorMethod method,double td,pose_t &out_interp) const
64 {
65  using mrpt::math::TPose3D;
67  X[0] = p1.second.x; Y[0] = p1.second.y; Z[0] = p1.second.z;
68  X[1] = p2.second.x; Y[1] = p2.second.y; Z[1] = p2.second.z;
69  X[2] = p3.second.x; Y[2] = p3.second.y; Z[2] = p3.second.z;
70  X[3] = p4.second.x; Y[3] = p4.second.y; Z[3] = p4.second.z;
71 
72  yaw[0] = p1.second.yaw; pitch[0] = p1.second.pitch; roll[0] = p1.second.roll;
73  yaw[1] = p2.second.yaw; pitch[1] = p2.second.pitch; roll[1] = p2.second.roll;
74  yaw[2] = p3.second.yaw; pitch[2] = p3.second.pitch; roll[2] = p3.second.roll;
75  yaw[3] = p4.second.yaw; pitch[3] = p4.second.pitch; roll[3] = p4.second.roll;
76 
77  unwrap2PiSequence(yaw);
80 
81  // Target interpolated values:
82  switch (method)
83  {
84  case imSpline:
85  {
86  // ---------------------------------------
87  // SPLINE INTERPOLATION
88  // ---------------------------------------
89  out_interp.x = math::spline(td, ts, X);
90  out_interp.y = math::spline(td, ts, Y);
91  out_interp.z = math::spline(td, ts, Z);
92  out_interp.yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
93  out_interp.pitch = math::spline(td, ts, pitch, true );
94  out_interp.roll = math::spline(td, ts, roll, true );
95  }
96  break;
97 
98  case imLinear2Neig:
99  {
100  out_interp.x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
101  out_interp.y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
102  out_interp.z = math::interpolate2points(td, ts[1],Z[1],ts[2],Z[2]);
103  out_interp.yaw = math::interpolate2points(td, ts[1],yaw[1],ts[2],yaw[2], true ); // Wrap 2pi
104  out_interp.pitch = math::interpolate2points(td, ts[1],pitch[1],ts[2],pitch[2], true );
105  out_interp.roll = math::interpolate2points(td, ts[1],roll[1],ts[2],roll[2], true );
106  }
107  break;
108 
109  case imLinear4Neig:
110  {
111  out_interp.x = math::leastSquareLinearFit<double,decltype(ts), 4>(td, ts, X);
112  out_interp.y = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
113  out_interp.z = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
114  out_interp.yaw = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, yaw, true ); // Wrap 2pi
115  out_interp.pitch = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, pitch, true );
116  out_interp.roll = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, roll, true );
117  }
118  break;
119 
120  case imSSLLLL:
121  {
122  out_interp.x = math::spline(td, ts, X);
123  out_interp.y = math::spline(td, ts, Y);
124  out_interp.z = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
125  out_interp.yaw = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, yaw, true ); // Wrap 2pi
126  out_interp.pitch = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, pitch, true );
127  out_interp.roll = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, roll, true );
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.z = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Z);
136  out_interp.yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
137  out_interp.pitch = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, pitch, true );
138  out_interp.roll = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, roll, true );
139  }
140  break;
141 
142  case imLinearSlerp:
143  {
144  const double ratio = (td-ts[1])/(ts[2]-ts[1]);
145  mrpt::math::slerp_ypr(TPose3D(0,0,0,yaw[1], pitch[1], roll[1]), TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
146 
147  out_interp.x = math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
148  out_interp.y = math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
149  out_interp.z = math::interpolate2points(td, ts[1], Z[1], ts[2], Z[2]);
150  }
151  break;
152 
153  case imSplineSlerp:
154  {
155  const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
156  mrpt::math::slerp_ypr(TPose3D(0, 0, 0, yaw[1], pitch[1], roll[1]), TPose3D(0, 0, 0, yaw[2], pitch[2], roll[2]), ratio, out_interp);
157 
158  out_interp.x = math::spline(td, ts, X);
159  out_interp.y = math::spline(td, ts, Y);
160  out_interp.z = math::spline(td, ts, Z);
161  }
162  break;
163 
164  default: THROW_EXCEPTION("Unknown value for interpolation method!");
165  }; // end switch
166 }
167 
168 
169 // Explicit instantations:
171 }
172 }
173 
174 
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
Base class for SE(2)/SE(3) interpolators.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
void BASE_IMPEXP slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:54
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
int version
Definition: mrpt_jpeglib.h:898
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:70
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
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:6301
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
double BASE_IMPEXP 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:2043
GLfloat GLfloat p
Definition: glext.h:5587
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019