levenberg_marquardt.hpp

Go to the documentation of this file.

/*
 * Copyright(c):
 * Signal Image and Communications (SIC) Department 
 * http://www.sic.sp2mi.univ-poitiers.fr/ 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - XLIM  Institute UMR CNRS 7252  http://www.xlim.fr/ 
 * 
 * and 
 * 
 * D2 Fluid, Thermic and Combustion 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - PPRIME Institute -  UPR CNRS 3346  http://www.pprime.fr
 *  - ISAE-ENSMA http://www.ensma.fr
 *
 * Contributor(s):
 * The SLIP team,
 * Benoit Tremblais <tremblais_AT_sic.univ-poitiers.fr>,
 * Laurent David <laurent.david_AT_lea.univ-poitiers.fr>, 
 * Ludovic Chatellier <ludovic.chatellier_AT_univ-poitiers.fr>, 
 * Lionel Thomas <lionel.thomas_AT_univ-poitiers.fr>, 
 * Denis Arrivault <arrivault_AT_sic.univ-poitiers.fr>, 
 * Julien Dombre <julien.dombre_AT_univ-poitiers.fr>.
 *
 * Description:
 * The Simple Library of Image Processing (SLIP) is a new image processing 
 * library. It is written in the C++ language following as much as possible 
 * the ISO/ANSI C++ standard. It is consequently compatible with any system  
 * satisfying the ANSI C++ complience. It works on different Unix , Linux ,  
 * Mircrosoft Windows and Mac OS X plateforms. SLIP is a research library that  
 * was created by the Signal, Image and Communications (SIC) departement of  
 * the XLIM, UMR 7252 CNRS Institute in collaboration with the Fluids, Thermic  
 * and Combustion departement of the P', UPR 3346 CNRS Institute of the  
 * University of Poitiers.
 * 
 * The SLIP Library source code has been registered to the APP (French Agency 
 * for the Protection of Programs) by the University of Poitiers and CNRS, 
 * under  registration number IDDN.FR.001.300034.000.S.P.2010.000.21000.

 * http://www.sic.sp2mi.univ-poitiers.fr/slip/
 *
 * This software is governed by the CeCILL-C license under French law and
 * abiding by the rules of distribution of free software.  You can  use, 
 * modify and/ or redistribute the software under the terms of the CeCILL-C 
 * license as circulated by CEA, CNRS and INRIA at the following URL 
 * http://www.cecill.info.
 * As a counterpart to the access to the source code and  rights to copy,
 * modify and redistribute granted by the license, users are provided only
 * with a limited warranty  and the software's author,  the holder of the
 * economic rights,  and the successive licensors  have only  limited
 * liability.
 * 
 * In this respect, the user's attention is drawn to the risks associated
 * with loading,  using,  modifying and/or developing or reproducing the
 * software by the user in light of its specific status of free software,
 * that may mean  that it is complicated to manipulate,  and  that  also
 * therefore means  that it is reserved for developers  and  experienced
 * professionals having in-depth computer knowledge. Users are therefore
 * encouraged to load and test the software's suitability as regards their
 * requirements in conditions enabling the security of their systems and/or
 * data to be ensured and,  more generally, to use and operate it in the 
 * same conditions as regards security. 
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL-C license and that you accept its terms.
 */



#ifndef SLIP_LEVENBERG_MARQUARDT_HPP
#define SLIP_LEVENBERG_MARQUARDT_HPP

#include "Vector3d.hpp"
#include "KVector.hpp"
#include "Matrix.hpp"
#include "linear_algebra.hpp"


namespace slip
{

template <typename Type>
struct funLM_bp : public std::unary_function <slip::Vector<Type>,slip::Vector<Type> > 
{
  typedef funLM_bp<Type> self;
 funLM_bp(const slip::Vector<Type>& Data) : 
   data_(Data),
   r_(slip::Vector<Type>(2))
 {}
 slip::Vector<Type>& operator() (const slip::Vector<Type>& pt) // define the function
 {
   Type u = data_[21];
   Type v = data_[22];
   Type X = pt[0];
   Type Y = pt[1];
   Type Z  =data_[23];
   Type den = (data_[6]*X + data_[7]*Y + data_[8]*Z + data_[11]);
   Type xd = (data_[0]*X + data_[1]*Y + data_[2]*Z + data_[9])/den;
   Type yd = (data_[3]*X + data_[4]*Y + data_[5]*Z + data_[10])/den;
   Type R1 = data_[14]; 
   Type R2 = data_[15]; 
   Type R3 = data_[16]; 
   Type D1 = data_[17];
   Type D2 = data_[18]; 
   Type P1 = data_[19]; 
   Type P2 = data_[20];
   Type x_p = xd-data_[12];
   Type y_p = yd-data_[13];
   Type x_p2 = x_p*x_p;
   Type y_p2 = y_p*y_p;
   Type two_x_p_y_p = static_cast<Type>(2.0) * x_p*y_p;
   Type rhoi2 = x_p2+y_p2;
   Type rhoi2_2 = rhoi2*rhoi2;
   Type rhoi2_3 = rhoi2_2*rhoi2;
   Type three = static_cast<Type>(3.0);
   Type rad = (R1*rhoi2 + R2*rhoi2_2 + R3*rhoi2_3);
   xd = xd + x_p*rad + D1*two_x_p_y_p + D2*(three*x_p2 + y_p2) + P1*rhoi2;
   yd = yd + y_p*rad + D2*two_x_p_y_p + D1*(three*y_p2 + x_p2) + P2*rhoi2;
   
   r_[0] = u-xd;
   r_[1] = v-yd;

   return r_;
}
 slip::Vector<Type> data_;
 slip::Vector<Type> r_;
};



template <typename Type>
struct funLM_DLT : public std::unary_function <slip::Vector<Type>,slip::Vector<Type> > // parameter : Vector, return a double
{

   typedef funLM_DLT<Type> self;

  funLM_DLT(const slip::Vector<Type>& Data) : 
    data_(Data), // initialize function with given data
    N_((Data.size()-19)/5),
    _2N_(2*N_),
    _3N_(3*N_),
    _4N_(4*N_),
    _5N_(5*N_),
    _5N_p_7_(_5N_ + 7),
    _5N_p_19_(_5N_ + 19),
    M_(slip::Vector<Type>(12)),
    r_(slip::Vector<Type>(2*N_))
 {
   std::copy(data_.begin()+_5N_p_7_,data_.begin()+_5N_p_19_,M_.begin());
 }
 
   
 slip::Vector<Type>& operator() (const slip::Vector<Type>& p) // define the function
 {
   Type R1 = p[2]*data_[_5N_]; 
   Type R2 = p[3]*data_[_5N_+1]; 
   Type R3 = p[4]*data_[_5N_+2]; 
   Type D1 = p[5]*data_[_5N_+3];
   Type D2 = p[6]*data_[_5N_+4]; 
   Type P1 = p[7]*data_[_5N_+5]; 
   Type P2 = p[8]*data_[_5N_+6];
   
   Type U0 = p[0]; 
   Type V0 = p[1];
   Type two = static_cast<Type>(2.0);
   Type three = static_cast<Type>(3.0);
      
   for(std::size_t i=0, k = 0; i < N_; ++i,k+=2) 
     {
       Type u = data_(i); 
       Type v = data_(N_+i);
       Type X = data_(_2N_+i); 
       Type Y = data_(_3N_+i); 
       Type Z = data_(_4N_+i);
       Type den = (M_[6]*X + M_[7]*Y + M_[8]*Z + M_[11]);
       Type xr = (M_[0]*X + M_[1]*Y + M_[2]*Z + M_[9] )/den;
       Type yr = (M_[3]*X + M_[4]*Y + M_[5]*Z + M_[10])/den;
       
       Type xr_p = xr-U0;
       Type yr_p = yr-V0;
       Type xr_p2 = xr_p * xr_p;
       Type yr_p2 = yr_p * yr_p;
       Type two_xr_p_yr_p = two * xr_p * yr_p;
       Type rhoi2 = xr_p2 + yr_p2;
       Type rhoi22  = rhoi2  * rhoi2; 
       Type rhoi222 = rhoi22 * rhoi2;
       Type R123 = (R1*rhoi2 + R2*rhoi22 + R3*rhoi222);
       Type xd = xr + xr_p*R123 + D1*two_xr_p_yr_p + D2*(yr_p2 + three*xr_p2) + P1*rhoi2;
       Type yd = yr + yr_p*R123 + D2*two_xr_p_yr_p + D1*(xr_p2 + three*yr_p2) + P2*rhoi2;
       r_[k]   = u - xd;
       r_[k+1] = v - yd;
     }
   return r_;
 }

 
  slip::Vector<Type> data_;
  std::size_t N_;
  std::size_t _2N_;
  std::size_t _3N_;
  std::size_t _4N_;
  std::size_t _5N_;
  std::size_t _5N_p_7_;
  std::size_t _5N_p_19_;
  slip::Vector<Type> M_;
  slip::Vector<Type> r_;
};


template <typename T,typename RandomAccessIterator>
T LM_chi2(RandomAccessIterator first, RandomAccessIterator last)
{
  T chisq = T();
  const T N = static_cast<T>(last-first);
  for(; first != last; ++first)
    {
      chisq += (*first * *first)/N;
    }
  return chisq;
}


template<typename Function, 
                 typename Real,
                 typename DerivativeFunction>
void marquardt (Function& fun,
                                DerivativeFunction& df,
                                slip::Vector<Real> &par, 
                                slip::Vector<Real>& r,
                                Real& calchi2,
                                const int maxiter = 10000,
                                const Real eps = static_cast<Real>(1e-6))
{

  //const int maxiter = 10000;
   const std::size_t N = r.size();
   const std::size_t npar = par.size();

   slip::Vector<Real> par2(npar);
   slip::Vector<Real> delta(npar);
   slip::Vector<Real> b(npar);
   //  double tmp;
   slip::Matrix<Real> aprime(npar,npar);
   //std::cout<<"npar = "<<npar<<" N = "<<N<<std::endl;
   slip::Matrix<Real> A(npar,N);


   //const Real eps = static_cast<Real>(1e-6);
   Real lamfac = static_cast<Real>(2.0);
   Real two = static_cast<Real>(2.0);
   Real minus_two = - two;
   Real zero = static_cast<Real>(0.0);
   Real lambda = static_cast<Real>(0.5); 
   bool done = 0; 
   int decrease = 0; 
   int niter = 1;

   //value for the initial fit
   r = fun(par);
   //compute jacobian
   df(par.begin(),par.end(),A); /* A= matrix jacobien transpose */

   //compute chi^2
   Real chisq = LM_chi2<Real>(r.begin(),r.end());

//initial matrices; curvature matrix H and b
   slip::Matrix<Real> AAt(npar,npar);
   slip::matrix_matrixt_multiplies(A,AAt);

   for(std::size_t i = 0; i < npar; ++i) 
      {
                b[i] = zero;
                for(std::size_t k=0; k < N; ++k)
                  {
                    b[i] += minus_two*A[i][k]*r[k] ;
                  }
      }

//Main Loop
   while(!done) 
     {
       /* Set up a' */
       for (std::size_t k = 0; k < npar; ++k) 
                 {
                   for (std::size_t i = 0; i < npar; ++i) 
                     {
                       aprime[i][k] = two * AAt[i][k];
                     }
                   aprime[k][k] = (two*AAt[k][k] + lambda);
                 }
       /* Solve Matrix Equation: a'.delta = b */
       slip::lu_solve(aprime,delta,b);

       /* Increment parameters in work space and evaluate new chisq */
       for (std::size_t i = 0; i < npar; ++i)
                 {
                   par2[i] = par[i] + delta[i];
                 }
       r = fun(par2);

       //compute chisq2
       Real chisq2 = LM_chi2<Real>(r.begin(),r.end());

       if (chisq2 <= chisq) 
                 {
                   decrease = 1;
                   if (chisq == chisq2)
                     {
                       done = 1;
                     }
                   if (fabs(((chisq - chisq2)/chisq)) < eps && decrease )
                     {
                       done = 1;
                     }
           std::copy(par2.begin(),par2.end(),par.begin());

           if (!done) 
                     {
                       // new Hessien and b
                       df(par.begin(),par.end(),A);
                       slip::matrix_matrixt_multiplies(A,AAt);
                       for(std::size_t i = 0; i < npar; ++i) 
                                 {
                                   b[i] = zero;
                                   for(std::size_t k = 0; k < N; ++k)
                                     {
                                       b[i] += minus_two* A[i][k] * r[k] ;
                                     }
                                 }
                       chisq = chisq2; 
                       lambda = lambda / lamfac; 
                       decrease = 1;
                     }
                 }
       else 
                 {
                   decrease = 0;
                   lambda = two*lambda;
                 }

       if (niter > maxiter) 
                 {
                   done = 1;
                 }
       niter++;
     }
   
   calchi2 = LM_chi2<Real>(r.begin(),r.end());
}

template <typename Function,
                  typename Type>
struct LMDerivFunctor
{

  LMDerivFunctor(const Function& fun,
                                 const int N,
                                 const int npar):
    fun_(fun),
    N_(N),
    npar_(npar),
    p_inf_(slip::Vector<Type>(npar)),
    p_sup_(slip::Vector<Type>(npar)),
    rinf_(slip::Vector<Type>(N,static_cast<Type>(0.0))),
    rsup_(slip::Vector<Type>(N,static_cast<Type>(0.0))),
    delta_(0.001),
    delta2_(0.002)
     {}

   LMDerivFunctor(const Function& fun,
                                 const int N,
                                  const int npar,
                                  const Type& delta):
    fun_(fun),
    N_(N),
    npar_(npar),
    p_inf_(slip::Vector<Type>(npar)),
    p_sup_(slip::Vector<Type>(npar)),
    rinf_(slip::Vector<Type>(N,static_cast<Type>(0.0))),
    rsup_(slip::Vector<Type>(N,static_cast<Type>(0.0))),
    delta_(delta),
    delta2_(static_cast<Type>(2.0)*delta)
     {}
  
 
  template <typename RandomAccessIterator>
  void operator()(RandomAccessIterator p_first, 
                                  RandomAccessIterator p_last,
                                  slip::Matrix<Type>& A)
  {
    assert(static_cast<int>(p_last-p_first)==npar_);
    
    std::copy(p_first,p_last,p_sup_.begin());
    std::copy(p_first,p_last,p_inf_.begin());
    typename slip::Vector<Type>::iterator it_p_inf = p_inf_.begin();
    typename slip::Vector<Type>::iterator it_p_sup = p_sup_.begin();
    
    for(int k =0; p_first != p_last; ++k,++p_first, ++it_p_inf,++it_p_sup)
      {
                Type pk = *p_first;
                *it_p_inf = pk - delta_;
                *it_p_sup = pk + delta_;
                rsup_ = fun_(p_sup_);
                rinf_ = fun_(p_inf_);
                for (int i = 0; i < N_ ; ++i)
                  {             
                    A[k][i] =(rsup_[i] - rinf_[i]) / delta2_;
                  }
                *it_p_inf = pk;
                *it_p_sup = pk;
      }
  }

  //attributes
  Function fun_;
  int N_;
  int npar_;
  slip::Vector<Type> p_inf_;
  slip::Vector<Type> p_sup_;
  slip::Vector<Type> rinf_;
  slip::Vector<Type> rsup_; 
  Type delta_;
  Type delta2_;
};



}//slip::

#endif // SLIP_LEVENBERG_MARQUARDT_HPP