MRPT  1.9.9
interp_fit.hpp
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 details in https://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 #pragma once
11 
12 #include <mrpt/math/interp_fit.h>
13 
14 namespace mrpt::math
15 {
16 template <class T, class VECTOR>
17 T interpolate(const T& x, const VECTOR& ys, const T& x0, const T& x1)
18 {
20  ASSERT_(x1 > x0);
21  ASSERT_(ys.size() > 0);
22  const size_t N = ys.size();
23  if (x <= x0) return ys[0];
24  if (x >= x1) return ys[N - 1];
25  const T Ax = (x1 - x0) / T(N);
26  const size_t i = int((x - x0) / Ax);
27  if (i >= N - 1) return ys[N - 1];
28  const T Ay = ys[i + 1] - ys[i];
29  return ys[i] + (x - (x0 + i * Ax)) * Ay / Ax;
30  MRPT_END
31 }
32 
33 template <typename NUMTYPE, class VECTORLIKE>
34 NUMTYPE spline(
35  const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi)
36 {
37  // Check input data
38  ASSERT_(x.size() == 4 && y.size() == 4);
39  ASSERT_(x[0] <= x[1] && x[1] <= x[2] && x[2] <= x[3]);
40  ASSERT_(t > x[0] && t < x[3]);
41 
42  NUMTYPE h[3];
43  for (unsigned int i = 0; i < 3; i++) h[i] = x[i + 1] - x[i];
44 
45  double k = 1 / (4 * h[0] * h[1] + 4 * h[0] * h[2] + 3 * h[1] * h[1] +
46  4 * h[1] * h[2]);
47  double a11 = 2 * (h[1] + h[2]) * k;
48  double a12 = -h[1] * k;
49  double a22 = 2 * (h[0] + h[1]) * k;
50 
51  double y0, y1, y2, y3;
52 
53  if (!wrap2pi)
54  {
55  y0 = y[0];
56  y1 = y[1];
57  y2 = y[2];
58  y3 = y[3];
59  }
60  else
61  {
62  // Assure the function is linear without jumps in the interval:
63  y0 = mrpt::math::wrapToPi(y[0]);
64  y1 = mrpt::math::wrapToPi(y[1]);
65  y2 = mrpt::math::wrapToPi(y[2]);
66  y3 = mrpt::math::wrapToPi(y[3]);
67 
68  double Ay;
69 
70  Ay = y1 - y0;
71  if (Ay > M_PI)
72  y1 -= M_2PI;
73  else if (Ay < -M_PI)
74  y1 += M_2PI;
75 
76  Ay = y2 - y1;
77  if (Ay > M_PI)
78  y2 -= M_2PI;
79  else if (Ay < -M_PI)
80  y2 += M_2PI;
81 
82  Ay = y3 - y2;
83  if (Ay > M_PI)
84  y3 -= M_2PI;
85  else if (Ay < -M_PI)
86  y3 += M_2PI;
87  }
88 
89  double b1 = (y2 - y1) / h[1] - (y1 - y0) / h[0];
90  double b2 = (y3 - y2) / h[2] - (y2 - y1) / h[1];
91 
92  double z0 = 0;
93  double z1 = 6 * (a11 * b1 + a12 * b2);
94  double z2 = 6 * (a12 * b1 + a22 * b2);
95  double z3 = 0;
96 
97  double res = 0;
98  if (t < x[1])
99  res = (z1 * pow((t - x[0]), 3) + z0 * pow((x[1] - t), 3)) / (6 * h[0]) +
100  (y1 / h[0] - h[0] / 6 * z1) * (t - x[0]) +
101  (y0 / h[0] - h[0] / 6 * z0) * (x[1] - t);
102  else
103  {
104  if (t < x[2])
105  res = (z2 * pow((t - x[1]), 3) + z1 * pow((x[2] - t), 3)) /
106  (6 * h[1]) +
107  (y2 / h[1] - h[1] / 6 * z2) * (t - x[1]) +
108  (y1 / h[1] - h[1] / 6 * z1) * (x[2] - t);
109  else if (t < x[3])
110  res = (z3 * pow((t - x[2]), 3) + z2 * pow((x[3] - t), 3)) /
111  (6 * h[2]) +
112  (y3 / h[2] - h[2] / 6 * z3) * (t - x[2]) +
113  (y2 / h[2] - h[2] / 6 * z2) * (x[3] - t);
114  }
115  return wrap2pi ? mrpt::math::wrapToPi(res) : res;
116 }
117 
118 template <typename NUMTYPE, class VECTORLIKE, int NUM_POINTS>
120  const NUMTYPE t, const VECTORLIKE& x, const VECTORLIKE& y, bool wrap2pi)
121 {
122  MRPT_START
123 
124  // http://en.wikipedia.org/wiki/Linear_least_squares
125  ASSERT_(x.size() == y.size());
126  ASSERT_(x.size() > 1);
127 
128  const size_t N = x.size();
129 
130  // X= [1 columns of ones, x' ]
131  const NUMTYPE x_min = x.minCoeff();
133  Xt.resize(2, N);
134  for (size_t i = 0; i < N; i++)
135  {
136  Xt(0, i) = 1;
137  Xt(1, i) = x[i] - x_min;
138  }
139 
140  const auto B =
141  ((Xt * Xt.transpose()).inverse().eval() * Xt * y.asEigen()).eval();
142  ASSERT_(B.size() == 2);
143 
144  NUMTYPE ret = B[0] + B[1] * (t - x_min);
145 
146  // wrap?
147  if (!wrap2pi)
148  return ret;
149  else
150  return mrpt::math::wrapToPi(ret);
151 
152  MRPT_END
153 }
154 
155 template <
156  class VECTORLIKE1, class VECTORLIKE2, class VECTORLIKE3, int NUM_POINTS>
158  const VECTORLIKE1& ts, VECTORLIKE2& outs, const VECTORLIKE3& x,
159  const VECTORLIKE3& y, bool wrap2pi)
160 {
161  MRPT_START
162 
163  // http://en.wikipedia.org/wiki/Linear_least_squares
164  ASSERT_(x.size() == y.size());
165  ASSERT_(x.size() > 1);
166 
167  const size_t N = x.size();
168 
169  // X= [1 columns of ones, x' ]
170  using NUM = typename VECTORLIKE3::value_type;
171  const NUM x_min = x.minCoeff();
173  Xt.resize(2, N);
174  for (size_t i = 0; i < N; i++)
175  {
176  Xt(0, i) = 1;
177  Xt(1, i) = x[i] - x_min;
178  }
179 
180  // (Xt * Xt.transpose()).inverse_LLt().eval() * Xt * y;
182  XXt.matProductOf_AAt(Xt);
183  const auto XXt_inv = XXt.inverse_LLt();
184  const auto XXt_inv_X = XXt_inv * Xt;
186  B.matProductOf_Ab(XXt_inv_X, y);
187  ASSERT_(B.size() == 2);
188 
189  const size_t tsN = size_t(ts.size());
190  outs.resize(tsN);
191  if (!wrap2pi)
192  for (size_t k = 0; k < tsN; k++)
193  outs[k] = B[0] + B[1] * (ts[k] - x_min);
194  else
195  for (size_t k = 0; k < tsN; k++)
196  outs[k] = mrpt::math::wrapToPi(B[0] + B[1] * (ts[k] - x_min));
197  MRPT_END
198 }
199 } // namespace mrpt::math
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:119
#define MRPT_START
Definition: exceptions.h:241
void resize(size_t row, size_t col)
#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:17
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void matProductOf_Ab(const CMatrixDynamic< T > &A, const CVectorDynamic< T > &b)
this = A * b , with A and b a dynamic matrix & vector
CMatrixDynamic< T > inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
#define MRPT_END
Definition: exceptions.h:245
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
This template class provides the basic functionality for a general 2D any-size, resizable container o...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020