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
14#include <mrpt/math/wrap2pi.h>
15
16namespace 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;
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
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 {
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) );
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.6 for MRPT 1.4.0 SVN: at Tue Jan 17 22:40:29 UTC 2023