Skip to content

GitLab

Commit 6b66d564 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[algorithm] Backed to interpolation of 1D data (for consistency with spline interpolation)

parent 6c027e11
Hide whitespace changes
Inline Side-by-side
include/rtac_base/interpolation.h
View file @ 6b66d564
......@@ -13,7 +13,7 @@ namespace rtac { namespace algorithm {
/**
* Abstract base class representing a generic interpolator.
*/
template <typename T>
template <typename T, template<typename> class VectorT = std::vector>
class Interpolator
{
public:
......@@ -21,27 +21,24 @@ class Interpolator
using Ptr = types::Handle<Interpolator>;
using ConstPtr = types::Handle<const Interpolator>;
using Indexes = types::Vector<unsigned int>;
using Vector = types::Vector<T>;
using Matrix = types::Matrix<T>;
using Indexes = VectorT<unsigned int>;
using Vector = VectorT<T>;
using Xconst_iterator = typename Vector::const_iterator;
protected:
types::Handle<const Vector> x0_;
types::Handle<const Matrix> y0_;
Vector x0_;
Vector y0_;
Interpolator(const types::Handle<const Vector>& x0, const types::Handle<const Matrix>& y0);
Interpolator(const Vector& x0, const Vector& y0);
public:
unsigned int output_dimension() const;
Matrix operator()(const Vector& x) const;
Vector operator()(const Vector& x) const;
Xconst_iterator lower_bound(T x) const;
template <class VectorT>
std::vector<Xconst_iterator> lower_bound(const VectorT& x) const;
std::vector<Xconst_iterator> lower_bound(const Vector& x) const;
/**
* Core interpolating method. To be reimplemented in subclasses.
......@@ -49,57 +46,48 @@ class Interpolator
* @param x values where to interpolate.
* @param output matrix where to write the interpolated values.
*/
virtual void interpolate(const Vector& x, Matrix& output) const = 0;
virtual void interpolate(const Vector& x, Vector& output) const = 0;
};
/**
* Nearest-Neighbor interpolator.
*/
template <typename T>
class InterpolatorNearest : public Interpolator<T>
template <typename T, template<typename>class VectorT = std::vector>
class InterpolatorNearest : public Interpolator<T,VectorT>
{
public:
using Indexes = typename Interpolator<T>::Indexes;
using Vector = typename Interpolator<T>::Vector;
using Matrix = typename Interpolator<T>::Matrix;
using Indexes = typename Interpolator<T,VectorT>::Indexes;
using Vector = typename Interpolator<T,VectorT>::Vector;
public:
InterpolatorNearest(const types::Handle<const Vector>& x0,
const types::Handle<const Matrix>& y0);
InterpolatorNearest(const Vector& x0,
const Matrix& y0);
InterpolatorNearest(const Vector& x0, const Vector& y0);
virtual void interpolate(const Vector& x, Matrix& output) const;
virtual void interpolate(const Vector& x, Vector& output) const;
};
/**
* Linear interpolator.
*/
template <typename T>
class InterpolatorLinear : public Interpolator<T>
template <typename T, template<typename>class VectorT = std::vector>
class InterpolatorLinear : public Interpolator<T,VectorT>
{
public:
using Indexes = typename Interpolator<T>::Indexes;
using Vector = typename Interpolator<T>::Vector;
using Matrix = typename Interpolator<T>::Matrix;
using Indexes = typename Interpolator<T,VectorT>::Indexes;
using Vector = typename Interpolator<T,VectorT>::Vector;
public:
InterpolatorLinear(const types::Handle<const Vector>& x0,
const types::Handle<const Matrix>& y0);
InterpolatorLinear(const Vector& x0,
const Matrix& y0);
InterpolatorLinear(const Vector& x0, const Vector& y0);
virtual void interpolate(const Vector& x, Matrix& output) const;
virtual void interpolate(const Vector& x, Vector& output) const;
};
// Interpolator IMPLEMENTATION //////////////////////////////////////////
template <typename T>
Interpolator<T>::Interpolator(const types::Handle<const Vector>& x0,
const types::Handle<const Matrix>& y0) :
template <typename T, template<typename>class VectorT>
Interpolator<T,VectorT>::Interpolator(const Vector& x0, const Vector& y0) :
x0_(x0), y0_(y0)
{}
......@@ -110,38 +98,27 @@ Interpolator<T>::Interpolator(const types::Handle<const Vector>& x0,
*
* @return Interpolated values.
*/
template <typename T>
typename Interpolator<T>::Matrix Interpolator<T>::operator()(const Vector& x) const
template <typename T, template<typename>class VectorT>
typename Interpolator<T,VectorT>::Vector Interpolator<T,VectorT>::operator()(const Vector& x) const
{
Matrix output(x.rows(), this->output_dimension());
Vector output(x.size());
this->interpolate(x, output);
return output;
}
/**
* Expected dimension of the interpolated values.
*
* (Interpolated values can be vectors).
*/
template <typename T>
unsigned int Interpolator<T>::output_dimension() const
{
return y0_->cols();
}
/**
* Find an iterator in x0_ the closest below or equal x.
*
* throws a std::range error if such iterator could not be found.
*/
template <typename T>
typename Interpolator<T>::Xconst_iterator Interpolator<T>::lower_bound(T x) const
template <typename T, template<typename>class VectorT>
typename Interpolator<T,VectorT>::Xconst_iterator Interpolator<T,VectorT>::lower_bound(T x) const
{
auto it = std::lower_bound(x0_->begin(), x0_->end(), x);
if(it == x0_->end() || it == x0_->begin() && *it > x) {
auto it = std::lower_bound(x0_.begin(), x0_.end(), x);
if(it == x0_.end() || it == x0_.begin() && *it > x) {
std::ostringstream oss;
oss << "Iterator : a requested input value is not in input range ("
<< "range is [" << (*x0_)(0) << "-" << (*x0_)(Eigen::last)
<< "range is [" << x0_[0] << "-" << *(x0_.end() - 1)
<< "], got " << x << ").";
throw std::range_error(oss.str());
}
......@@ -157,77 +134,62 @@ typename Interpolator<T>::Xconst_iterator Interpolator<T>::lower_bound(T x) cons
* @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).
*/
template <typename T> template <class VectorT>
std::vector<typename Interpolator<T>::Xconst_iterator>
Interpolator<T>::lower_bound(const VectorT& x) const
template <typename T, template<typename>class VectorT>
std::vector<typename Interpolator<T,VectorT>::Xconst_iterator>
Interpolator<T,VectorT>::lower_bound(const Vector& x) const
{
std::vector<Xconst_iterator> output(x.rows());
std::vector<Xconst_iterator> output(x.size());
for(int i = 0; i < output.size(); i++) {
output[i] = this->lower_bound(x(i));
output[i] = this->lower_bound(x[i]);
}
return output;
}
// InterpolatorNearest IMPLEMENTATION //////////////////////////////////////////
template <typename T>
InterpolatorNearest<T>::InterpolatorNearest(const types::Handle<const Vector>& x0,
const types::Handle<const Matrix>& y0) :
Interpolator<T>(x0, y0)
{}
template <typename T>
InterpolatorNearest<T>::InterpolatorNearest(const Vector& x0, const Matrix& y0) :
InterpolatorNearest<T>(types::Handle<Vector>(new Vector(x0)),
types::Handle<Matrix>(new Matrix(y0)))
template <typename T, template<typename>class VectorT>
InterpolatorNearest<T,VectorT>::InterpolatorNearest(const Vector& x0, const Vector& y0) :
Interpolator<T,VectorT>(x0, y0)
{}
template <typename T>
void InterpolatorNearest<T>::interpolate(const Vector& x, Matrix& output) const
template <typename T, template<typename>class VectorT>
void InterpolatorNearest<T,VectorT>::interpolate(const Vector& x, Vector& output) const
{
using namespace rtac::types::indexing;
auto iterators = this->lower_bound(x);
for(int i = 0; i < x.rows(); i++) {
if(iterators[i] == this->x0_->end() - 1) {
output(i,all) = (*this->y0_)(last, all);
for(int i = 0; i < x.size(); i++) {
if(iterators[i] == this->x0_.end() - 1) {
output[i] = *(this->y0_.end() - 1);
continue;
}
unsigned int idx = iterators[i] - this->x0_->begin();
if(x(i) - (*this->x0_)(idx) <= (*this->x0_)(idx + 1) - x(i))
output(i,all) = (*this->y0_)(idx, all);
unsigned int idx = iterators[i] - this->x0_.begin();
if(x[i] - this->x0_[idx] <= this->x0_[idx + 1] - x[i])
output[i] = this->y0_[idx];
else
output(i,all) = (*this->y0_)(idx + 1, all);
output[i] = this->y0_[idx + 1];
}
}
// InterpolatorLinear IMPLEMENTATION //////////////////////////////////////////
template <typename T>
InterpolatorLinear<T>::InterpolatorLinear(const types::Handle<const Vector>& x0,
const types::Handle<const Matrix>& y0) :
Interpolator<T>(x0, y0)
{}
template <typename T>
InterpolatorLinear<T>::InterpolatorLinear(const Vector& x0, const Matrix& y0) :
InterpolatorLinear<T>(types::Handle<Vector>(new Vector(x0)),
types::Handle<Matrix>(new Matrix(y0)))
template <typename T, template<typename>class VectorT>
InterpolatorLinear<T,VectorT>::InterpolatorLinear(const Vector& x0, const Vector& y0) :
Interpolator<T,VectorT>(x0, y0)
{}
template <typename T>
void InterpolatorLinear<T>::interpolate(const Vector& x, Matrix& output) const
template <typename T, template<typename>class VectorT>
void InterpolatorLinear<T,VectorT>::interpolate(const Vector& x, Vector& output) const
{
using namespace rtac::types::indexing;
auto iterators = this->lower_bound(x);
for(int i = 0; i < x.rows(); i++) {
if(iterators[i] == this->x0_->end() - 1) {
output(i,all) = (*this->y0_)(last, all);
for(int i = 0; i < x.size(); i++) {
if(iterators[i] == this->x0_.end() - 1) {
output[i] = *(this->y0_.end() - 1);
continue;
}
unsigned int idx = iterators[i] - this->x0_->begin();
T lambda = (x(i) - (*this->x0_)(idx))
/ ((*this->x0_)(idx + 1) - (*this->x0_)(idx));
output(i,all) = (1.0 - lambda) * (*this->y0_)(idx, all)
+ lambda * (*this->y0_)(idx + 1, all);
unsigned int idx = iterators[i] - this->x0_.begin();
T lambda = (x[i] - this->x0_[idx])
/ (this->x0_[idx + 1] - this->x0_[idx]);
output[i] = (1.0 - lambda)*this->y0_[idx] + lambda*this->y0_[idx + 1];
}
}
......
tests/src/interpolation_test.cpp
View file @ 6b66d564
......@@ -6,23 +6,28 @@ namespace plt = matplotlibcpp;
#include <rtac_base/types/common.h>
#include <rtac_base/interpolation.h>
//using namespace rtac::types;
template <typename T>
//using Vector = std::vector<T>;
using Vector = rtac::types::Vector<T>;
using namespace rtac::algorithm;
using namespace rtac::types::indexing;
Interpolator<float>::Vector linspace(float vmin, float vmax, unsigned int N)
template <typename VectorT>
VectorT linspace(float vmin, float vmax, unsigned int N)
{
Interpolator<float>::Vector output(N);
VectorT output(N);
for(int n = 0; n < N; n++) {
output[n] = (vmax - vmin) * n / (N - 1) + vmin;
}
return output;
}
Interpolator<float>::Matrix dickins_data()
Eigen::MatrixXf dickins_data()
{
Interpolator<float>::Matrix data(23,2);
Eigen::MatrixXf data(23,2);
data << 0, 1476.7,
38, 1476.7,
50, 1472.6,
......@@ -49,20 +54,16 @@ Interpolator<float>::Matrix dickins_data()
return data;
}
std::vector<float> to_vector(const Interpolator<float>::Vector& v)
const std::vector<float>& to_vector(const std::vector<float>& v)
{
std::vector<float> out(v.rows());
for(int i = 0; i < out.size(); i++) {
out[i] = v(i);
}
return out;
return v;
}
std::vector<float> to_vector(const Interpolator<float>::Matrix& v)
std::vector<float> to_vector(const rtac::types::Vector<float>& v)
{
std::vector<float> out(v.rows());
std::vector<float> out(v.size());
for(int i = 0; i < out.size(); i++) {
out[i] = v(i,0);
out[i] = v(i);
}
return out;
}
......@@ -70,14 +71,16 @@ std::vector<float> to_vector(const Interpolator<float>::Matrix& v)
int main()
{
auto data = dickins_data();
Interpolator<float>::Vector x0 = data(all,0);
Interpolator<float>::Vector y0 = data(all,1);
auto x0 = data(all,0);
auto y0 = data(all,1);
auto x = linspace(data(0,0), data(last,0), 512);
auto x = linspace<Vector<float>>(data(0,0), data(last,0), 512);
InterpolatorNearest<float> interpNN(data(all,0), data(all,1));
InterpolatorNearest<float, Vector> interpNN(x0, y0);
//InterpolatorNearest<float, Vector> interpNN(to_vector(x0), to_vector(y0));
auto ynn = interpNN(x);
InterpolatorLinear<float> interpLinear(data(all,0), data(all,1));
InterpolatorLinear<float, Vector> interpLinear(x0, y0);
//InterpolatorLinear<float, Vector> interpLinear(to_vector(x0), to_vector(y0));
auto yl = interpLinear(x);
plt::plot(to_vector(x0), to_vector(y0), {{"label", "original"}});
......
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