fitter.cpp 4.88 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
#include "fitter.h"

#include <Eigen/Dense>
#include <ceres/ceres.h>

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

#include <core/common.h>

ALTA_DLL_EXPORT fitter* provide_fitter()
{
    return new nonlinear_fitter_ceres();
}

class CeresFunctor : public ceres::CostFunction
{
	public:
23
		CeresFunctor(const ptr<nonlinear_function>& f, const vec& xi, const arguments& args) : _f(f), _xi(xi)
24 25 26
		{
			set_num_residuals(f->dimY());
			mutable_parameter_block_sizes()->push_back(f->nbParameters());
27 28

			_log_fit = args.is_defined("log-fit");
29 30
		}

31
		virtual bool Evaluate(double const* const* x, double* y, double** dy) const
32
		{
33 34 35 36 37 38 39 40 41 42 43 44
			// Check that the parameters used are within the bounds defined
			// by the function
			vec p_min = _f->getParametersMin();
			vec p_max = _f->getParametersMax();
			for(int i=0; i<_f->nbParameters(); ++i)
			{
				if(x[0][i] < p_min[i] || x[0][i] > p_max[i])
				{
					return false;
				}
			}

45 46 47 48 49 50 51
			// Update the parameters vector
			vec _p(_f->nbParameters());
			for(int i=0; i<_f->nbParameters(); ++i) { _p[i] = x[0][i]; }
			_f->setParameters(_p);

			vec _di = vec(_f->dimY());
			for(int i=0; i<_f->dimY(); ++i)
52 53 54
			{
				_di[i] = _xi[_f->dimX() + i];
			}
55

56 57
			const vec _yi = _f->value(_xi);
			const vec _y = _di - _yi;
58
			for(int i=0; i<_f->dimY(); ++i)
59
			{
60
				y[i] = (_log_fit) ? log(1.0 + _di[i]) - log(1.0 + _yi[i]) : _y[i] ;
61
			}
62 63 64

			if(dy != NULL)
			{
65
				df(_di, dy);
66 67 68 69 70 71 72
			}

			return true;
		}

		// The parameter of the function _f should be set prior to this function
		// call. If not it will produce undesirable results.
73
		virtual void df(const vec& di, double ** fjac) const
74 75 76 77 78 79 80 81 82 83 84 85
		{
			// Get the jacobian of the function at position x_i for the current
			// set of parameters (set prior to function call)
			vec _jac = _f->parametersJacobian(_xi);

			// For each output channel, update the subpart of the
			// vector row
			for(int i=0; i<_f->dimY(); ++i)
			{
				// Fill the columns of the matrix
				for(int j=0; j<_f->nbParameters(); ++j)
				{
86
                    fjac[0][i*_f->nbParameters() + j] = - ((_log_fit) ? _jac[i*_f->nbParameters() + j]/(1.0 + di[i]) : _jac[i*_f->nbParameters() + j]);
Laurent Belcour's avatar
Laurent Belcour committed
87 88
				}
         }
89 90 91 92
		}

	protected:

93
		// Data point and function to optimize
94
		const ptr<nonlinear_function>& _f;
95 96 97 98
		const vec _xi;

		// Arguments of the fitting procedure
		bool _log_fit;
99 100
};

101
nonlinear_fitter_ceres::nonlinear_fitter_ceres()
102 103
{
}
104
nonlinear_fitter_ceres::~nonlinear_fitter_ceres()
105 106 107
{
}

108
bool nonlinear_fitter_ceres::fit_data(const ptr<data>& d, const ptr<function>& fit, const arguments &args)
109 110 111 112 113 114 115 116 117
{
    // 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()) ;

	 // Convert the function and bootstrap it with the data
118 119
   ptr<nonlinear_function> nf = dynamic_pointer_cast<nonlinear_function>(fit);
    if(!nf)
120 121 122 123
    {
        std::cerr << "<<ERROR>> the function is not a non-linear function" << std::endl;
        return false;
    }
124

125

126 127 128 129 130 131 132
#ifndef DEBUG
	 std::cout << "<<DEBUG>> number of parameters: " << nf->nbParameters() << std::endl;
#endif
	 if(nf->nbParameters() == 0)
	 {
		 return true;
	 }
133

134 135
	 /* Bootstrap the function */
	 nf->bootstrap(d, args);
136

137 138
	 /* the following starting values provide a rough fit. */
	 vec p = nf->parameters();
139

140 141
	 std::cout << "<<DEBUG>> Starting vector: " << p << std::endl;
	 std::cout << "<<DEBUG>> Final vector should be between " << nf->getParametersMin() << " and " << nf->getParametersMax() << std::endl;
142

143 144 145 146 147
	 // Create the problem
	 ceres::Problem problem;
	 for(int i=0; i<d->size(); ++i)
	 {
		 vec xi = d->get(i);
148 149 150 151 152 153 154 155 156
		 vec xf(nf->dimX() + nf->dimY());

		 // Convert the sample to be in the parametrizatio of the function
		 params::convert(&xi[0], d->input_parametrization(), nf->input_parametrization(), &xf[0]);
		 for(int k=0; k<nf->dimY(); ++k)
		 {
			 xf[nf->dimX() + k] = xi[d->dimX() + k];
		 }

157
		 problem.AddResidualBlock(new CeresFunctor(nf, xf, args), NULL, &p[0]);
158
	 }
159

160 161 162
	 // Solves the NL problem
	 ceres::Solver::Summary summary;
	 ceres::Solve(options, &problem, &summary);
163 164 165


#ifdef DEBUG
166
	 std::cout << summary.BriefReport() << std::endl;
167
#endif
168
	 std::cout << "<<INFO>> found parameters: " << p << std::endl;
169

170 171
	 nf->setParameters(p);
	 return true;
172 173 174 175
}

void nonlinear_fitter_ceres::set_parameters(const arguments& args)
{
176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
    if(args.is_defined("ceres-max-num-iterations"))
    {
        options.max_num_iterations = args.get_int("ceres-max-num-iterations", 50); // Default value = 50
    }

    if(args["ceres-factorization"] == "normal-cholesky")
    {
        options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    }
    else
    {
        options.linear_solver_type = ceres::DENSE_QR;
    }

    if(args.is_defined("ceres-debug"))
    {
        options.minimizer_progress_to_stdout = true; // Default value = false;
    }
194
}