---

PositiveDefiniteLevenbergMarquardtMinimizer.cpp

Go to the documentation of this file.

// PositiveDefiniteLevenbergMarquardtMinimizer.cpp: implementation of the LevenbergMarquardtMinimizer class.
//

#include "PositiveDefiniteLevenbergMarquardtMinimizer.h"

// Construction/Destruction

CPositiveDefiniteLevenbergMarquardtMinimizer::CPositiveDefiniteLevenbergMarquardtMinimizer()
{
        m_dMarquardt = 0.00001;
        m_dChiSqTol = 0.001;
        m_dGradTol = 0.001;
        m_iNIterations = 1000;
        // need to add filter to default constructor
}

CPositiveDefiniteLevenbergMarquardtMinimizer::CPositiveDefiniteLevenbergMarquardtMinimizer(CParameterFilter *pFilter, double marquardt, double chiTol, double gradTol, int nIterations)
{
        m_dMarquardt = marquardt;
        m_dChiSqTol = chiTol;
        m_dGradTol = gradTol;
        m_iNIterations = nIterations;
        m_pFilter = pFilter;
}

CPositiveDefiniteLevenbergMarquardtMinimizer::~CPositiveDefiniteLevenbergMarquardtMinimizer()
{

}

double CPositiveDefiniteLevenbergMarquardtMinimizer::Minimize(double *parameters, Minimizable *minimizable)
{
        int i;
        // set to false if the derivatives don't need to be recomputed
        bool computeFlag = true;
        m_bBreakFlag = false;
        m_bReconditionFlag = true;
        // downcast the minimizable, and throw an exception if it is not an NLLSMinimizable 
        // (LevenbergMarquardt should only be used on a sum of squares objective function)
        NLLSMinimizable *nlls = dynamic_cast<NLLSMinimizable *>(minimizable);
        if(0 == nlls)
        {
                throw std::runtime_error("Minimizable is not a sum of squares!");
        }
        
        // get the number of parameters and allocate 
        nParameters = nlls->GetNParameters();
        m_iNResiduals = nlls->GetNResiduals();
        
        m_pdAlpha = new double *[nParameters];
        m_pdAlpha[0] = new double[nParameters*nParameters];
        for(int nPar = 1; nPar < nParameters; nPar++)
        {
                m_pdAlpha[nPar] = &(m_pdAlpha[0][nPar*nParameters]);
        }
        m_pdBeta = new double[nParameters];
        m_pdUpperBounds = new double[nParameters];
        m_pdLowerBounds = new double[nParameters];

        this->SetBounds(parameters);
        
        // non-member data
        double *deltaP = new double[nParameters];
        double *trialP = new double[nParameters];
        
        // compute the starting cost
        double chiSqOld = nlls->ObjectiveFunction(parameters);
        cout << "Starting cost of " << chiSqOld << endl;
        double chiSqNew = chiSqOld;

        int nIterations = 0;
        while( (nIterations < m_iNIterations) && (!m_bBreakFlag))
        {
                // compute alpha,beta
                if(computeFlag){ComputeDerivativeInformation(parameters,nlls);}
                
                
                // rescale the diagonal elements of the hessian
                for(i = 0; i < nParameters; i++) 
                { 
                        m_pdAlpha[i][i] = (1.0 + m_dMarquardt)*m_pdAlpha[i][i];
                }
        
                SVDSolveMarquardtSystem(deltaP);
                
                // undo hessian scaling for next pass (the Marquardt parameter
                // changes regardless)
                for(i = 0; i < nParameters; i++) 
                { 
                        m_pdAlpha[i][i] = m_pdAlpha[i][i]/(1.0 + m_dMarquardt);
                }
                
                // now evaluate the cost for the new set of parameters
                m_pFilter->ForwardTransformation(parameters,nParameters);
                for(int j = 0; j < nParameters; j++)
                {
                        trialP[j] = parameters[j] + deltaP[j];
                }
                //this->CheckBounds(trialP);
                m_pFilter->BackwardTransformation(parameters,nParameters);
                m_pFilter->BackwardTransformation(trialP,nParameters);

                
                chiSqNew = nlls->ObjectiveFunction(trialP);
                
                
                // Sometimes the trial parameters are too big and we get -1*INF for 
                // the objective function; for a sum-of-squares objective function,
                // we know it is intrinsically positive so we should not accept 
                // that move and hence those parameters.  The "or" statement in the 
                // next line is an attempt to recover from these errors.
                
                if((chiSqNew >= chiSqOld) || (chiSqNew < 0.0))
                {
                        m_dMarquardt = 10*m_dMarquardt;
                        computeFlag = false;
                }
                else
                {       
                        cout << "Move accepted, new cost is " << chiSqNew << endl;
                        double fracTol = (chiSqNew - chiSqOld)/chiSqOld;
                        // accept new parameters
                        for(i = 0; i < nParameters; i++)
                        {
                                parameters[i] = trialP[i];
                        }                                               
                        chiSqOld = chiSqNew;

                        // write the accepted parameters to a file, because numerically 
                        // computing the hessian is extremely expensive
                        fstream *pftemp = new fstream("martemp.par",ios::out);
                        *pftemp << "##################################################################" << endl;
                        *pftemp << "# BEST PARAMETERS" << endl;
                        *pftemp << "# COST : " << chiSqNew << endl;
                        *pftemp << "##################################################################" << endl;
                        for(i = 0; i < nParameters; i++)
                        {
                                *pftemp << parameters[i] << endl;
                        }
                        delete pftemp;
                        // END WRITE
                        
                        // update marquardt parameter
                        m_dMarquardt = 0.1*m_dMarquardt;
                        computeFlag = true;     
                        // set new bounds
                        this->SetBounds(parameters);
                        
                        // quit the minimization if the fractional change in cost is small
                        if(fabs(fracTol) < m_dChiSqTol)
                        {
                                cout << "Convergence on Chi-Squared criterion." << endl;
                                m_bBreakFlag = true; 
                        }
                }

                nIterations++;
        }
        

        delete [] deltaP;
        delete [] trialP;
        delete [] m_pdBeta;
        delete [] m_pdAlpha[0];
        delete [] m_pdAlpha;

        return chiSqOld;
}

// Compute approximate Hessian for use in Levenberg-Marquardt alg.
// The hessian is symmetric so we only need compute 1/2 of it then
// copy the rest (might not even need to store the other 1/2?)

void CPositiveDefiniteLevenbergMarquardtMinimizer::ComputeDerivativeInformation(double *parameters, NLLSMinimizable *nlls)
{
        int i;
        double transparameter = 0.0;
        double saveparameter = 0.0;
        double gradNorm = 0.0;
        double *fxPlus = new double[m_iNResiduals];
        double *fx = new double[m_iNResiduals];
        double **jacobian = new double *[m_iNResiduals];
        jacobian[0] = new double[m_iNResiduals*nParameters];
        for(int nR = 1; nR < m_iNResiduals; nR++)
        {
                jacobian[nR] = &(jacobian[0][nR*nParameters]);
        }

        double chiSq = nlls->ObjectiveFunction(parameters);
        for(int k = 0; k < m_iNResiduals; k++)
        {
                fx[k] = (nlls->GetResiduals())[k];
        }

        // loop over parameters
        for(i = 0; i < nParameters; i++)
        {
                // transform the parameter and save its old value
                saveparameter = parameters[i];
                transparameter = m_pFilter->Operator(parameters[i]);

                // if the parameter happens to equal 0.0, the stepsize will then be 
                // zero and give NaN upon division, so we set it to 1.0e-08
                double h = 1.0e-08;
                if(transparameter > 0.0)
                {
                        h = pow(DBL_EPSILON,0.333333)*transparameter;
                        //h = DBL_EPSILON*transparameter;
                }
                                
                // forward step
                parameters[i] = m_pFilter->OperatorInverse(transparameter + h);
                double chiSqPlus = nlls->ObjectiveFunction(parameters);
                for(int k = 0; k < m_iNResiduals; k++)
                {
                        fxPlus[k] = (nlls->GetResiduals())[k];
                }

                
                // compute scaled gradient element
                m_pdBeta[i] = -0.5*((chiSqPlus - chiSq)/h);
                gradNorm += m_pdBeta[i]*m_pdBeta[i];
                
                parameters[i] = saveparameter;

                // fill in jacobian
                for(int j = 0; j < m_iNResiduals; j++)
                {
                        double delta = (fxPlus[j] - fx[j])/h;
                        jacobian[j][i] = -1*delta;
                }
        }
        /*
        // compute -J^T f
        for(i = 0; i < nParameters; i++)
        {
                m_pdBeta[i] = 0.0;
                for(int j = 0; j < m_iNResiduals; j++)
                {
                        m_pdBeta[i] += -1.0*jacobian[j][i]*fx[j]; 
                }
                gradNorm += m_pdBeta[i]*m_pdBeta[i];
        }
        */
        // compute gradient norm and check it
        gradNorm = sqrt(gradNorm);
        if(gradNorm < m_dGradTol) 
        {
                cout << "Convergence on ||grad|| criterion." << endl;
                m_bBreakFlag = true;
        }

        // compute the scaled hessian (it is symmetric)
        for(i = 0; i < nParameters; i++)
        {
                for(int k = i; k < nParameters; k++)
                {
                        m_pdAlpha[i][k] = 0.0;
                        for(int j = 0; j < m_iNResiduals; j++)
                        {
                                m_pdAlpha[i][k] += jacobian[j][i]*jacobian[j][k];
                        }
                        m_pdAlpha[k][i] = m_pdAlpha[i][k];
                }
        }

        delete [] fxPlus;
        delete [] fx;
        delete [] jacobian[0];
        delete [] jacobian;

        cout << "Hessian calculated . . ." << endl;

        return;
}

// Solves the Levenberg-Marquardt system alpha.deltaP = beta, using 
// LAPACK functions for Cholesky decomposition and backsubstitution.
// On return, deltaP contains the solution, and alpha and beta are 
// not modified.


void CPositiveDefiniteLevenbergMarquardtMinimizer::SVDSolveMarquardtSystem(double *deltaP)
{
        int l,i;
        // solve the system by SVD decomposition
        char JOBU = 'A';
        char JOBVT = 'A';
        int M = nParameters;
        int N = nParameters;
        
        // matrix A in column-major order
        double *A = new double[nParameters*nParameters];
        int counter = 0;
        for(int j = 0; j < nParameters; j++)
        {
                deltaP[j] = 0.0;
                for(int i = 0; i < nParameters; i++)
                {
                        A[counter] = m_pdAlpha[i][j];
                        counter++;
                }
        }
        
        double *D = new double[nParameters];
        double *E = new double[nParameters];
        double *TAUQ = new double[nParameters];
        double *TAUP = new double[nParameters];

        double *PT = new double[nParameters*nParameters];
        double *Q = new double[nParameters*nParameters];
        double *ATEMP = new double[nParameters*nParameters];

        int LDA = nParameters;
        double *S = new double[nParameters];
        int LDU = nParameters;
        int LDVT = nParameters;
        int LWORK = 5*M;
        double *WORK = new double[LWORK];
        int INFO = 0;

        // put the matrix into band diagonal form
        DGEBRD(&M,&N,A,&LDA,D,E,TAUQ,TAUP,WORK,&LWORK,&INFO);
        //cout << "Band diagonal form: INFO = " << INFO << endl;
        
        // now obtain Q and copy it into our array
        for(l = 0; l < nParameters*nParameters; l++)
        {
                ATEMP[l] = A[l];
        }
        char VECT = 'Q';
        int K = M;
        DORGBR(&VECT,&M,&N,&K,ATEMP,&LDA,TAUQ,WORK,&LWORK,&INFO);
        //cout << "Q : INFO = " << INFO << endl;
        for(l = 0; l < nParameters*nParameters; l++)
        {
                Q[l] = ATEMP[l];
        }
        // now obtain PT
        for(l = 0; l < nParameters*nParameters; l++)
        {
                ATEMP[l] = A[l];
        }
        VECT = 'P';
        DORGBR(&VECT,&M,&N,&K,ATEMP,&LDA,TAUP,WORK,&LWORK,&INFO);
        //cout << "PT : INFO = " << INFO << endl;
        for(l = 0; l < nParameters*nParameters; l++)
        {
                PT[l] = ATEMP[l];
        }

        // obtain the SVD decomposition of A, using the matrices found above
        char UPLO = 'U';
        int NCVT = nParameters;
        int NRU = nParameters;
        int NCC = 0;
        double *C = 0;
        int LDC = 1;
        DBDSQR(&UPLO,&N,&NCVT,&NRU,&NCC,D,E,PT,&LDVT,Q,&LDU,C,&LDC,WORK,&INFO);
        cout << "Matrix decomposed. Condition number of Hessian is " << D[0]/D[nParameters-1] << endl;

        // scroll through the singular values and find the ones whose inverses will be 
        // huge and set them to zero.
        double cutoff = 1.0e-06*D[0];
        for(i = 0; i < nParameters; i++)
        {
                if(D[i] < cutoff) {D[i] = 0;}
                else {D[i] = 1/D[i];}
        }

        // now do the multiplications to find the new parameters
        // first UT
        for(i = 0; i < nParameters; i++)
        {
                for(int j = 0; j < nParameters; j++)
                {
                        deltaP[i] += Q[i*nParameters + j]*m_pdBeta[j];
                }
        }
        // now the inverse singular values
        for(i = 0; i < nParameters; i++)
        {
                deltaP[i] = deltaP[i]*D[i];
        }
        // now VT
        for(i = 0; i < nParameters; i++)
        {
                S[i] = 0.0;
                for(int j = 0; j < nParameters; j++)
                {
                        S[i] += PT[i*nParameters + j]*deltaP[j];
                }
        }
        // copy back into deltaP
        for(i = 0; i < nParameters; i++)
        {
                deltaP[i] = S[i];
        }

        delete [] A;
        delete [] ATEMP;
        delete [] TAUQ;
        delete [] TAUP;
        delete [] Q;
        delete [] PT;
        delete [] D;
        delete [] E;
        delete [] S;
        delete [] WORK;
}

void CPositiveDefiniteLevenbergMarquardtMinimizer::LUSolveMarquardtSystem(double *deltaP)
{
        // we're giving the LAPACK routine the upper triangle (though it doesn't matter)
        char UPLO = 'U';
        char TRANS = 'N';
        // can check for correct factorization
        int INFO=0; 
        // order of A
        int N = nParameters;
        int M = nParameters;
        
        // need to be >= max(1,N);
        int LDA = nParameters;
        int LDB = nParameters;
        // only one right hand side
        int NRHS  = 1;

        // allocate temporary storage space
        double *B = new double[nParameters];

        // LU DECOMPOSITION & SOLUTION (LAPACK)
        // copy alpha and beta appropriately
        double *A = new double[nParameters*nParameters];
        int *IPIV = new int[nParameters];
        int counter = 0;
        for(int j = 0; j < nParameters; j++)
        {
                B[j] = m_pdBeta[j];
                for(int i = 0; i < nParameters; i++)
                {
                        A[counter] = m_pdAlpha[i][j];
                        counter++;
                }
        }
        
        // LU decompose
        DGETRF(&M,&N,A,&LDA,IPIV,&INFO);
        if(INFO > 0)
        {
                m_bReconditionFlag = true;
                cout << "Matrix singularity suspected, attempting to recondition . . . " << endl;
        }
        else
        {
                m_bReconditionFlag = false;
                // solve
                DGETRS(&TRANS,&N,&NRHS,A,&LDA,IPIV,B,&LDB,&INFO);
                // copy back the parameters
                for(int i = 0; i < nParameters; i++)
                {
                        deltaP[i] = B[i];
                }
        }

                
        delete [] A;
        delete [] B;
        delete [] IPIV;
}

void CPositiveDefiniteLevenbergMarquardtMinimizer::SetBounds(double *parameters)
{
        for(int i = 0; i < nParameters; i++)
        {
                m_pdLowerBounds[i] = (1.0/100.0)*parameters[i];
                m_pdUpperBounds[i] = 100.0*parameters[i];
        }
}

void CPositiveDefiniteLevenbergMarquardtMinimizer::CheckBounds(double *parameters)
{
        for(int i = 0; i < nParameters; i++)
        {
                if(parameters[i] < m_pdLowerBounds[i])
                {
                        parameters[i] = m_pdLowerBounds[i];
                }
                else if(parameters[i] > m_pdUpperBounds[i])
                {
                        parameters[i] = m_pdUpperBounds[i];
                }
        }
}

---