Commit d1e5c51f authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[algorithm] Added CubicSpline interpolation

parent 6b66d564
...@@ -13,7 +13,7 @@ namespace rtac { namespace algorithm { ...@@ -13,7 +13,7 @@ namespace rtac { namespace algorithm {
/** /**
* Abstract base class representing a generic interpolator. * Abstract base class representing a generic interpolator.
*/ */
template <typename T, template<typename> class VectorT = std::vector> template <typename T>
class Interpolator class Interpolator
{ {
public: public:
...@@ -21,8 +21,8 @@ class Interpolator ...@@ -21,8 +21,8 @@ class Interpolator
using Ptr = types::Handle<Interpolator>; using Ptr = types::Handle<Interpolator>;
using ConstPtr = types::Handle<const Interpolator>; using ConstPtr = types::Handle<const Interpolator>;
using Indexes = VectorT<unsigned int>; using Indexes = types::Vector<unsigned int>;
using Vector = VectorT<T>; using Vector = types::Vector<T>;
using Xconst_iterator = typename Vector::const_iterator; using Xconst_iterator = typename Vector::const_iterator;
...@@ -36,9 +36,11 @@ class Interpolator ...@@ -36,9 +36,11 @@ class Interpolator
public: public:
Vector operator()(const Vector& x) const; Vector operator()(const Vector& x) const;
unsigned int size() const;
Xconst_iterator lower_bound(T x) const; Xconst_iterator lower_bound(T x) const;
std::vector<Xconst_iterator> lower_bound(const Vector& x) const; std::vector<Xconst_iterator> lower_bound(const Vector& x) const;
Indexes lower_bound_indexes(const Vector& x) const;
/** /**
* Core interpolating method. To be reimplemented in subclasses. * Core interpolating method. To be reimplemented in subclasses.
...@@ -52,13 +54,13 @@ class Interpolator ...@@ -52,13 +54,13 @@ class Interpolator
/** /**
* Nearest-Neighbor interpolator. * Nearest-Neighbor interpolator.
*/ */
template <typename T, template<typename>class VectorT = std::vector> template <typename T>
class InterpolatorNearest : public Interpolator<T,VectorT> class InterpolatorNearest : public Interpolator<T>
{ {
public: public:
using Indexes = typename Interpolator<T,VectorT>::Indexes; using Indexes = typename Interpolator<T>::Indexes;
using Vector = typename Interpolator<T,VectorT>::Vector; using Vector = typename Interpolator<T>::Vector;
public: public:
...@@ -70,13 +72,13 @@ class InterpolatorNearest : public Interpolator<T,VectorT> ...@@ -70,13 +72,13 @@ class InterpolatorNearest : public Interpolator<T,VectorT>
/** /**
* Linear interpolator. * Linear interpolator.
*/ */
template <typename T, template<typename>class VectorT = std::vector> template <typename T>
class InterpolatorLinear : public Interpolator<T,VectorT> class InterpolatorLinear : public Interpolator<T>
{ {
public: public:
using Indexes = typename Interpolator<T,VectorT>::Indexes; using Indexes = typename Interpolator<T>::Indexes;
using Vector = typename Interpolator<T,VectorT>::Vector; using Vector = typename Interpolator<T>::Vector;
public: public:
...@@ -85,11 +87,40 @@ class InterpolatorLinear : public Interpolator<T,VectorT> ...@@ -85,11 +87,40 @@ class InterpolatorLinear : public Interpolator<T,VectorT>
virtual void interpolate(const Vector& x, Vector& output) const; virtual void interpolate(const Vector& x, Vector& output) const;
}; };
/**
* Cubic spline interpolator.
*
* y = an_.(x-xn)**3 + bn_.(x-xn)**2 + cn_.(x-xn) + dn_
*/
template <typename T>
class InterpolatorCubicSpline : public Interpolator<T>
{
public:
using Indexes = typename Interpolator<T>::Indexes;
using Vector = typename Interpolator<T>::Vector;
protected:
Vector a_;
Vector b_;
Vector c_;
Vector d_;
public:
InterpolatorCubicSpline(const Vector& x0, const Vector& y0);
virtual void interpolate(const Vector& x, Vector& output) const;
};
// Interpolator IMPLEMENTATION ////////////////////////////////////////// // Interpolator IMPLEMENTATION //////////////////////////////////////////
template <typename T, template<typename>class VectorT> template <typename T>
Interpolator<T,VectorT>::Interpolator(const Vector& x0, const Vector& y0) : Interpolator<T>::Interpolator(const Vector& x0, const Vector& y0) :
x0_(x0), y0_(y0) x0_(x0), y0_(y0)
{} {
assert(x0_.size() == y0_.size());
}
/** /**
* Interpolate at values x. * Interpolate at values x.
...@@ -98,21 +129,31 @@ Interpolator<T,VectorT>::Interpolator(const Vector& x0, const Vector& y0) : ...@@ -98,21 +129,31 @@ Interpolator<T,VectorT>::Interpolator(const Vector& x0, const Vector& y0) :
* *
* @return Interpolated values. * @return Interpolated values.
*/ */
template <typename T, template<typename>class VectorT> template <typename T>
typename Interpolator<T,VectorT>::Vector Interpolator<T,VectorT>::operator()(const Vector& x) const typename Interpolator<T>::Vector Interpolator<T>::operator()(const Vector& x) const
{ {
Vector output(x.size()); Vector output(x.size());
this->interpolate(x, output); this->interpolate(x, output);
return output; return output;
} }
/**
* @return number of data element which are interpolated (= size of origin data
* vectors)
*/
template <typename T>
unsigned int Interpolator<T>::size() const
{
return x0_.size();
}
/** /**
* Find an iterator in x0_ the closest below or equal x. * Find an iterator in x0_ the closest below or equal x.
* *
* throws a std::range error if such iterator could not be found. * throws a std::range error if such iterator could not be found.
*/ */
template <typename T, template<typename>class VectorT> template <typename T>
typename Interpolator<T,VectorT>::Xconst_iterator Interpolator<T,VectorT>::lower_bound(T x) const typename Interpolator<T>::Xconst_iterator Interpolator<T>::lower_bound(T x) const
{ {
auto it = std::lower_bound(x0_.begin(), x0_.end(), x); auto it = std::lower_bound(x0_.begin(), x0_.end(), x);
if(it == x0_.end() || it == x0_.begin() && *it > x) { if(it == x0_.end() || it == x0_.begin() && *it > x) {
...@@ -134,9 +175,9 @@ typename Interpolator<T,VectorT>::Xconst_iterator Interpolator<T,VectorT>::lower ...@@ -134,9 +175,9 @@ typename Interpolator<T,VectorT>::Xconst_iterator Interpolator<T,VectorT>::lower
* @return a vector of iterators pointing to valid values in x0_ below of equal * @return a vector of iterators pointing to valid values in x0_ below of equal
* to x values (a std::range_error is throwed if am iterator is not valid). * to x values (a std::range_error is throwed if am iterator is not valid).
*/ */
template <typename T, template<typename>class VectorT> template <typename T>
std::vector<typename Interpolator<T,VectorT>::Xconst_iterator> std::vector<typename Interpolator<T>::Xconst_iterator>
Interpolator<T,VectorT>::lower_bound(const Vector& x) const Interpolator<T>::lower_bound(const Vector& x) const
{ {
std::vector<Xconst_iterator> output(x.size()); std::vector<Xconst_iterator> output(x.size());
for(int i = 0; i < output.size(); i++) { for(int i = 0; i < output.size(); i++) {
...@@ -145,51 +186,113 @@ std::vector<typename Interpolator<T,VectorT>::Xconst_iterator> ...@@ -145,51 +186,113 @@ std::vector<typename Interpolator<T,VectorT>::Xconst_iterator>
return output; return output;
} }
/**
* Retrieve indexes to the x0_ elements just below or equal to a value, for
* each value in x.
*
* @return a vector of indexes pointing values in x0_ below of equal to x
* values (a std::range_error is throwed if an iterator is not valid).
*/
template <typename T>
typename Interpolator<T>::Indexes
Interpolator<T>::lower_bound_indexes(const Vector& x) const
{
Indexes output(x.size());
for(int i = 0; i < output.size(); i++) {
// not working
//output[i] = std::distance(this->lower_bound(x[i]), this->x0_.begin());
output[i] = this->lower_bound(x[i]) - this->x0_.begin();
}
return output;
}
// InterpolatorNearest IMPLEMENTATION ////////////////////////////////////////// // InterpolatorNearest IMPLEMENTATION //////////////////////////////////////////
template <typename T, template<typename>class VectorT> template <typename T>
InterpolatorNearest<T,VectorT>::InterpolatorNearest(const Vector& x0, const Vector& y0) : InterpolatorNearest<T>::InterpolatorNearest(const Vector& x0, const Vector& y0) :
Interpolator<T,VectorT>(x0, y0) Interpolator<T>(x0, y0)
{} {}
template <typename T, template<typename>class VectorT> template <typename T>
void InterpolatorNearest<T,VectorT>::interpolate(const Vector& x, Vector& output) const void InterpolatorNearest<T>::interpolate(const Vector& x, Vector& output) const
{ {
using namespace rtac::types::indexing; using namespace rtac::types::indexing;
auto iterators = this->lower_bound(x); Indexes idx = this->lower_bound_indexes(x);
for(int i = 0; i < x.size(); i++) { for(int i = 0; i < x.size(); i++) {
if(iterators[i] == this->x0_.end() - 1) { if(idx[i] == this->x0_.size() - 1) {
output[i] = *(this->y0_.end() - 1); output[i] = this->y0_[idx[i]];
continue; continue;
} }
unsigned int idx = iterators[i] - this->x0_.begin(); if(x[i] - this->x0_[idx[i]] <= this->x0_[idx[i] + 1] - x[i])
if(x[i] - this->x0_[idx] <= this->x0_[idx + 1] - x[i]) output[i] = this->y0_[idx[i]];
output[i] = this->y0_[idx];
else else
output[i] = this->y0_[idx + 1]; output[i] = this->y0_[idx[i] + 1];
} }
} }
// InterpolatorLinear IMPLEMENTATION ////////////////////////////////////////// // InterpolatorLinear IMPLEMENTATION //////////////////////////////////////////
template <typename T, template<typename>class VectorT> template <typename T>
InterpolatorLinear<T,VectorT>::InterpolatorLinear(const Vector& x0, const Vector& y0) : InterpolatorLinear<T>::InterpolatorLinear(const Vector& x0, const Vector& y0) :
Interpolator<T,VectorT>(x0, y0) Interpolator<T>(x0, y0)
{} {}
template <typename T, template<typename>class VectorT> template <typename T>
void InterpolatorLinear<T,VectorT>::interpolate(const Vector& x, Vector& output) const void InterpolatorLinear<T>::interpolate(const Vector& x, Vector& output) const
{
using namespace rtac::types::indexing;
Indexes idx = this->lower_bound_indexes(x);
for(int i = 0; i < x.size(); i++) {
if(idx[i] == this->x0_.size() - 1) {
output[i] = this->y0_[idx[i]];
continue;
}
T lambda = (x[i] - this->x0_[idx[i]])
/ (this->x0_[idx[i] + 1] - this->x0_[idx[i]]);
output[i] = (1.0 - lambda)*this->y0_[idx[i]] + lambda*this->y0_[idx[i] + 1];
}
}
// InterpolatorCubicSpline IMPLEMENTATION //////////////////////////////////////////
template <typename T>
InterpolatorCubicSpline<T>::InterpolatorCubicSpline(const Vector& x0, const Vector& y0) :
Interpolator<T>(x0, y0)//,
//a_(x0.size()-1), b_(x0.size()-1), c_(x0.size()-1), d_(x0.size()-1)
{
using namespace rtac::types::indexing;
Vector dx = x0(seq(1,last)) - x0(seq(0,last-1));
Vector dy = (y0(seq(1,last)) - y0(seq(0,last-1))).array() / dx.array();
Vector beta = 6.0*(dy(seq(1,last)) - dy(seq(0,last-1)));
types::Matrix<T> A = (2.0*(x0(seq(2,last)) - x0(seq(0,last-2)))).asDiagonal();
for(int i = 0; i < this->size() - 2; i++) {
A(i,i+1) = dx(i+1);
A(i+1,i) = dx(i+1);
}
Vector alpha(this->size());
alpha(seq(1,last-1)) = A.colPivHouseholderQr().solve(beta);
alpha(0) = 0.0;
alpha(alpha.size() - 1) = 0.0;
this->a_ = (alpha(seq(1,last)) - alpha(seq(0,last-1))).array() / (6.0*dx.array());
this->b_ = 0.5*alpha(seq(1,last));
this->c_ = dy.array()
+ dx.array() * (2.0*alpha(seq(1,last)) + alpha(seq(0,last-1))).array() / 6.0;
this->d_ = y0(seq(1,last));
}
template <typename T>
void InterpolatorCubicSpline<T>::interpolate(const Vector& x, Vector& output) const
{ {
using namespace rtac::types::indexing; using namespace rtac::types::indexing;
auto iterators = this->lower_bound(x); Indexes idx = this->lower_bound_indexes(x);
for(int i = 0; i < x.size(); i++) { for(int i = 0; i < x.size(); i++) {
if(iterators[i] == this->x0_.end() - 1) { if(idx[i] == this->x0_.size() - 1) {
output[i] = *(this->y0_.end() - 1); output[i] = this->y0_[idx[i]];
continue; continue;
} }
unsigned int idx = iterators[i] - this->x0_.begin(); T v = x[i] - this->x0_[idx[i] + 1];
T lambda = (x[i] - this->x0_[idx]) output[i] = a_[idx[i]]*v*v*v + b_[idx[i]]*v*v + c_[idx[i]]*v + d_[idx[i]];
/ (this->x0_[idx + 1] - this->x0_[idx]);
output[i] = (1.0 - lambda)*this->y0_[idx] + lambda*this->y0_[idx + 1];
} }
} }
......
...@@ -6,9 +6,7 @@ namespace plt = matplotlibcpp; ...@@ -6,9 +6,7 @@ namespace plt = matplotlibcpp;
#include <rtac_base/types/common.h> #include <rtac_base/types/common.h>
#include <rtac_base/interpolation.h> #include <rtac_base/interpolation.h>
//using namespace rtac::types;
template <typename T> template <typename T>
//using Vector = std::vector<T>;
using Vector = rtac::types::Vector<T>; using Vector = rtac::types::Vector<T>;
using namespace rtac::algorithm; using namespace rtac::algorithm;
using namespace rtac::types::indexing; using namespace rtac::types::indexing;
...@@ -74,18 +72,20 @@ int main() ...@@ -74,18 +72,20 @@ int main()
auto x0 = data(all,0); auto x0 = data(all,0);
auto y0 = data(all,1); auto y0 = data(all,1);
auto x = linspace<Vector<float>>(data(0,0), data(last,0), 512); auto x = linspace<Vector<float>>(data(0,0), data(last,0), 8192);
InterpolatorNearest<float, Vector> interpNN(x0, y0); InterpolatorNearest<float> interpNN(x0, y0);
//InterpolatorNearest<float, Vector> interpNN(to_vector(x0), to_vector(y0));
auto ynn = interpNN(x); auto ynn = interpNN(x);
InterpolatorLinear<float, Vector> interpLinear(x0, y0); InterpolatorLinear<float> interpLinear(x0, y0);
//InterpolatorLinear<float, Vector> interpLinear(to_vector(x0), to_vector(y0));
auto yl = interpLinear(x); auto yl = interpLinear(x);
InterpolatorCubicSpline<float> interpCubicSpline(x0, y0);
auto yc = interpCubicSpline(x);
plt::plot(to_vector(x0), to_vector(y0), {{"label", "original"}}); plt::plot(to_vector(x0), to_vector(y0), {{"marker", "o"},
{"label", "original"}});
plt::plot(to_vector(x), to_vector(ynn), {{"label", "nearest"}}); plt::plot(to_vector(x), to_vector(ynn), {{"label", "nearest"}});
plt::plot(to_vector(x), to_vector(yl), {{"label", "linear"}}); plt::plot(to_vector(x), to_vector(yl), {{"label", "linear"}});
plt::plot(to_vector(x), to_vector(yc), {{"label", "cubic"}});
plt::legend(); plt::legend();
plt::show(); plt::show();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment