Main MRPT website > C++ reference for MRPT 1.5.7
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-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 #pragma once
10 
11 #include <mrpt/math/interp_fit.h>
12 
13 namespace mrpt {
14 namespace math {
15 
16 template <class T,class VECTOR>
18  const T &x,
19  const VECTOR &ys,
20  const T &x0,
21  const T &x1 )
22 {
24  ASSERT_(x1>x0); ASSERT_(!ys.empty());
25  const size_t N = ys.size();
26  if (x<=x0) return ys[0];
27  if (x>=x1) return ys[N-1];
28  const T Ax = (x1-x0)/T(N);
29  const size_t i = int( (x-x0)/Ax );
30  if (i>=N-1) return ys[N-1];
31  const T Ay = ys[i+1]-ys[i];
32  return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
33  MRPT_END
34 }
35 
36 template <typename NUMTYPE, class VECTORLIKE>
37 NUMTYPE spline(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++)
46  h[i] = x[i + 1] - x[i];
47 
48  double k = 1 / (4 * h[0] * h[1] + 4 * h[0] * h[2] + 3 * h[1] * h[1] + 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]) + (y1 / h[0] - h[0] / 6 * z1)*(t - x[0]) + (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)) / (6 * h[1]) + (y2 / h[1] - h[1] / 6 * z2)*(t - x[1]) + (y1 / h[1] - h[1] / 6 * z1)*(x[2] - t);
106  else
107  if (t < x[3])
108  res = (z3*pow((t - x[2]), 3) + z2*pow((x[3] - t), 3)) / (6 * h[2]) + (y3 / h[2] - h[2] / 6 * z3)*(t - x[2]) + (y2 / h[2] - h[2] / 6 * z2)*(x[3] - t);
109  }
110  return wrap2pi ? mrpt::math::wrapToPi(res) : res;
111 }
112 
113 template <typename NUMTYPE,class VECTORLIKE, int NUM_POINTS>
114 NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi)
115 {
116  MRPT_START
117 
118  // http://en.wikipedia.org/wiki/Linear_least_squares
119  ASSERT_(x.size()==y.size());
120  ASSERT_(x.size()>1);
121 
122  const size_t N = x.size();
123 
124  // X= [1 columns of ones, x' ]
125  const NUMTYPE x_min = x.minimum();
126  Eigen::Matrix<NUMTYPE, 2, NUM_POINTS> Xt;
127  Xt.resize(2,N);
128  for (size_t i=0;i<N;i++)
129  {
130  Xt.set_unsafe(0,i, 1);
131  Xt.set_unsafe(1,i, x[i]-x_min);
132  }
133 
134  const auto B = ((Xt*Xt.transpose()).inv().eval()*Xt*y).eval();
135  ASSERT_(B.size()==2)
136 
137  NUMTYPE ret = B[0] + B[1]*(t-x_min);
138 
139  // wrap?
140  if (!wrap2pi)
141  return ret;
142  else return mrpt::math::wrapToPi(ret);
143 
144  MRPT_END
145 }
146 
147 template <class VECTORLIKE1,class VECTORLIKE2,class VECTORLIKE3, int NUM_POINTS>
148 void leastSquareLinearFit( const VECTORLIKE1 &ts,
149  VECTORLIKE2 &outs,
150  const VECTORLIKE3 &x,
151  const VECTORLIKE3 &y,
152  bool wrap2pi)
153 {
154  MRPT_START
155 
156  // http://en.wikipedia.org/wiki/Linear_least_squares
157  ASSERT_(x.size()==y.size());
158  ASSERT_(x.size()>1);
159 
160  const size_t N = x.size();
161 
162  // X= [1 columns of ones, x' ]
163  typedef typename VECTORLIKE3::value_type NUM;
164  const NUM x_min = x.minimum();
165  Eigen::Matrix<NUM, 2, NUM_POINTS> Xt;
166  Xt.resize(2, N);
167  for (size_t i=0;i<N;i++)
168  {
169  Xt.set_unsafe(0,i, 1);
170  Xt.set_unsafe(1,i, x[i]-x_min);
171  }
172 
173  const auto B = ((Xt*Xt.transpose()).inv().eval()*Xt*y).eval();
174  ASSERT_(B.size() == 2)
175 
176  const size_t tsN = size_t(ts.size());
177  outs.resize(tsN);
178  if (!wrap2pi)
179  for (size_t k=0;k<tsN;k++)
180  outs[k] = B[0] + B[1]*(ts[k]-x_min);
181  else
182  for (size_t k=0;k<tsN;k++)
183  outs[k] = mrpt::math::wrapToPi( B[0] + B[1]*(ts[k]-x_min) );
184  MRPT_END
185 }
186 
187 } // end ns: math
188 } // end ns: 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:114
GLdouble GLdouble t
Definition: glext.h:3610
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
#define M_PI
Definition: bits.h:78
#define M_2PI
Definition: mrpt_macros.h:380
#define MRPT_END
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
#define MRPT_START
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
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
EIGEN_STRONG_INLINE PlainObject inv() const



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