Commit 19e4e31c authored by Laurent Belcour's avatar Laurent Belcour
Browse files

Adding Eigen solver. But not working right now.

parent 5ceccff7
#pragma once
#include <string>
#include <pair>
template<class X, class Y> class data
{
public: // methods
// Load data from a file
virtual void load(const std::string& filename) = 0 ;
virtual void load(const std::string& filename, X min, X max) = 0 ;
// Acces to data
virtual bool get(int i, X& x, Y& y) const = 0 ;
virtual const std::pair<X, Y>& operator[](int i) const = 0 ;
// Get data size
virtual int size() const = 0 ;
// Get min and max input parameters
virtual X min() const = 0 ;
virtual X max() const = 0 ;
} ;
#pragma once
#include "function.h"
#include "data.h"
/*
* Fitting interface for generic fitting algorithms
*
*/
class fitting_algorithm
template<class X, class Y> class fitter
{
public:
// The class function has to be defined for
// each fitting algorithm. Of course fitting
// algorithms can share the same function.
// Also we require that the function class
// is a functor.
//
class function ;
// Data class that contains the data to fit.
// For a rational function it will be intervals
// but can be anything of any dimension.
//
class data ;
// Static function to fit a data set d with the
// underling function class. Return the best
// fit (along with fitting information ?)
//
virtual static function fit_data(const data& d) = 0 ;
virtual bool fit_data(const data<X, Y>& d, function<X, Y>& f) = 0 ;
} ;
#pragma once
template<class X, class Y> class function : public std::function<X(Y)>
{
public: // methods
// Overload the function operator
virtual Y operator()(float X) const = 0 ;
// IO function to text files
virtual void load(const std::string& filename) = 0 ;
virtual void save() const = 0 ;
} ;
......@@ -84,33 +84,31 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <Eigen/Dense>
#include <cmath>
namespace Eigen {
// namespace internal {
template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
Scalar a1, b1, t;
a1 = internal::abs(a);
b1 = internal::abs(b);
a1 = std::abs(a);
b1 = std::abs(b);
if (a1 > b1)
{
t = (b1 / a1);
return a1 * internal::sqrt(1.0 + t * t);
return a1 * std::sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
t = (a1 / b1);
return b1 * internal::sqrt(1.0 + t * t);
return b1 * std::sqrt(1.0 + t * t);
}
return a1 * internal::sqrt(2.0);
return a1 * std::sqrt(2.0);
}
// }
inline void compute_d(VectorXd &d, const MatrixXd& J, const VectorXd& np)
{
d = J.adjoint() * np;
......@@ -215,7 +213,7 @@ inline double solve_quadprog(MatrixXd & G, VectorXd & g0,
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (internal::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -267,7 +265,7 @@ l1: iter++;
#endif
if (internal::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
......@@ -336,7 +334,7 @@ l2a:/* Step 2a: determine step direction */
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (internal::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -508,10 +506,10 @@ inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, doubl
std::cerr << iq << std::endl;
#endif
if (internal::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
// problem degenerate
return false;
R_norm = std::max<double>(R_norm, internal::abs(d(iq - 1)));
R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
return true;
}
......
#include "rational_1d_fitter.h"
#include <boost/regex.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <limits>
#include <algorithm>
rational_1d::rational_1d()
{
}
rational_1d::rational_1d(const std::vector<float>& a,
const std::vector<float>& b) :
a(a), b(b)
{
}
rational_1d::~rational_1d()
{
}
// Overload the function operator
float rational_1d::operator()(float x) const
{
float p = 0.0f ;
float q = 0.0f ;
for(int i=a.size()-1; i>=0; --i)
{
p = x*p + a[i] ;
}
for(int i=b.size()-1; i>=0; --i)
{
q = x*q + b[i] ;
}
return p/q ;
}
// Get the p_i and q_j function
float rational_1d::p(float x, int i) const
{
return pow(x, i) ;
}
float rational_1d::q(float x, int j) const
{
return pow(x, j) ;
}
// IO function to text files
void rational_1d::load(const std::string& filename)
{
}
void rational_1d::save() const
{
}
std::ostream& operator<< (std::ostream& out, const rational_1d& r)
{
std::cout << "p = [" ;
for(int i=0; i<r.a.size(); ++i)
{
if(i != 0)
{
std::cout << ", " ;
}
std::cout << r.a[i] ;
}
std::cout << "]" << std::endl ;
std::cout << "q = [" ;
for(int i=0; i<r.b.size(); ++i)
{
if(i != 0)
{
std::cout << ", " ;
}
std::cout << r.b[i] ;
}
std::cout << "]" << std::endl ;
}
void rational_1d_data::load(const std::string& filename)
{
load(filename, -std::numeric_limits<float>::max(), std::numeric_limits<float>::max()) ;
}
void rational_1d_data::load(const std::string& filename, float min, float max)
{
std::ifstream file(filename) ;
_min = std::numeric_limits<float>::max() ;
_max = -std::numeric_limits<float>::max() ;
if(!file.is_open())
{
std::cerr << "<<ERROR>> unable to open file \"" << filename << "\"" << std::endl ;
}
// N-Floats regexp
boost::regex e ("^([0-9]*\.?[0-9]+[\\t ]?)+");
float x, y, dy ;
while(file.good())
{
std::string line ;
std::getline(file, line) ;
std::stringstream linestream(line) ;
// Discard incorrect lines
if(!boost::regex_match(line,e))
{
continue ;
}
linestream >> x >> y ;
if(linestream.good()) {
linestream >> dy ;
} else {
// TODO Specify the delta in case
dy = 0.001f ;
}
if(x <= max && x >= min)
{
std::vector<float> v ;
v.push_back(x) ;
v.push_back(y-dy) ;
v.push_back(y+dy) ;
_data.push_back(v) ;
// Update min and max
_min = std::min(_min, x) ;
_max = std::max(_max, x) ;
}
}
// Sort the vector
std::sort(_data.begin(), _data.end(), [](const std::vector<float>& a, const std::vector<float>& b){return (a[0]<b[0]);});
std::cout << "<<INFO>> loaded file \"" << filename << "\"" << std::endl ;
std::cout << "<<INFO>> data inside [" << _min << ", " << _max << "]" << std::endl ;
}
bool rational_1d_data::get(int i, float& x, float& yl, float& yu) const
{
if(i >= (int)_data.size())
{
return false ;
}
x = _data[i][0] ;
yl = _data[i][1] ;
yu = _data[i][2] ;
return true ;
}
const std::vector<float>& rational_1d_data::operator[](int i) const
{
return _data[i] ;
}
int rational_1d_data::size() const
{
return _data.size() ;
}
float rational_1d_data::min() const
{
return _min ;
}
float rational_1d_data::max() const
{
return _max ;
}
#pragma once
// Include STL
#include <functional>
#include <vector>
#include <string>
#include <tuple>
class rational_1d : public std::function<float(float)>
{
public: // methods
rational_1d() ;
rational_1d(const std::vector<float>& a, const std::vector<float>& b) ;
virtual ~rational_1d() ;
// Overload the function operator
virtual float operator()(float x) const ;
// Get the p_i and q_j function
virtual float p(float x, int i) const ;
virtual float q(float x, int j) const ;
// IO function to text files
void load(const std::string& filename) ;
void save() const ;
// STL stream ouput
friend std::ostream& operator<< (std::ostream& out, const rational_1d& r) ;
private: // data
// Store the coefficients for the moment, I assume
// the functions to be polynomials.
std::vector<float> a ;
std::vector<float> b ;
} ;
class rational_1d_data // : public fitting_data
{
public: // methods
// Load data from a file
void load(const std::string& filename) ;
void load(const std::string& filename, float min, float max) ;
// Acces to data
bool get(int i, float& x, float& yl, float& yu) const ;
const std::vector<float>& operator[](int i) const ;
// Get data size
int size() const ;
// Get min and max input parameters
float min() const ;
float max() const ;
private: // data
// Store for each point of data, the upper
// and lower value
std::vector<std::vector<float> > _data ;
// Store the min and max value on the input
// domain
float _min, _max ;
} ;
class rational_1d_fitter // : public fitting_algorithm
{
public: // methods
// Fitting a data object
virtual bool fit_data(const rational_1d_data& data, rational_1d& fit) = 0;
// Fitting a data object using np elements
// in the numerator and nq elements in the
// denominator
virtual bool fit_data(const rational_1d_data& data, int np, int nq, rational_1d& fit) = 0;
} ;
......@@ -4,6 +4,7 @@
#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>
#include <CGAL/MP_Float.h>
#include <Eigen/SVD>
#include <boost/regex.hpp>
#include <string>
......@@ -12,194 +13,18 @@
#include <limits>
#include <algorithm>
rational_1d::rational_1d()
{
}
rational_1d::rational_1d(const std::vector<float>& a,
const std::vector<float>& b) :
a(a), b(b)
{
}
rational_1d::~rational_1d()
{
}
// Overload the function operator
float rational_1d::operator()(float x) const
{
float p = 0.0f ;
float q = 0.0f ;
for(int i=a.size()-1; i>=0; --i)
{
p = x*p + a[i] ;
}
for(int i=b.size()-1; i>=0; --i)
{
q = x*q + b[i] ;
}
return p/q ;
}
// Get the p_i and q_j function
float rational_1d::p(float x, int i) const
{
return pow(x, i) ;
}
float rational_1d::q(float x, int j) const
{
return pow(x, j) ;
}
// IO function to text files
void rational_1d::load(const std::string& filename)
{
}
void rational_1d::save() const
{
}
std::ostream& operator<< (std::ostream& out, const rational_1d& r)
{
std::cout << "p = [" ;
for(int i=0; i<r.a.size(); ++i)
{
if(i != 0)
{
std::cout << ", " ;
}
std::cout << r.a[i] ;
}
std::cout << "]" << std::endl ;
std::cout << "q = [" ;
for(int i=0; i<r.b.size(); ++i)
{
if(i != 0)
{
std::cout << ", " ;
}
std::cout << r.b[i] ;
}
std::cout << "]" << std::endl ;
}
void rational_1d_data::load(const std::string& filename)
{
load(filename, -std::numeric_limits<float>::max(), std::numeric_limits<float>::max()) ;
}
void rational_1d_data::load(const std::string& filename, float min, float max)
{
std::ifstream file(filename) ;
_min = std::numeric_limits<float>::max() ;
_max = -std::numeric_limits<float>::max() ;
if(!file.is_open())
{
std::cerr << "<<ERROR>> unable to open file \"" << filename << "\"" << std::endl ;
}
// N-Floats regexp
boost::regex e ("^([0-9]*\.?[0-9]+[\\t ]?)+");
float x, y, dy ;
while(file.good())
{
std::string line ;
std::getline(file, line) ;
std::stringstream linestream(line) ;
// Discard incorrect lines
if(!boost::regex_match(line,e))
{
continue ;
}
linestream >> x >> y ;
if(linestream.good()) {
linestream >> dy ;
} else {
// TODO Specify the delta in case
dy = 0.001f ;
}
if(x <= max && x >= min)
{
std::vector<float> v ;
v.push_back(x) ;
v.push_back(y-dy) ;
v.push_back(y+dy) ;
_data.push_back(v) ;
// Update min and max
_min = std::min(_min, x) ;
_max = std::max(_max, x) ;
}
}
// Sort the vector
std::sort(_data.begin(), _data.end(), [](const std::vector<float>& a, const std::vector<float>& b){return (a[0]<b[0]);});
for(int i=0; i<_data.size(); ++i)
{
std::cout << _data[i][0] << ", " << _data[i][1] << ", " << _data[i][2] << std::endl ;
}
std::cout << "<<INFO>> loaded file \"" << filename << "\"" << std::endl ;
std::cout << "<<INFO>> data inside [" << _min << ", " << _max << "]" << std::endl ;
}
bool rational_1d_data::get(int i, float& x, float& yl, float& yu) const
{
if(i >= (int)_data.size())
{
return false ;
}
x = _data[i][0] ;
yl = _data[i][1] ;
yu = _data[i][2] ;
return true ;
}
const std::vector<float>& rational_1d_data::operator[](int i) const
{
return _data[i] ;
}
int rational_1d_data::size() const
{
return _data.size() ;
}