Adding multiple-lobe fitting in the Eigen LM solver

sources/plugins/nonlinear_levenberg_eigen/fitter.cpp

@@ -22,8 +22,8 @@ ALTA_DLL_EXPORT fitter* provide_fitter()
 
 struct EigenFunctor: Eigen::DenseFunctor<double>
 {
-    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)
+	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)
 	{
 #ifndef DEBUG
 		std::cout << "<<DEBUG>> constructing an EigenFunctor for n=" << inputs() << " parameters and m=" << values() << " points" << std::endl ;
@@ -45,21 +45,21 @@ struct EigenFunctor: Eigen::DenseFunctor<double>
 		{
 			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];
-            }
+			// 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];
 
 			// Should add the resulting vector completely
-            vec _y = _di - cos*(*_f)(_x);
+			vec _y = _di - cos*(*_f)(_x);
 			for(int i=0; i<_f->dimY(); ++i)
 				y(i*_d->size() + s) = _y[i];
 
@@ -67,7 +67,7 @@ struct EigenFunctor: Eigen::DenseFunctor<double>
 #ifdef DEBUG
 		std::cout << "diff vector:" << std::endl << y << std::endl << std::endl ;
 #endif
-		
+
 		return 0;
 	}
 
@@ -84,51 +84,147 @@ struct EigenFunctor: Eigen::DenseFunctor<double>
 			// 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];
-            }
+			// 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
 			vec _jac = _f->parametersJacobian(xi);
-			
+
 			// Fill the columns of the matrix
-#ifdef DEBUG
-			Eigen::MatrixXd temp (_f->dimY(), _f->nbParameters());
-#endif
 			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];
-#ifdef DEBUG
-					temp(i, j) = _jac[i*_f->nbParameters() + j];
-#endif
+					fjac(i*_d->size() + s, j) = - cos * _jac[i*_f->nbParameters() + j];
 				}
 			}
+		}
+		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
+	}
 
+	int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& y) const
+	{
 #ifdef DEBUG
-			std::cout << temp << std::endl << std::endl ;
+		std::cout << "parameters:" << std::endl << x << std::endl << std::endl ;
 #endif
+
+		// 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)
+			{
+				_fy += (*(*_f)[i])(_x);
+			}
+
+			// 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];
+
 		}
 #ifdef DEBUG
-			std::cout << "jacobian :" << std::endl << fjac << std::endl << std::endl;
+		std::cout << "diff vector:" << std::endl << y << std::endl << std::endl ;
 #endif
+
 		return 0;
 	}
 
+	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); }
 
-	nonlinear_function* _f;
-    const data* _d;
+		nonlinear_function* f = (*_f)[_index];
+		f->setParameters(_p);
 
-    // Flags
-    bool _cosine;
+		// 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
+			vec _jac = f->parametersJacobian(xi);
+
+			// 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;
 };
 
 nonlinear_fitter_eigen::nonlinear_fitter_eigen() 
@@ -168,35 +264,88 @@ bool nonlinear_fitter_eigen::fit_data(const data* d, function* fit, const argume
     vec nf_x = nf->parameters();
 
     int info;
-    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;
-    }
-
 
-    vec p(nf->nbParameters());
+	 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();
 
-    for(int i=0; i<p.size(); ++i)
-    {
-        p[i] = x(i);
-    }
-    std::cout << "<<INFO>> found parameters: " << p << std::endl;
+#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);
+		 }
 #ifndef DEBUG
     std::cout << "<<DEBUG>> using " << lm.iterations() << " iterations" << std::endl;
 #endif
-    nf->setParameters(p);
+	 }
+
+    std::cout << "<<INFO>> found parameters: " << nf_x << std::endl;
+    nf->setParameters(nf_x);
     return true;
 
 }