Main MRPT website > C++ reference for MRPT 1.5.7
CPose2DInterpolator.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 CPose2DInterpolator::writeToStream(mrpt::utils::CStream &out,int *version) const
22 {
23  if (version)
24  *version = 0;
25  else
26  {
27  out << m_path;
28  }
29 }
30 
31 void CPose2DInterpolator::readFromStream(mrpt::utils::CStream &in,int version)
32 {
33  switch(version)
34  {
35  case 0:
36  {
37  in >> m_path;
38  }
39  break;
40  default:
42  };
43 }
44 
45 namespace mrpt {
46 namespace poses {
47 
48 // Specialization for DIM=2
49 template <>
52  const TTimePosePair p1,const TTimePosePair p2,const TTimePosePair p3,const TTimePosePair p4,
53  const TInterpolatorMethod method,double td,pose_t &out_interp) const
54 {
55  using mrpt::math::TPose2D;
57  X[0] = p1.second.x; Y[0] = p1.second.y; yaw[0] = p1.second.phi;
58  X[1] = p2.second.x; Y[1] = p2.second.y; yaw[1] = p2.second.phi;
59  X[2] = p3.second.x; Y[2] = p3.second.y; yaw[2] = p3.second.phi;
60  X[3] = p4.second.x; Y[3] = p4.second.y; yaw[3] = p4.second.phi;
61 
62  unwrap2PiSequence(yaw);
63 
64  // Target interpolated values:
65  switch (method)
66  {
67  case imSpline:
68  {
69  // ---------------------------------------
70  // SPLINE INTERPOLATION
71  // ---------------------------------------
72  out_interp.x = math::spline(td, ts, X);
73  out_interp.y = math::spline(td, ts, Y);
74  out_interp.phi = math::spline(td, ts, yaw, true ); // Wrap 2pi
75  }
76  break;
77 
78  case imLinear2Neig:
79  {
80  out_interp.x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
81  out_interp.y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
82  out_interp.phi = math::interpolate2points(td, ts[1],yaw[1],ts[2],yaw[2], true ); // Wrap 2pi
83  }
84  break;
85 
86  case imLinear4Neig:
87  {
88  out_interp.x = math::leastSquareLinearFit<double,decltype(ts), 4>(td, ts, X);
89  out_interp.y = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
90  out_interp.phi = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, yaw, true ); // Wrap 2pi
91  }
92  break;
93 
94  case imSSLLLL:
95  {
96  out_interp.x = math::spline(td, ts, X);
97  out_interp.y = math::spline(td, ts, Y);
98  out_interp.phi = math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, yaw, true ); // Wrap 2pi
99  }
100  break;
101 
102  case imSSLSLL:
103  {
104  out_interp.x = math::spline(td, ts, X);
105  out_interp.y = math::spline(td, ts, Y);
106  out_interp.phi = math::spline(td, ts, yaw, true ); // Wrap 2pi
107  }
108  break;
109 
110  case imLinearSlerp:
111  {
112  const double ratio = (td-ts[1])/(ts[2]-ts[1]);
113  const double Aang = mrpt::math::angDistance(yaw[1],yaw[2]);
114  out_interp.phi = yaw[1] + ratio*Aang;
115 
116  out_interp.x = math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
117  out_interp.y = math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
118  }
119  break;
120 
121  case imSplineSlerp:
122  {
123  const double ratio = (td-ts[1])/(ts[2]-ts[1]);
124  const double Aang = mrpt::math::angDistance(yaw[1],yaw[2]);
125  out_interp.phi = yaw[1] + ratio*Aang;
126 
127  out_interp.x = math::spline(td, ts, X);
128  out_interp.y = math::spline(td, ts, Y);
129  }
130  break;
131 
132  default: THROW_EXCEPTION("Unknown value for interpolation method!");
133  }; // end switch
134 }
135 
136 // Explicit instantations:
138 
139 }
140 }
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.
This class stores a time-stamped trajectory in SE(2) (mrpt::math::TPose2D poses). ...
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)
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:91
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
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
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
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019