Go to the documentation of this file.
9 #ifndef MRPT_MATH_FIT_INTERP_H
10 #define MRPT_MATH_FIT_INTERP_H
29 template <
class T,
class VECTOR>
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;
53 double BASE_IMPEXP interpolate2points(
const double x,
const double x0,
const double y0,
const double x1,
const double y1,
bool wrap2pi =
false);
67 template <
typename NUMTYPE,
class VECTORLIKE>
76 const size_t N = x.size();
78 typedef typename VECTORLIKE::Scalar NUM;
81 const NUM x_min = x.minimum();
83 for (
size_t i=0;i<N;i++)
85 Xt.set_unsafe(0,i, 1);
86 Xt.set_unsafe(1,i, x[i]-x_min);
96 XtXinvXt.multiply(XtXinv,Xt);
99 XtXinvXt.multiply_Ab(y,B);
103 NUM ret = B[0] + B[1]*(
t-x_min);
117 template <
class VECTORLIKE1,
class VECTORLIKE2,
class VECTORLIKE3>
119 const VECTORLIKE1 &ts,
121 const VECTORLIKE3 &x,
122 const VECTORLIKE3 &y,
123 bool wrap2pi =
false)
131 const size_t N = x.size();
134 typedef typename VECTORLIKE3::Scalar NUM;
135 const NUM x_min = x.minimum();
137 for (
size_t i=0;i<N;i++)
139 Xt.set_unsafe(0,i, 1);
140 Xt.set_unsafe(1,i, x[i]-x_min);
144 XtX.multiply_AAt(Xt);
147 XtX.inv_fast(XtXinv);
150 XtXinvXt.multiply(XtXinv,Xt);
153 XtXinvXt.multiply_Ab(y,B);
157 const size_t tsN = size_t(ts.size());
160 for (
size_t k=0;k<tsN;k++)
161 outs[k] = B[0] + B[1]*(ts[k]-x_min);
163 for (
size_t k=0;k<tsN;k++)
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction.
const EIGEN_STRONG_INLINE AdjointReturnType t() const
Transpose.
A matrix of dynamic size.
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",...
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 wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Page generated by Doxygen 1.8.17 for MRPT 1.4.0 SVN: at Tue Mar 3 09:15:16 UTC 2020 | | |