fitter.cpp 9.33 KB
Newer Older
1 2 3 4
#include "fitter.h"

#include <Eigen/Dense>
#include <Eigen/SVD>
Laurent Belcour's avatar
Laurent Belcour committed
5
#include <unsupported/Eigen/LevenbergMarquardt>
6 7 8 9 10 11 12 13 14 15

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

#include <QTime>

16 17 18
#include <core/common.h>

ALTA_DLL_EXPORT fitter* provide_fitter()
19 20 21 22
{
    return new nonlinear_fitter_eigen();
}

Laurent Belcour's avatar
Laurent Belcour committed
23
struct EigenFunctor: Eigen::DenseFunctor<double>
24
{
25 26
	EigenFunctor(nonlinear_function* f, const data* d, bool use_cosine) :
		DenseFunctor<double>(f->nbParameters(), d->dimY()*d->size()), _f(f), _d(d), _cosine(use_cosine)
27
	{
Laurent Belcour's avatar
Laurent Belcour committed
28 29 30
#ifndef DEBUG
		std::cout << "<<DEBUG>> constructing an EigenFunctor for n=" << inputs() << " parameters and m=" << values() << " points" << std::endl ;
#endif
31 32 33 34
	}

	int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& y) const
	{
Laurent Belcour's avatar
Laurent Belcour committed
35
#ifdef DEBUG
36 37
		std::cout << "parameters:" << std::endl << x << std::endl << std::endl ;
#endif
Laurent Belcour's avatar
Laurent Belcour committed
38 39 40

		// Update the parameters vector
		vec _p(inputs());
41
		for(int i=0; i<inputs(); ++i) { _p[i] = x(i); }
Laurent Belcour's avatar
Laurent Belcour committed
42 43
		_f->setParameters(_p);

44
		for(int s=0; s<_d->size(); ++s)
Laurent Belcour's avatar
Laurent Belcour committed
45 46 47
		{
			vec _x  = _d->get(s);

48 49 50 51 52 53 54 55
			// Compute the cosine factor. Only update the constant if the flag
			// is set in the object.
			double cos = 1.0;
			if(_cosine)
			{
				double cart[6]; params::convert(&_x[0], _d->input_parametrization(), params::CARTESIAN, cart);
				cos = cart[5];
			}
56

Laurent Belcour's avatar
Laurent Belcour committed
57 58 59 60 61
			vec _di = vec(_f->dimY());
			for(int i=0; i<_f->dimY(); ++i)
				_di[i] = _x[_f->dimX() + i];

			// Should add the resulting vector completely
62
			vec _y = _di - cos*(*_f)(_x);
63 64
			for(int i=0; i<_f->dimY(); ++i)
				y(i*_d->size() + s) = _y[i];
65

Laurent Belcour's avatar
Laurent Belcour committed
66
		}
Laurent Belcour's avatar
Laurent Belcour committed
67
#ifdef DEBUG
68 69
		std::cout << "diff vector:" << std::endl << y << std::endl << std::endl ;
#endif
70

71 72 73 74 75
		return 0;
	}

	int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) const
	{
76
		// Update the paramters
Laurent Belcour's avatar
Laurent Belcour committed
77
		vec _p(inputs());
78
		for(int i=0; i<inputs(); ++i) { _p[i] = x(i); }
Laurent Belcour's avatar
Laurent Belcour committed
79 80
		_f->setParameters(_p);

81 82
		// For each element to fit, fill the rows of the matrix
		for(int s=0; s<_d->size(); ++s)
Laurent Belcour's avatar
Laurent Belcour committed
83
		{
84 85
			// Get the position
			vec xi = _d->get(s);
86

87 88 89 90 91 92 93 94
			// Compute the cosine factor. Only update the constant if the flag
			// is set in the object.
			double cos = 1.0;
			if(_cosine)
			{
				double cart[6]; params::convert(&xi[0], _d->input_parametrization(), params::CARTESIAN, cart);
				cos = cart[5];
			}
95

96
			// Get the associated jacobian
Laurent Belcour's avatar
Laurent Belcour committed
97
			vec _jac = _f->parametersJacobian(xi);
98

99
			// Fill the columns of the matrix
Laurent Belcour's avatar
Laurent Belcour committed
100
			for(int j=0; j<_f->nbParameters(); ++j)
101
			{
102 103 104 105
				// For each output channel, update the subpart of the
				// vector row
				for(int i=0; i<_f->dimY(); ++i)
				{
106
					fjac(i*_d->size() + s, j) = - cos * _jac[i*_f->nbParameters() + j];
107
				}
108
			}
109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129
		}
		return 0;
	}


	nonlinear_function* _f;
	const data* _d;

	// Flags
	bool _cosine;
};

struct CompoundFunctor: Eigen::DenseFunctor<double>
{
	CompoundFunctor(compound_function* f, int index, const data* d, bool use_cosine) :
		DenseFunctor<double>((*f)[index]->nbParameters(), d->dimY()*d->size()), _f(f), _index(index), _d(d), _cosine(use_cosine)
	{
#ifndef DEBUG
		std::cout << "<<DEBUG>> constructing an EigenFunctor for n=" << inputs() << " parameters and m=" << values() << " points" << std::endl ;
#endif
	}
130

131 132
	int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& y) const
	{
133
#ifdef DEBUG
134
		std::cout << "parameters:" << std::endl << x << std::endl << std::endl ;
135
#endif
136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163

		// Update the parameters vector
		vec _p(inputs());
		for(int i=0; i<inputs(); ++i) { _p[i] = x(i); }
		nonlinear_function* f = (*_f)[_index];
		f->setParameters(_p);

		for(int s=0; s<_d->size(); ++s)
		{
			vec _x  = _d->get(s);

			// Compute the cosine factor. Only update the constant if the flag
			// is set in the object.
			double cos = 1.0;
			if(_cosine)
			{
				double cart[6]; params::convert(&_x[0], _d->input_parametrization(), params::CARTESIAN, cart);
				cos = cart[5];
			}

			vec _di = vec(f->dimY());
			for(int i=0; i<f->dimY(); ++i)
				_di[i] = _x[f->dimX() + i];

			// Compute the value of the preceding functions
			vec _fy = vec::Zero(f->dimY());
			for(int i=0; i<_index+1; ++i)
			{
164 165 166 167 168 169 170 171 172 173 174 175
                const nonlinear_function* f = (*_f)[i];
                if(f->input_parametrization() != _d->input_parametrization())
                {
                    vec x(f->dimX());
                    params::convert(&_x[0], _d->input_parametrization(), f->input_parametrization(), &x[0]);

                    _fy += (*f)(x);
                }
                else
                {
                    _fy += (*f)(_x);
                }
176 177 178 179 180 181 182
			}

			// Should add the resulting vector completely
			vec _y = _di - cos*_fy;
			for(int i=0; i<f->dimY(); ++i)
				y(i*_d->size() + s) = _y[i];

183
		}
Laurent Belcour's avatar
Laurent Belcour committed
184
#ifdef DEBUG
185
		std::cout << "diff vector:" << std::endl << y << std::endl << std::endl ;
186
#endif
187

188 189 190
		return 0;
	}

191 192 193 194 195
	int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) const
	{
		// Update the paramters
		vec _p(inputs());
		for(int i=0; i<inputs(); ++i) { _p[i] = x(i); }
196

197 198
		nonlinear_function* f = (*_f)[_index];
		f->setParameters(_p);
199

200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215
		// For each element to fit, fill the rows of the matrix
		for(int s=0; s<_d->size(); ++s)
		{
			// Get the position
			vec xi = _d->get(s);

			// Compute the cosine factor. Only update the constant if the flag
			// is set in the object.
			double cos = 1.0;
			if(_cosine)
			{
				double cart[6]; params::convert(&xi[0], _d->input_parametrization(), params::CARTESIAN, cart);
				cos = cart[5];
			}

			// Get the associated jacobian
216 217 218 219 220 221 222 223 224 225 226 227 228
            vec _jac;
            if(f->input_parametrization() != _d->input_parametrization())
            {
                vec x(f->dimX());
                params::convert(&xi[0], _d->input_parametrization(), f->input_parametrization(), &x[0]);

                _jac = f->parametersJacobian(x);
            }
            else
            {
                _jac = f->parametersJacobian(xi);
            }

229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250

			// Fill the columns of the matrix
			for(int j=0; j<f->nbParameters(); ++j)
			{
				// For each output channel, update the subpart of the
				// vector row
				for(int i=0; i<_f->dimY(); ++i)
				{
					fjac(i*_d->size() + s, j) = - cos * _jac[i*f->nbParameters() + j];
				}
			}
		}
		return 0;
	}


	compound_function* _f;
	const data* _d;

	// Flags
	bool _cosine;
	int _index;
251 252
};

253
nonlinear_fitter_eigen::nonlinear_fitter_eigen() 
254 255 256 257 258 259
{
}
nonlinear_fitter_eigen::~nonlinear_fitter_eigen() 
{
}

260
bool nonlinear_fitter_eigen::fit_data(const data* d, function* fit, const arguments &args)
261
{
262 263 264 265 266 267 268
    // I need to set the dimension of the resulting function to be equal
    // to the dimension of my fitting problem
    fit->setDimX(d->dimX()) ;
    fit->setDimY(d->dimY()) ;
    fit->setMin(d->min()) ;
    fit->setMax(d->max()) ;

269
	 // Convert the function and bootstrap it with the data
270 271
    if(dynamic_cast<nonlinear_function*>(fit) == NULL)
    {
272
        std::cerr << "<<ERROR>> the function is not a non-linear function" << std::endl;
273 274 275
        return false;
    }
    nonlinear_function* nf = dynamic_cast<nonlinear_function*>(fit);
276
	 nf->bootstrap(d, args);
277

Laurent Belcour's avatar
Laurent Belcour committed
278 279 280
#ifndef DEBUG
	 std::cout << "<<DEBUG>> number of parameters: " << nf->nbParameters() << std::endl;
#endif
Laurent Belcour's avatar
Laurent Belcour committed
281 282 283 284
	 if(nf->nbParameters() == 0)
	 {
		 return true;
	 }
Laurent Belcour's avatar
Laurent Belcour committed
285

286
    /* the following starting values provide a rough fit. */
287
    vec nf_x = nf->parameters();
Laurent Belcour's avatar
Laurent Belcour committed
288

289 290
    int info;

291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335
	 if(args.is_defined("fit-compound"))
	 {
		 if(dynamic_cast<compound_function*>(nf) == NULL)
		 {
			 std::cerr << "<<ERROR>> you should use --fit-compound with a compound function" << std::endl;
			 return false;
		 }

		 compound_function* compound = dynamic_cast<compound_function*>(nf);

		 // Register how many parameter are already fitted
		 int already_fit = 0;

		 // Get the i-th function of the compound
		 for(int index=0; index<compound->size(); ++index)
		 {
			 nonlinear_function* f = (*compound)[index];
			 if(f->nbParameters() == 0)
				 continue;

			 Eigen::VectorXd x(f->nbParameters());
			 for(int i=0; i<f->nbParameters(); ++i)
			 {
				 x[i] = nf_x[i+already_fit];
			 }

			 CompoundFunctor functor(compound, index, d, args.is_defined("fit-with-cosine"));
			 Eigen::LevenbergMarquardt<CompoundFunctor> lm(functor);

			 info = lm.minimize(x);

			 if(info == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
			 {
				 std::cerr << "<<ERROR>> incorrect parameters" << std::endl;
				 return false;
			 }

			 // Update the vector of parameters
			 for(int i=0; i<f->nbParameters(); ++i)
			 {
				 nf_x[i+already_fit] = x[i];
			 }

			 // Update the number of already fitted parameters
			 already_fit += f->nbParameters();
336

337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
#ifndef DEBUG
    std::cout << "<<DEBUG>> function " << index+1 << " using " << lm.iterations() << " iterations" << std::endl;
#endif
		 }
	 }
	 else
	 {
		 Eigen::VectorXd x(nf->nbParameters());
		 for(int i=0; i<nf->nbParameters(); ++i)
		 {
			 x[i] = nf_x[i];
		 }

		 EigenFunctor functor(nf, d, args.is_defined("fit-with-cosine"));
		 Eigen::LevenbergMarquardt<EigenFunctor> lm(functor);

		 info = lm.minimize(x);

		 if(info == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
		 {
			 std::cerr << "<<ERROR>> incorrect parameters" << std::endl;
			 return false;
		 }

		 for(int i=0; i<nf->nbParameters(); ++i)
		 {
			 nf_x[i] = x(i);
		 }
Laurent Belcour's avatar
Laurent Belcour committed
365 366 367
#ifndef DEBUG
    std::cout << "<<DEBUG>> using " << lm.iterations() << " iterations" << std::endl;
#endif
368 369 370 371
	 }

    std::cout << "<<INFO>> found parameters: " << nf_x << std::endl;
    nf->setParameters(nf_x);
372
    return true;
373 374 375 376 377 378

}

void nonlinear_fitter_eigen::set_parameters(const arguments& args)
{
}