Main MRPT website > C++ reference for MRPT 1.4.0
interp_fit.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, 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 #ifndef MRPT_MATH_FIT_INTERP_H
10 #define MRPT_MATH_FIT_INTERP_H
11 
12 #include <mrpt/utils/utils_defs.h>
14 #include <mrpt/math/wrap2pi.h>
15 
16 namespace mrpt
17 {
18  namespace math
19  {
20 
21  /** @addtogroup interpolation_grp Interpolation, least-squares fit, splines
22  * \ingroup mrpt_base_grp
23  * @{ */
24 
25  /** Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximation of the sequence at the point "x".
26  * If the point "x" is out of the range [x0,x1], the closest extreme "ys" value is returned.
27  * \sa spline, interpolate2points
28  */
29  template <class T,class VECTOR>
31  const T &x,
32  const VECTOR &ys,
33  const T &x0,
34  const T &x1 )
35  {
37  ASSERT_(x1>x0); ASSERT_(!ys.empty());
38  const size_t N = ys.size();
39  if (x<=x0) return ys[0];
40  if (x>=x1) return ys[N-1];
41  const T Ax = (x1-x0)/T(N);
42  const size_t i = int( (x-x0)/Ax );
43  if (i>=N-1) return ys[N-1];
44  const T Ay = ys[i+1]-ys[i];
45  return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
46  MRPT_END
47  }
48 
49  /** Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
50  * If wrap2pi is true, output is wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
51  * \sa spline, interpolate, leastSquareLinearFit
52  */
53  double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
54 
55  /** Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the two middle points
56  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
57  * \sa leastSquareLinearFit
58  */
59  double BASE_IMPEXP spline(const double t, const CVectorDouble &x, const CVectorDouble &y, bool wrap2pi = false);
60 
61  /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a single point "t".
62  * The vectors x and y must have size >=2, and all values of "x" must be different.
63  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
64  * \sa spline
65  * \sa getRegressionLine, getRegressionPlane
66  */
67  template <typename NUMTYPE,class VECTORLIKE>
68  NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi = false)
69  {
71 
72  // http://en.wikipedia.org/wiki/Linear_least_squares
73  ASSERT_(x.size()==y.size());
74  ASSERT_(x.size()>1);
75 
76  const size_t N = x.size();
77 
78  typedef typename VECTORLIKE::Scalar NUM;
79 
80  // X= [1 columns of ones, x' ]
81  const NUM x_min = x.minimum();
83  for (size_t i=0;i<N;i++)
84  {
85  Xt.set_unsafe(0,i, 1);
86  Xt.set_unsafe(1,i, x[i]-x_min);
87  }
88 
90  XtX.multiply_AAt(Xt);
91 
93  XtX.inv_fast(XtXinv);
94 
95  mrpt::math::CMatrixTemplateNumeric<NUM> XtXinvXt; // B = inv(X' * X)*X' (pseudoinverse)
96  XtXinvXt.multiply(XtXinv,Xt);
97 
98  VECTORLIKE B;
99  XtXinvXt.multiply_Ab(y,B);
100 
101  ASSERT_(B.size()==2)
102 
103  NUM ret = B[0] + B[1]*(t-x_min);
104 
105  // wrap?
106  if (!wrap2pi)
107  return ret;
108  else return mrpt::math::wrapToPi(ret);
109 
110  MRPT_END
111  }
112 
113  /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a sequence of points "ts" and returned at "outs".
114  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
115  * \sa spline, getRegressionLine, getRegressionPlane
116  */
117  template <class VECTORLIKE1,class VECTORLIKE2,class VECTORLIKE3>
119  const VECTORLIKE1 &ts,
120  VECTORLIKE2 &outs,
121  const VECTORLIKE3 &x,
122  const VECTORLIKE3 &y,
123  bool wrap2pi = false)
124  {
125  MRPT_START
126 
127  // http://en.wikipedia.org/wiki/Linear_least_squares
128  ASSERT_(x.size()==y.size());
129  ASSERT_(x.size()>1);
130 
131  const size_t N = x.size();
132 
133  // X= [1 columns of ones, x' ]
134  typedef typename VECTORLIKE3::Scalar NUM;
135  const NUM x_min = x.minimum();
137  for (size_t i=0;i<N;i++)
138  {
139  Xt.set_unsafe(0,i, 1);
140  Xt.set_unsafe(1,i, x[i]-x_min);
141  }
142 
144  XtX.multiply_AAt(Xt);
145 
147  XtX.inv_fast(XtXinv);
148 
149  mrpt::math::CMatrixTemplateNumeric<NUM> XtXinvXt; // B = inv(X' * X)*X' (pseudoinverse)
150  XtXinvXt.multiply(XtXinv,Xt);
151 
152  VECTORLIKE3 B;
153  XtXinvXt.multiply_Ab(y,B);
154 
155  ASSERT_(B.size()==2)
156 
157  const size_t tsN = size_t(ts.size());
158  outs.resize(tsN);
159  if (!wrap2pi)
160  for (size_t k=0;k<tsN;k++)
161  outs[k] = B[0] + B[1]*(ts[k]-x_min);
162  else
163  for (size_t k=0;k<tsN;k++)
164  outs[k] = mrpt::math::wrapToPi( B[0] + B[1]*(ts[k]-x_min) );
165  MRPT_END
166  }
167 
168  /** @} */ // end grouping interpolation_grp
169 
170 
171  } // End of MATH namespace
172 } // End of namespace
173 
174 #endif
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
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.h:68
double BASE_IMPEXP spline(const double t, const CVectorDouble &x, const CVectorDouble &y, bool wrap2pi=false)
Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the tw...
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.h:30
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_END
Definition: mrpt_macros.h:353
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:37:47 UTC 2022