Main MRPT website > C++ reference for MRPT 1.5.7
CSplineInterpolator1D.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/utils/types_math.h> // for dynamic_vector, CVector...
13 #include <mrpt/math/CSplineInterpolator1D.h> // for CSplineInterpolator1D
14 #include <mrpt/utils/CStream.h> // for CStream, operator<<
15 #include <mrpt/utils/stl_serialization.h> // for operator<<, operator>>
16 #include <map> // for _Rb_tree_const_iterator
17 #include <mrpt/math/interp_fit.hpp> // for spline
18 #include <mrpt/utils/CObject.h> // for CSplineInterpolator1D::...
19 #include <mrpt/utils/CSerializable.h> // for CSerializable, CSeriali...
20 #include <mrpt/utils/bits.h> // for format
21 #include <mrpt/utils/mrpt_macros.h> // for MRPT_THROW_UNKNOWN_SERI...
22 
23 using namespace mrpt;
24 using namespace mrpt::math;
25 using namespace mrpt::utils;
26 using namespace std;
27 
28 // This must be added to any CSerializable class implementation file.
30 
31 /*---------------------------------------------------------------
32  Constructor
33  ---------------------------------------------------------------*/
34 CSplineInterpolator1D::CSplineInterpolator1D( bool wrap2pi ) : m_wrap2pi(wrap2pi)
35 {
36 }
37 
38 /*---------------------------------------------------------------
39  appendXY
40  ---------------------------------------------------------------*/
41 void CSplineInterpolator1D::appendXY( double x, double y )
42 {
43  m_x2y[x] = y;
44 }
45 
46 /*---------------------------------------------------------------
47  query
48  ---------------------------------------------------------------*/
49 double & CSplineInterpolator1D::query( double x, double &y, bool &out_valid ) const
50 {
51  out_valid = false;
52  y=0;
53 
54  std::pair<double,double> p1,p2,p3,p4;
55 
56  std::map<double,double>::const_iterator it_ge1 = m_x2y.lower_bound( x );
57 
58  // Exact match?
59  if( it_ge1 != m_x2y.end() && it_ge1->first == x )
60  {
61  y = it_ge1->second;
62  out_valid = true;
63  return y;
64  }
65 
66  // Are we in the beginning or the end of the path?
67  if( it_ge1 == m_x2y.end() || it_ge1 == m_x2y.begin() )
68  {
69  return y;
70  }
71 
72  p3 = *it_ge1; // Third pair
73  ++it_ge1;
74  if( it_ge1 == m_x2y.end() )
75  {
76  return y;
77  }
78  p4 = *it_ge1; // Fourth pair
79 
80  --it_ge1;
81  --it_ge1;
82  p2 = *it_ge1; // Second pair
83 
84  if( it_ge1 == m_x2y.begin() )
85  {
86  return y;
87  }
88 
89  p1 = *(--it_ge1); // First pair
90 
91  // ---------------------------------------
92  // SPLINE INTERPOLATION
93  // ---------------------------------------
94  CVectorDouble xs(4);
95  xs[0] = p1.first;
96  xs[1] = p2.first;
97  xs[2] = p3.first;
98  xs[3] = p4.first;
99 
100  CVectorDouble ys(4);
101  ys[0] = p1.second;
102  ys[1] = p2.second;
103  ys[2] = p3.second;
104  ys[3] = p4.second;
105 
106  out_valid = true;
107  return y = math::spline(x,xs, ys, m_wrap2pi);
108 }
109 
110 
111 /*---------------------------------------------------------------
112  Implements the writing to a CStream capability of
113  CSerializable objects
114  ---------------------------------------------------------------*/
116 {
117  if (version)
118  *version = 0;
119  else
120  {
121  out << m_x2y << m_wrap2pi;
122  }
123 
124 }
125 
126 /*---------------------------------------------------------------
127  Implements the reading from a CStream capability of
128  CSerializable objects
129  ---------------------------------------------------------------*/
131 {
132  switch(version)
133  {
134  case 0: // floats
135  {
136  in >> m_x2y >> m_wrap2pi;
137  } break;
138  default:
140  };
141 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void appendXY(double x, double y)
Append a new point:
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A (persistent) sequence of (x,y) coordinates, allowing queries of intermediate points through spline ...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
int version
Definition: mrpt_jpeglib.h:898
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516
double & query(double x, double &y, bool &out_valid) const
Query an interpolation of the curve at some "x".



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