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

#include <Eigen/Dense>
#include <Eigen/SVD>
#include "eiquadprog.hpp"

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

#include <QTime>


17
data* rational_fitter_quadproge::provide_data() const
18
{
19
	return new vertical_segment() ;
20 21
}

22
function* rational_fitter_quadproge::provide_function() const 
23 24 25 26
{
	return new rational_function() ;
}

27
rational_fitter_quadproge::rational_fitter_quadproge() : QObject()
28 29
{
}
30
rational_fitter_quadproge::~rational_fitter_quadproge() 
31 32 33
{
}

34
bool rational_fitter_quadproge::fit_data(const data* dat, function* fit)
35 36
{
	rational_function* r = dynamic_cast<rational_function*>(fit) ;
37
	const vertical_segment* d = dynamic_cast<const vertical_segment*>(dat) ;
38 39 40 41 42 43 44 45 46 47
	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()) ;
48 49
	r->setMin(d->min()) ;
	r->setMax(d->max()) ;
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88

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


	int temp_np = _min_np, temp_nq = _min_nq ;
	while(temp_np <= _max_np || temp_nq <= _max_nq)
	{
		QTime time ;
		time.start() ;
		
		if(fit_data(d, temp_np, temp_nq, r))
		{
			int msec = time.elapsed() ;
			int sec  = (msec / 1000) % 60 ;
			int min  = (msec / 60000) % 60 ;
			int hour = (msec / 3600000) ;
			std::cout << "<<INFO>> got a fit using np = " << temp_np << " & nq =  " << temp_nq << "      " << std::endl ;
			std::cout << "<<INFO>> it took " << hour << "h " << min << "m " << sec << "s" << std::endl ;

			return true ;
		}

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

		if(temp_np <= _max_np)
		{
			++temp_np ;
		}
		if(temp_nq <= _max_nq)
		{
			++temp_nq ;
		}
	}

	return false ;
}

89
void rational_fitter_quadproge::set_parameters(const arguments& args)
90 91 92 93 94 95 96 97
{
	_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) ;
}
		

98
bool rational_fitter_quadproge::fit_data(const vertical_segment* d, int np, int nq, rational_function* r) 
99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
{

	// 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)
	{
		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)) ; }
	}

	r->update(Pn, Qn) ;
	return true ;
}

// 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
122
bool rational_fitter_quadproge::fit_data(const vertical_segment* dat, int np, int nq, int ny, rational_function* rf) 
123 124
{
	rational_function* r = dynamic_cast<rational_function*>(rf) ;
125
	const vertical_segment* d = dynamic_cast<const vertical_segment*>(dat) ;
126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
	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()) ;

	// 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() ;

	// Matrices of the problem
	Eigen::MatrixXd G (np+nq, np+nq) ;
	Eigen::VectorXd g (np+nq) ;
	Eigen::MatrixXd CI(np+nq, 2*d->size()) ;
	Eigen::VectorXd ci(2*d->size()) ;
	Eigen::MatrixXd CE(np+nq, 2*d->size()) ;
	Eigen::VectorXd ce(2*d->size()) ;

	// Select the size of the result vector to
	// be equal to the dimension of p + q
	for(int i=0; i<np+nq; ++i)
	{
153 154 155 156 157 158 159 160 161 162 163 164
		for(int j=0; j<np+nq; ++j)
		{
			if(i == j)
			{
				G(i,j) = 1.0 ; 
			}
			else
			{
				G(i,j) = 0.0 ;
			}
		}
		g(i) = 0.0 ;
165 166 167 168 169 170 171 172 173 174 175 176
	}
	
	// Each constraint (fitting interval or point
	// add another dimension to the constraint
	// matrix
	for(int i=0; i<d->size(); ++i)	
	{		
		// Norm of the row vector
		double a0_norm = 0.0 ;
		double a1_norm = 0.0 ;

		vec xi = d->get(i) ;
177
/*		for(int k=0; k<d->dimX(); ++k)
178 179 180
		{
			xi[k] /= dmax[k] ;
		}
181
*/
182 183 184 185 186 187
		// 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)
		{
188 189 190
			CE(j, i) = 0.0 ;
			CE(j, i+d->size()) = 0.0 ;

191 192 193 194 195 196
			// Filling the p part
			if(j<np)
			{
				const double pi = r->p(xi, j) ;
				a0_norm += pi*pi ;
				a1_norm += pi*pi ;
197 198
				CI(j,i) =  pi ;
				CI(j,i+d->size()) = -pi ;
199 200 201 202 203 204 205 206 207 208
			}
			// Filling the q part
			else
			{
				vec yl, yu ; 
				d->get(i, yl, yu) ;

				const double qi = r->q(xi, j-np) ;
				a0_norm += qi*qi * (yl[ny]*yl[ny]) ;
				a1_norm += qi*qi * (yu[ny]*yu[ny]) ;
209
				CI(j, i) = -yl[ny] * qi ;
210
				CI(j, i+d->size()) = yu[ny] * qi ;
211 212 213 214 215
			}
		}
	
		// Set the c vector, will later be updated using the
		// delta parameter.
216 217
		ci(i)           = -sqrt(a0_norm) ;
		ci(i+d->size()) = -sqrt(a1_norm) ;
218
		ce(i)           = 0.0 ;
219
		ce(i+d->size()) = 0.0 ;
220 221 222 223
/*	
		CI.col(i)           /= sqrt(a0_norm) ;
		CI.col(i+d->size()) /= sqrt(a1_norm) ;
*/
224 225 226 227 228 229 230 231 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
	}
#ifdef DEBUG
	std::cout << "CI = [" ;
	for(int j=0; j<d->size()*2; ++j)
	{
		for(int i=0; i<np+nq; ++i)
		{
			std::cout << CI(i,j) ;
			if(i != np+nq-1) std::cout << ", ";
		}
		if(j != d->size()*2-1)
			std::cout << ";" << std::endl; 
		else
			std::cout << "]" << std::endl ;
	}
#endif
	// Update the ci column with the delta parameter
	// (See Celis et al. 2007 p.12)
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(CI);
	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
	
	double delta = sigma_m / sigma_M ;
	if(std::isnan(delta) || (std::abs(delta) == std::numeric_limits<double>::infinity()))
	{
#ifdef DEBUG
		std::cerr << "<<ERROR>> delta factor is NaN of Inf" << std::endl ;
#endif
		return false ;
	}
	else if(delta == 0.0)
	{
		delta = 1.0 ;
	}


#ifndef DEBUG
	std::cout << "<<DEBUG>> delta factor: " << sigma_m << " / " << sigma_M << " = " << delta << std::endl ;
#endif
	for(int i=0; i<2*d->size(); ++i)	
273
	{
274 275
		ci(i) = ci(i) * delta;
//		ci(i) = delta ; 
276 277
	}

278 279 280 281 282 283 284
#ifdef DEBUG
	std::cout  << G << std::endl << std::endl ;
	std::cout  << g << std::endl << std::endl ;
	std::cout  << CI << std::endl << std::endl ;
	std::cout  << ci << std::endl << std::endl ;
#endif

285
	// Compute the solution
286
	Eigen::VectorXd x(np+nq);
287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
	double cost = solve_quadprog(G, g, CE, ce, CI, ci, x);


	bool solves_qp = !(cost == std::numeric_limits<double>::infinity());
	for(int i=0; i<np+nq; ++i)
	{
		const double v = x[i];
		solves_qp = solves_qp && !std::isnan(v) && (v != std::numeric_limits<double>::infinity()) ;
	}

	if(solves_qp)
	{
		// Recopy the vector d
		std::vector<double> p, q;
		double norm = 0.0 ;
		for(int i=0; i<np+nq; ++i)
		{
			const double v = x[i];
			norm += v*v ;
			if(i < np)
			{
308
				p.push_back(v) ;
309 310 311
			}
			else
			{
312
				q.push_back(v) ;
313 314 315 316 317 318 319 320 321 322 323 324 325 326 327
			}
		}

		r->update(p, q);
		std::cout << "<<INFO>> got solution " << *r << std::endl ;
		
		return norm > 0.0;
	}
	else

	{
		return false; 
	}
}

328
Q_EXPORT_PLUGIN2(rational_fitter_quadproge, rational_fitter_quadproge)