Main MRPT website > C++ reference for MRPT 1.9.9
interp_fit.hpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, 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 #pragma once
11 
12 #include <mrpt/math/interp_fit.h>
13 
14 namespace mrpt
15 {
16 namespace math
17 {
18 template <class T, class VECTOR>
19 T interpolate(const T& x, const VECTOR& ys, const T& x0, const T& x1)
20 {
22  ASSERT_(x1 > x0);
23  ASSERT_(!ys.empty());
24  const size_t N = ys.size();
25  if (x <= x0) return ys[0];
26  if (x >= x1) return ys[N - 1];
27  const T Ax = (x1 - x0) / T(N);
28  const size_t i = int((x - x0) / Ax);
29  if (i >= N - 1) return ys[N - 1];
30  const T Ay = ys[i + 1] - ys[i];
31  return ys[i] + (x - (x0 + i * Ax)) * Ay / Ax;
32  MRPT_END
33 }
34 
35 template <typename NUMTYPE, class VECTORLIKE>
36 NUMTYPE spline(
37  const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi)
38 {
39  // Check input data
40  ASSERT_(x.size() == 4 && y.size() == 4);
41  ASSERT_(x[0] <= x[1] && x[1] <= x[2] && x[2] <= x[3]);
42  ASSERT_(t > x[0] && t < x[3]);
43 
44  NUMTYPE h[3];
45  for (unsigned int i = 0; i < 3; i++) h[i] = x[i + 1] - x[i];
46 
47  double k = 1 / (4 * h[0] * h[1] + 4 * h[0] * h[2] + 3 * h[1] * h[1] +
48  4 * h[1] * h[2]);
49  double a11 = 2 * (h[1] + h[2]) * k;
50  double a12 = -h[1] * k;
51  double a22 = 2 * (h[0] + h[1]) * k;
52 
53  double y0, y1, y2, y3;
54 
55  if (!wrap2pi)
56  {
57  y0 = y[0];
58  y1 = y[1];
59  y2 = y[2];
60  y3 = y[3];
61  }
62  else
63  {
64  // Assure the function is linear without jumps in the interval:
65  y0 = mrpt::math::wrapToPi(y[0]);
66  y1 = mrpt::math::wrapToPi(y[1]);
67  y2 = mrpt::math::wrapToPi(y[2]);
68  y3 = mrpt::math::wrapToPi(y[3]);
69 
70  double Ay;
71 
72  Ay = y1 - y0;
73  if (Ay > M_PI)
74  y1 -= M_2PI;
75  else if (Ay < -M_PI)
76  y1 += M_2PI;
77 
78  Ay = y2 - y1;
79  if (Ay > M_PI)
80  y2 -= M_2PI;
81  else if (Ay < -M_PI)
82  y2 += M_2PI;
83 
84  Ay = y3 - y2;
85  if (Ay > M_PI)
86  y3 -= M_2PI;
87  else if (Ay < -M_PI)
88  y3 += M_2PI;
89  }
90 
91  double b1 = (y2 - y1) / h[1] - (y1 - y0) / h[0];
92  double b2 = (y3 - y2) / h[2] - (y2 - y1) / h[1];
93 
94  double z0 = 0;
95  double z1 = 6 * (a11 * b1 + a12 * b2);
96  double z2 = 6 * (a12 * b1 + a22 * b2);
97  double z3 = 0;
98 
99  double res = 0;
100  if (t < x[1])
101  res = (z1 * pow((t - x[0]), 3) + z0 * pow((x[1] - t), 3)) / (6 * h[0]) +
102  (y1 / h[0] - h[0] / 6 * z1) * (t - x[0]) +
103  (y0 / h[0] - h[0] / 6 * z0) * (x[1] - t);
104  else
105  {
106  if (t < x[2])
107  res = (z2 * pow((t - x[1]), 3) + z1 * pow((x[2] - t), 3)) /
108  (6 * h[1]) +
109  (y2 / h[1] - h[1] / 6 * z2) * (t - x[1]) +
110  (y1 / h[1] - h[1] / 6 * z1) * (x[2] - t);
111  else if (t < x[3])
112  res = (z3 * pow((t - x[2]), 3) + z2 * pow((x[3] - t), 3)) /
113  (6 * h[2]) +
114  (y3 / h[2] - h[2] / 6 * z3) * (t - x[2]) +
115  (y2 / h[2] - h[2] / 6 * z2) * (x[3] - t);
116  }
117  return wrap2pi ? mrpt::math::wrapToPi(res) : res;
118 }
119 
120 template <typename NUMTYPE, class VECTORLIKE, int NUM_POINTS>
122  const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi)
123 {
124  MRPT_START
125 
126  // http://en.wikipedia.org/wiki/Linear_least_squares
127  ASSERT_(x.size() == y.size());
128  ASSERT_(x.size() > 1);
129 
130  const size_t N = x.size();
131 
132  // X= [1 columns of ones, x' ]
133  const NUMTYPE x_min = x.minimum();
134  Eigen::Matrix<NUMTYPE, 2, NUM_POINTS> Xt;
135  Xt.resize(2, N);
136  for (size_t i = 0; i < N; i++)
137  {
138  Xt.set_unsafe(0, i, 1);
139  Xt.set_unsafe(1, i, x[i] - x_min);
140  }
141 
142  const auto B = ((Xt * Xt.transpose()).inv().eval() * Xt * y).eval();
143  ASSERT_(B.size() == 2);
144 
145  NUMTYPE ret = B[0] + B[1] * (t - x_min);
146 
147  // wrap?
148  if (!wrap2pi)
149  return ret;
150  else
151  return mrpt::math::wrapToPi(ret);
152 
153  MRPT_END
154 }
155 
156 template <
157  class VECTORLIKE1, class VECTORLIKE2, class VECTORLIKE3, int NUM_POINTS>
159  const VECTORLIKE1& ts, VECTORLIKE2& outs, const VECTORLIKE3& x,
160  const VECTORLIKE3& y, bool wrap2pi)
161 {
162  MRPT_START
163 
164  // http://en.wikipedia.org/wiki/Linear_least_squares
165  ASSERT_(x.size() == y.size());
166  ASSERT_(x.size() > 1);
167 
168  const size_t N = x.size();
169 
170  // X= [1 columns of ones, x' ]
171  using NUM = typename VECTORLIKE3::value_type;
172  const NUM x_min = x.minimum();
173  Eigen::Matrix<NUM, 2, NUM_POINTS> Xt;
174  Xt.resize(2, N);
175  for (size_t i = 0; i < N; i++)
176  {
177  Xt.set_unsafe(0, i, 1);
178  Xt.set_unsafe(1, i, x[i] - x_min);
179  }
180 
181  const auto B = ((Xt * Xt.transpose()).inv().eval() * Xt * y).eval();
182  ASSERT_(B.size() == 2);
183 
184  const size_t tsN = size_t(ts.size());
185  outs.resize(tsN);
186  if (!wrap2pi)
187  for (size_t k = 0; k < tsN; k++)
188  outs[k] = B[0] + B[1] * (ts[k] - x_min);
189  else
190  for (size_t k = 0; k < tsN; k++)
191  outs[k] = mrpt::math::wrapToPi(B[0] + B[1] * (ts[k] - x_min));
192  MRPT_END
193 }
194 
195 } // namespace math
196 } // namespace mrpt
NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi=false)
Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y"...
Definition: interp_fit.hpp:121
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
#define M_2PI
Definition: common.h:58
T interpolate(const T &x, const VECTOR &ys, const T &x0, const T &x1)
Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...
Definition: interp_fit.hpp:19
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MRPT_END
Definition: exceptions.h:266
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:36
GLenum GLint GLint y
Definition: glext.h:3538
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
EIGEN_STRONG_INLINE PlainObject inv() const



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019