MRPT  2.0.4
CSplineInterpolator1D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/CSplineInterpolator1D.h> // for CSplineInterpolator1D
14 #include <mrpt/serialization/CSerializable.h> // for CSerializable, CSeriali...
15 #include <mrpt/serialization/stl_serialization.h> // for operator<<, operator>>
16 #include <map>
17 #include <mrpt/math/interp_fit.hpp> // for spline
18 
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 // This must be added to any CSerializable class implementation file.
25 
26 /*---------------------------------------------------------------
27  Constructor
28  ---------------------------------------------------------------*/
29 CSplineInterpolator1D::CSplineInterpolator1D(bool wrap2pi) : m_wrap2pi(wrap2pi)
30 {
31 }
32 
33 /*---------------------------------------------------------------
34  appendXY
35  ---------------------------------------------------------------*/
36 void CSplineInterpolator1D::appendXY(double x, double y) { m_x2y[x] = y; }
37 /*---------------------------------------------------------------
38  query
39  ---------------------------------------------------------------*/
40 double& CSplineInterpolator1D::query(double x, double& y, bool& out_valid) const
41 {
42  out_valid = false;
43  y = 0;
44 
45  std::pair<double, double> p1, p2, p3, p4;
46 
47  auto it_ge1 = m_x2y.lower_bound(x);
48 
49  // Exact match?
50  if (it_ge1 != m_x2y.end() && it_ge1->first == x)
51  {
52  y = it_ge1->second;
53  out_valid = true;
54  return y;
55  }
56 
57  // Are we in the beginning or the end of the path?
58  if (it_ge1 == m_x2y.end() || it_ge1 == m_x2y.begin())
59  {
60  return y;
61  }
62 
63  p3 = *it_ge1; // Third pair
64  ++it_ge1;
65  if (it_ge1 == m_x2y.end())
66  {
67  return y;
68  }
69  p4 = *it_ge1; // Fourth pair
70 
71  --it_ge1;
72  --it_ge1;
73  p2 = *it_ge1; // Second pair
74 
75  if (it_ge1 == m_x2y.begin())
76  {
77  return y;
78  }
79 
80  p1 = *(--it_ge1); // First pair
81 
82  // ---------------------------------------
83  // SPLINE INTERPOLATION
84  // ---------------------------------------
85  CVectorDouble xs(4);
86  xs[0] = p1.first;
87  xs[1] = p2.first;
88  xs[2] = p3.first;
89  xs[3] = p4.first;
90 
91  CVectorDouble ys(4);
92  ys[0] = p1.second;
93  ys[1] = p2.second;
94  ys[2] = p3.second;
95  ys[3] = p4.second;
96 
97  out_valid = true;
98  return y = math::spline(x, xs, ys, m_wrap2pi);
99 }
100 
101 uint8_t CSplineInterpolator1D::serializeGetVersion() const { return 0; }
104 {
105  out << m_x2y << m_wrap2pi;
106 }
107 
109  mrpt::serialization::CArchive& in, uint8_t version)
110 {
111  switch (version)
112  {
113  case 0: // floats
114  {
115  in >> m_x2y >> m_wrap2pi;
116  }
117  break;
118  default:
120  };
121 }
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void appendXY(double x, double y)
Append a new point:
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A (persistent) sequence of (x,y) coordinates, allowing queries of intermediate points through spline ...
This base provides a set of functions for maths stuff.
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:54
mrpt::vision::TStereoCalibResults out
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:34
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020