rational_fitter_cgal.cpp 8.29 KB
Newer Older
1
#include "rational_fitter_cgal.h"
2 3 4 5 6 7 8 9 10 11 12 13 14 15

#include <CGAL/basic.h>
#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>
#include <CGAL/MP_Float.h>
#include <Eigen/SVD>

#include <string>
#include <iostream>
#include <fstream>
#include <limits>
#include <algorithm>
#include <cmath>

16 17
#include <QTime>

18 19 20 21 22
typedef CGAL::MP_Float ET ;
typedef CGAL::Quadratic_program<ET> Program ;
typedef CGAL::Quadratic_program_solution<ET> Solution ;
typedef CGAL::Quadratic_program_options Options ;

23 24 25

data* rational_fitter_cgal::provide_data() const
{
26
	return new vertical_segment() ;
27 28 29 30 31 32 33
}

function* rational_fitter_cgal::provide_function() const 
{
	return new rational_function() ;
}

34 35 36 37 38 39 40
rational_fitter_cgal::rational_fitter_cgal() : QObject()
{
}
rational_fitter_cgal::~rational_fitter_cgal() 
{
}

41
bool rational_fitter_cgal::fit_data(const data* dat, function* fit, const arguments &args)
42
{
43
	rational_function* r = dynamic_cast<rational_function*>(fit) ;
44
	const vertical_segment* d = dynamic_cast<const vertical_segment*>(dat) ;
45 46 47 48 49 50 51 52 53 54
	if(r == NULL || d == NULL)
	{
		std::cerr << "<<ERROR>> not passing the correct class to the fitter" << std::endl ;
		return false ;
	}

	// I need to set the dimension of the resulting function to be equal
	// to the dimension of my fitting problem
	r->setDimX(d->dimX()) ;
	r->setDimY(d->dimY()) ;
55 56
	r->setMin(d->min()) ;
	r->setMax(d->max()) ;
Laurent Belcour's avatar
Laurent Belcour committed
57

58 59 60
	std::cout << "<<INFO>> np in  [" << _min_np << ", " << _max_np 
	          << "] & nq in [" << _min_nq << ", " << _max_nq << "]" << std::endl ;

61

62
	int temp_np = _min_np, temp_nq = _min_nq ;
63
	while(temp_np <= _max_np || temp_nq <= _max_nq)
64
	{
65 66 67
		QTime time ;
		time.start() ;
		
68
		if(fit_data(d, temp_np, temp_nq, r))
69
		{
70 71 72 73
			int msec = time.elapsed() ;
			int sec  = (msec / 1000) % 60 ;
			int min  = (msec / 60000) % 60 ;
			int hour = (msec / 3600000) ;
Laurent Belcour's avatar
Update  
Laurent Belcour committed
74
			std::cout << "<<INFO>> got a fit using np = " << temp_np << " & nq =  " << temp_nq << "      " << std::endl ;
75 76
			std::cout << "<<INFO>> it took " << hour << "h " << min << "m " << sec << "s" << std::endl ;

77 78 79
			return true ;
		}

80
		std::cout << "<<INFO>> fit using np = " << temp_np << " & nq =  " << temp_nq << " failed\r"  ;
81
		std::cout.flush() ;
82

83
		if(temp_np <= _max_np)
84 85 86
		{
			++temp_np ;
		}
87
		if(temp_nq <= _max_nq)
88 89 90 91 92 93 94 95
		{
			++temp_nq ;
		}
	}

	return false ;
}

96
void rational_fitter_cgal::set_parameters(const arguments& args)
97 98 99 100 101 102 103
{
	_max_np = args.get_float("np", 10) ;
	_max_nq = args.get_float("nq", 10) ;
	_min_np = args.get_float("min-np", _max_np) ;
	_min_nq = args.get_float("min-nq", _max_nq) ;
}
		
104
bool rational_fitter_cgal::fit_data(const vertical_segment* d, int np, int nq, rational_function* r) 
105 106
{

107 108 109 110 111
	// Multidimensional coefficients
	std::vector<double> Pn ; Pn.reserve(d->dimY()*np) ;
	std::vector<double> Qn ; Qn.reserve(d->dimY()*nq) ;

	for(int j=0; j<d->dimY(); ++j)
112
	{
113 114 115 116 117
		if(!fit_data(d, np, nq, j, r))
			return false ;

		for(int i=0; i<np; ++i) { Pn.push_back(r->getP(i)) ; }
		for(int i=0; i<nq; ++i) { Qn.push_back(r->getQ(i)) ; }
118 119
	}

120 121 122
	r->update(Pn, Qn) ;
	return true ;
}
123

124 125 126 127
// dat is the data object, it contains all the points to fit
// np and nq are the degree of the RP to fit to the data
// y is the dimension to fit on the y-data (e.g. R, G or B for RGB signals)
// the function return a ration BRDF function and a boolean
128
bool rational_fitter_cgal::fit_data(const vertical_segment* d, int np, int nq, int ny, rational_function* r) 
129 130 131
{
	// by default, we have a nonnegative QP with Ax - b >= 0
	Program qp (CGAL::LARGER, false, 0, false, 0) ; 
132

133 134 135 136
	// Get the maximum value in data to scale the input parameter space
	// so that it reduces the values of the polynomial
	vec dmax = d->max() ;

137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
	// Select the size of the result vector to
	// be equal to the dimension of p + q
	for(int i=0; i<np+nq; ++i)
	{
		qp.set_d(i, i, 1.0) ; 
	}
	
	// Each constraint (fitting interval or point
	// add another dimension to the constraint
	// matrix
	Eigen::MatrixXd CI(np+nq, 2*d->size()) ;
	Eigen::VectorXd ci(2*d->size()) ;
	for(int i=0; i<d->size(); ++i)	
	{		
		// Norm of the row vector
		double a0_norm = 0.0 ;
		double a1_norm = 0.0 ;

155
		vec xi = d->get(i) ;
Laurent Belcour's avatar
Laurent Belcour committed
156
		
157 158 159 160 161 162 163 164 165
		// A row of the constraint matrix has this 
		// form: [p_{0}(x_i), .., p_{np}(x_i), -f(x_i) q_{0}(x_i), .., -f(x_i) q_{nq}(x_i)]
		// For the lower constraint and negated for 
		// the upper constraint
		for(int j=0; j<np+nq; ++j)
		{
			// Filling the p part
			if(j<np)
			{
166
				const double pi = r->p(xi, j) ;
167 168 169 170 171 172 173 174 175 176
				a0_norm += pi*pi ;
				a1_norm += pi*pi ;
				qp.set_a(j, i,  ET(pi)) ;
				qp.set_a(j, i+d->size(), ET(-pi)) ;
				CI(j, i) =  pi ;
				CI(j, i+d->size()) = -pi ;
			}
			// Filling the q part
			else
			{
177 178 179
				vec yl, yu ; 
				d->get(i, yl, yu) ;

180
				const double qi = r->q(xi, j-np) ;
181 182 183
				a0_norm += qi*qi * (yl[ny]*yl[ny]) ;
				qp.set_a(j, i, ET(-yl[ny] * qi)) ;
				CI(j, i) = -yl[ny] * qi ;
184
				
185 186 187
				a1_norm += qi*qi * (yu[ny]*yu[ny]) ;
				qp.set_a(j, i+d->size(),  ET(yu[ny] * qi)) ;
				CI(j, i+d->size()) = yu[ny] * qi ;
188 189 190 191 192 193 194 195
			}
		}
	
		// Set the c vector, will later be updated using the
		// delta parameter.
		ci(i) = sqrt(a0_norm) ;
		ci(i+d->size()) = sqrt(a1_norm) ;
	}
196 197 198 199 200 201 202 203 204
#ifdef DEBUG
	for(int j=0; j<d->size()*2; ++j)
	{
		for(int i=0; i<np+nq; ++i)
			std::cout << CI(i,j) << "\t";
		std::cout << std::endl; 
	}
	std::cout << std::endl ;
#endif
205 206
	// Update the ci column with the delta parameter
	// (See Celis et al. 2007 p.12)
207
	Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::HouseholderQRPreconditioner> svd(CI);
208 209 210 211 212 213 214 215 216 217 218 219
	const double sigma_m = svd.singularValues()(std::min(2*d->size(), np+nq)-1) ;
	const double sigma_M = svd.singularValues()(0) ;

#ifdef DEBUG
	std::cout << "<<DEBUG>> SVD = [ " ;
	for(int i=0; i<std::min(2*d->size(), np+nq); ++i)
	{
		std::cout << svd.singularValues()(i) << ", " ;
	}
	std::cout << " ]" << std::endl ;
#endif
	
220
	double delta = sigma_m / sigma_M ;
GUENNEBAUD Gael's avatar
GUENNEBAUD Gael committed
221
	if(std::isnan(delta) || (std::abs(delta) == std::numeric_limits<double>::infinity()))
222 223 224 225 226 227
	{
#ifdef DEBUG
		std::cerr << "<<ERROR>> delta factor is NaN of Inf" << std::endl ;
#endif
		return false ;
	}
228 229 230 231
	else if(delta == 0.0)
	{
		delta = 1.0 ;
	}
232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304

#ifdef DEBUG
	std::cout << "<<DEBUG>> delta factor: " << sigma_m << " / " << sigma_M << " = " << delta << std::endl ;
#endif
	for(int i=0; i<2*d->size(); ++i)	
	{		
		qp.set_b(i, ET(delta * ci(i))) ;
	}

#ifdef DEBUG
	// Export some informations on the problem to solve
	std::cout << "<<DEBUG>> " << qp.get_n() << " variables" << std::endl ;
	std::cout << "<<DEBUG>> " << qp.get_m() << " constraints" << std::endl ;
#endif

	if(qp.is_linear() && !qp.is_nonnegative())
	{
		std::cerr << "<<ERROR>> the problem should not be linear or negative!" << std::endl ;
		return false ;
	}

#ifdef EXTREM_DEBUG
	// Iterate over the rows
	std::cout << std::endl ;
	std::cout << "A = [" ;
	for(int j=0; j<qp.get_m(); ++j)
	{
		if(j == 0) std::cout << "   " ;

		// Iterate over the columns
		for(int i=0; i<qp.get_n(); ++i)
		{
			if(i > 0) std::cout << ",\t" ;
			std::cout << *(*(qp.get_a()+i) +j) ;
		}
		std::cout << ";" ;
		if(j < qp.get_n()-1) std::cout << std::endl ;
	}
	std::cout << "]" << std::endl << std::endl ;
	
	std::cout << std::endl ;
	std::cout << "D = [" ;
	for(int j=0; j<np+nq; ++j)
	{
		if(j == 0) std::cout << "   " ;

		// Iterate over the columns
		for(int i=0; i<np+nq; ++i)
		{
			if(i > 0) std::cout << ",\t" ;
			std::cout << *(*(qp.get_d()+i) +j) ;
		}
		std::cout << ";" ;
	}
	std::cout << "]" << std::endl << std::endl ;
#endif

	// solve the program, using ET as the exact type
	Options  o ;
	o.set_auto_validation(true) ;
	Solution s = CGAL::solve_quadratic_program(qp, ET(), o) ;

#ifdef DEBUG
	if(s.is_infeasible())
	{
		std::cout << "<<DEBUG>> the current program is infeasible" << std::endl ;
	}
#endif

	bool solves_qp = !s.is_infeasible() && s.solves_quadratic_program(qp)  ;
	for(int i=0; i<np+nq; ++i)
	{
		const double v = (double)CGAL::to_double(*(s.variable_numerators_begin()+i)) ;
GUENNEBAUD Gael's avatar
GUENNEBAUD Gael committed
305
		solves_qp = solves_qp && !std::isnan(v) && (v != std::numeric_limits<double>::infinity()) ;
306 307 308 309 310 311
	}

	if(solves_qp)
	{
		// Recopy the vector d
		std::vector<double> p, q;
312
		p.assign(np, 0.0) ; q.assign(nq, 0.0) ;
313 314 315 316 317 318
		for(int i=0; i<np+nq; ++i)
		{
			const double v = CGAL::to_double(*(s.variable_numerators_begin()+i)) ;

			if(i < np)
			{
319
				p[i] = v ;
320 321 322
			}
			else
			{
323
				q[i-np] = v ;
324 325
			}
		}
326
 		r->update(p, q) ;
Laurent Belcour's avatar
Laurent Belcour committed
327 328 329
#ifdef DEBUG
        std::cout << "<<INFO>> got solution " << *r << std::endl ;
#endif
330
		
331 332 333 334 335 336 337 338
		return true;
	}
	else
	{
		return false; 
	}
}

339
Q_EXPORT_PLUGIN2(rational_fitter_cgal, rational_fitter_cgal)