levmarq.h

Go to the documentation of this file.

#ifndef ARUCO_MM__LevMarq_H
#define ARUCO_MM__LevMarq_H
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <functional>
#include <iostream>
#include <cmath>
#include "ar_omp.h"
#include <ctime>
#include <cstring>
#include <vector>
#include <chrono>
#include <iomanip>
namespace aruco{
// Levenberg-Marquardt method for general problems Inspired in
//@MISC\{IMM2004-03215,
//    author       = "K. Madsen and H. B. Nielsen and O. Tingleff",
//    title        = "Methods for Non-Linear Least Squares Problems (2nd ed.)",
//    year         = "2004",
//    pages        = "60",
//    publisher    = "Informatics and Mathematical Modelling, Technical University of Denmark, {DTU}",
//    address      = "Richard Petersens Plads, Building 321, {DK-}2800 Kgs. Lyngby",
//    url          = "http://www.ltu.se/cms_fs/1.51590!/nonlinear_least_squares.pdf"
//}
template<typename T>
class   LevMarq{
public:


    typedef    Eigen::Matrix<T,Eigen::Dynamic,1>  eVector;
    typedef    std::function<void(const eVector  &, eVector &)> F_z_x;
    typedef    std::function<void(const eVector  &,  Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> &)> F_z_J;
    LevMarq();
    LevMarq(int maxIters,double minError,double min_step_error_diff=0,double tau=1 ,double der_epsilon=1e-3);

    void setParams(int maxIters,double minError,double min_step_error_diff=0,double tau=1 ,double der_epsilon=1e-3);

    double solve(  eVector  &z, F_z_x , F_z_J)throw (std::exception);


    void init(eVector  &z, F_z_x )throw (std::exception);
    bool step(  F_z_x f_z_x , F_z_J  f_z_J)throw (std::exception);
    bool step(  F_z_x f_z_x)throw (std::exception);
    double getCurrentSolution(eVector  &z)throw (std::exception);
    double getBestSolution(eVector  &z)throw (std::exception);

    double solve(  eVector  &z, F_z_x )throw (std::exception);
    //to enable verbose mode
    bool & verbose(){return _verbose;}

    //sets a callback func call at each step
    void setStepCallBackFunc(std::function<void(const eVector  &)> callback){_step_callback=callback;}
    //sets a function that indicates when the algorithm must be stop. returns true if must stop and false otherwise
    void setStopFunction( std::function<bool(const eVector  &)> stop_function){_stopFunction=stop_function;}

    void calcDerivates(const eVector & z ,  Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> &, F_z_x);
private:
    int _maxIters;
    double _minErrorAllowed,_der_epsilon,_tau,_min_step_error_diff;
    bool _verbose;
    //--------
    eVector curr_z,x64;
    double currErr,prevErr,minErr ;
    Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> I,J;
    double mu,v;
    std::function<void(const eVector  &)> _step_callback;
    std::function<bool(const eVector  &)> _stopFunction;


};



template<typename T>
LevMarq<T>::LevMarq(){
    _maxIters=1000;_minErrorAllowed=0;_der_epsilon=1e-3;_verbose=false;_tau=1;v=5;_min_step_error_diff=0;
}
template<typename T>
LevMarq<T>::LevMarq(int maxIters,double minError,double min_step_error_diff,double tau ,double der_epsilon ){
    _maxIters=maxIters;_minErrorAllowed=minError;_der_epsilon=der_epsilon;_verbose=false;_tau=tau;v=5;_min_step_error_diff=min_step_error_diff;
}

template<typename T>
void LevMarq<T>::setParams(int maxIters,double minError,double min_step_error_diff,double tau ,double der_epsilon){
    _maxIters=maxIters;
    _minErrorAllowed=minError;
    _der_epsilon=der_epsilon;
    _tau=tau;
    _min_step_error_diff=min_step_error_diff;

}


template<typename T>
void  LevMarq<T>:: calcDerivates(const eVector & z ,  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &J,  F_z_x f_z_x)
{
#pragma omp parallel for
    for (int i=0;i<z.rows();i++) {
        eVector zp(z),zm(z);
        zp(i)+=_der_epsilon;
        zm(i)-=_der_epsilon;
        eVector xp,xm;
        f_z_x( zp,xp);
        f_z_x( zm,xm);
        eVector dif=(xp-xm)/(2.f*_der_epsilon);
        J.middleCols(i,1)=dif;
    }
}

template<typename T>
double  LevMarq<T>:: solve(  eVector  &z, F_z_x f_z_x)throw (std::exception){
    return solve(z,f_z_x,std::bind(&LevMarq::calcDerivates,this,std::placeholders::_1,std::placeholders::_2,f_z_x));
}
template<typename T>
bool  LevMarq<T>:: step(  F_z_x f_z_x)throw (std::exception){
    return step(f_z_x,std::bind(&LevMarq::calcDerivates,this,std::placeholders::_1,std::placeholders::_2,f_z_x));
}

template<typename T>
void LevMarq<T>::init(eVector  &z, F_z_x f_z_x )throw (std::exception){
    curr_z=z;
    I.resize(z.rows(),z.rows());
    I.setIdentity();
    f_z_x(curr_z,x64);
    minErr=currErr=prevErr=x64.cwiseProduct(x64).sum();
    J.resize(x64.rows(),z.rows());
    mu=-1;


}


#define splm_get_time(a,b) std::chrono::duration_cast<std::chrono::duration<double>>(a-b).count()


template<typename T>
bool LevMarq<T>::step( F_z_x f_z_x, F_z_J f_J)throw (std::exception){

    f_J(curr_z,J);
    Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> Jt=J.transpose();
    Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> JtJ=(Jt*J);

    eVector  B=-Jt*x64;
    if(mu<0){//first time only
        int max=0;
        for(int j=1;j<JtJ.cols();j++) if (JtJ(j,j)>JtJ(max,max)) max=j;
        mu=JtJ(max,max)*_tau;
    }

    double gain=0,prev_mu=0;
    int ntries=0;
    bool isStepAccepted=false;
    do{
        //add/update dumping factor to JtJ.
        //very efficient in any case, but particularly if initial dump does not produce improvement and must reenter
        for(int j=0;j<JtJ.cols();j++)  JtJ(j,j)    += mu-prev_mu;//update mu
        prev_mu=mu;
        eVector delta=   JtJ.ldlt().solve(B);
        eVector  estimated_z=curr_z+delta;
        //compute error
        f_z_x(estimated_z,x64);
        auto err=x64.cwiseProduct(x64).sum();
        auto L=0.5*delta.transpose()*((mu*delta) - B);
        gain= (err-prevErr)/ L(0,0) ;
        //get gain
        if (gain>0){
            mu=mu*std::max(double(0.33),1.-pow(2*gain-1,3));
            v=5.f;
            currErr=err;
            curr_z=estimated_z;
            isStepAccepted=true;
        }
        else{ mu=mu*v; v=v*5;}

    }while(gain<=0 && ntries++<5);

    if (_verbose) std::cout<<std::setprecision(5) <<"Curr Error="<<currErr<<" AErr(prev-curr)="<<prevErr-currErr<<" gain="<<gain<<" dumping factor="<<mu<<std::endl;
    //    //check if we must move to the new position or exit
    if ( currErr<prevErr)
        std::swap ( currErr,prevErr );

     return isStepAccepted;

}


template<typename T>
double  LevMarq<T>:: getCurrentSolution(eVector  &z)throw (std::exception){

    z=curr_z;
    return currErr;
}
template<typename T>
double  LevMarq<T>::solve( eVector  &z, F_z_x  f_z_x, F_z_J f_J)throw (std::exception){

    init(z,f_z_x);

    if( _stopFunction){
        do{
            step(f_z_x,f_J);
            if (_step_callback) _step_callback(curr_z);
        }while(!_stopFunction(curr_z));

    }
    else{
        //intial error estimation
        int mustExit=0;
        for ( int i = 0; i < _maxIters && !mustExit; i++ ) {
            if (_verbose)std::cerr<<"iteration "<<i<<"/"<<_maxIters<< "  ";
            bool isStepAccepted=step(f_z_x,f_J);
            //check if we must exit
            if ( currErr<_minErrorAllowed ) mustExit=1;
            if( fabs( prevErr -currErr)<=_min_step_error_diff  || !isStepAccepted) mustExit=2;
            //exit if error increment
            if (currErr<prevErr  )mustExit=3;
            //            if (  (prevErr-currErr)  < 1e-5 )  mustExit=true;
            if (_step_callback) _step_callback(curr_z);
        }

//        std::cout<<"Exit code="<<mustExit<<std::endl;
    }
    z=curr_z;
    return currErr;

}

}

#endif