---

ScaleInvariantMarquardtMinimizer.cpp

Go to the documentation of this file.

// ScaleInvariantMarquardtMinimizer.cpp: implementation of the CScaleInvariantMarquardtMinimizer class.
//

#include "ScaleInvariantMarquardtMinimizer.h"

// Construction/Destruction

CScaleInvariantMarquardtMinimizer::CScaleInvariantMarquardtMinimizer()
{
        m_dLambda = 1.0e-02;
        m_dChiSqTol = 1.0e-03;
        m_dGradTol = 1.0e-03;
        m_dParTol = 1.0e-05;
        m_dTau = 1.0e-03;
        m_iNIterations = 1000;
        m_bPositiveSemiDef = true;
        m_bCheckChi = true;
        m_bCheckGrad = true;
        m_bCheckPar = true;
        m_dNu = 10.0;
        m_dFuncAccuracy = 1.0e-06;
}

CScaleInvariantMarquardtMinimizer::~CScaleInvariantMarquardtMinimizer()
{

}

CScaleInvariantMarquardtMinimizer::CScaleInvariantMarquardtMinimizer(CParameterFilter *pFilter, bool psdFlag, double marquardt, bool checkChi, double chiTol, bool checkGrad, double gradTol, bool checkPar, double parTol, int nIterations, double funcAccuracy)
{
        m_pFilter = pFilter;
        m_dLambda = marquardt;
        m_dChiSqTol = chiTol;
        m_bCheckChi = checkChi;
        m_dGradTol = gradTol;
        m_bCheckGrad = checkGrad;
        m_dParTol = parTol;
        m_bCheckPar = checkPar;
        m_iNIterations = nIterations;
        m_bPositiveSemiDef = psdFlag;
        m_dFuncAccuracy = funcAccuracy;
        m_dTau = 1.0e-03;
        m_dNu = 10.0;
}

double CScaleInvariantMarquardtMinimizer::Minimize(double *parameters, Minimizable *minimizable)
{
    //loop variables
        int nPar, i;
        // set to false if the derivatives don't need to be recomputed
        bool computeFlag = true;
        m_bChiSqFlag = false;
        m_bGradFlag = false;
        m_bParFlag = false;
        // 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(nPar = 1; nPar < nParameters; nPar++)
        {
                m_pdAlpha[nPar] = &(m_pdAlpha[0][nPar*nParameters]);
        }
        m_pdGrad = new double[nParameters];
        m_pdDiagonal = new double[nParameters];
        m_pdForceMatrix = new double *[nParameters];
        m_pdForceMatrix[0] = new double[nParameters*m_iNResiduals];
        for(nPar = 1; nPar < nParameters; nPar++)
        {
                m_pdForceMatrix[nPar] = &(m_pdForceMatrix[0][nPar*m_iNResiduals]);
        }
        
        // non-member data
        double *parLambda = new double[nParameters];
        double *parLambdaDiv = new double[nParameters];
        double *trialPOne = new double[nParameters];
        double *trialPTwo = new double[nParameters];

        // matrix/vector manipulations
        CMatrixOperations *pMatrixOps = new CMatrixOperations();
        
        // compute the starting cost
        double chiSqOld = nlls->ObjectiveFunction(parameters);
        cout << "Starting cost of " << chiSqOld << endl;
        double chiSqLambda = chiSqOld;
        double chiSqLambdaDiv = chiSqOld;

        int nIterations = 0;
        while( nIterations < m_iNIterations )
        {
                // resetting the Marquardt parameter at the beginning seems to help
                // with the number of evaluations sometimes
                // m_dLambda = 1.0e-02;

                // compute alpha,gradient
                if(computeFlag){
                        ComputeDerivativeInformation(parameters,nlls);
                        // now rescale both the Hessian and the gradient
                        for(i = 0; i < nParameters; i++)
                        {
                                m_pdGrad[i] = m_pdGrad[i]/sqrt(m_pdDiagonal[i]);
                                for(int j = 0; j < nParameters; j++)
                                {
                                        double sqrt1 = sqrt(m_pdDiagonal[i]);
                                        double sqrt2 = sqrt(m_pdDiagonal[j]);
                                        m_pdAlpha[i][j] = m_pdAlpha[i][j]/(sqrt1*sqrt2);
                                }
                        }
                }
                
                // solve for phi(lambda)
                RescaleDiagonal(m_dLambda);
                SVDSolveMarquardtSystem(parLambda);
                RestoreDiagonal();

                // solve for phi(lambda/nu)
                RescaleDiagonal(m_dLambda/m_dNu);
                SVDSolveMarquardtSystem(parLambdaDiv);
                RestoreDiagonal();
                
                ObtainTrialParameters(parameters, parLambda, trialPOne);
                chiSqLambda = nlls->ObjectiveFunction(trialPOne);
                ObtainTrialParameters(parameters, parLambdaDiv, trialPTwo);
                chiSqLambdaDiv = nlls->ObjectiveFunction(trialPTwo);
                
                if(chiSqLambdaDiv <= chiSqOld)  // Test I
                {
                        cout << "Test I passed." << endl;
                        m_dLambda = m_dLambda/m_dNu;
                        // accept move, set recompute flag
                        computeFlag = true;
                        m_bChiSqFlag = ComputeChiSqTol(chiSqOld,chiSqLambdaDiv);
                        m_bGradFlag = ComputeGradTol();
                        m_bParFlag = ComputeParTol(parameters,parLambdaDiv);
                        AcceptParameters(parameters,trialPTwo,chiSqLambdaDiv);
                        chiSqOld = chiSqLambdaDiv;
                        cout << "Move accepted, new cost is " << chiSqLambdaDiv << endl;
                }
                else if(chiSqLambda <= chiSqOld) // Test II
                {
                        cout << "Test II passed." << endl;
                        // Marquardt parameter is unchanged
                        // accept move, set recompute flag
                        computeFlag = true;
                        m_bChiSqFlag = ComputeChiSqTol(chiSqOld,chiSqLambda);
                        m_bGradFlag = ComputeGradTol();
                        m_bParFlag = ComputeParTol(parameters,parLambda);
                        AcceptParameters(parameters,trialPOne,chiSqLambda);
                        chiSqOld = chiSqLambda;
                        cout << "Move accepted, new cost is  " << chiSqLambda << endl;
                }
                else  // Test III
                {
                        double lambdaMult = m_dLambda;
                        double chiSqLambdaMult = chiSqLambda;

                        double piOverFour = 0.78539816339744825;

                        while(chiSqLambdaMult > chiSqOld)
                        {
                                double numerator = pMatrixOps->DotProduct(m_pdGrad,parLambda,nParameters);
                                double denominator = (pMatrixOps->VectorL2Norm(m_pdGrad,nParameters))*(pMatrixOps->VectorL2Norm(parLambda,nParameters));
                                double gamma = acos(numerator/denominator);

                                if(gamma > piOverFour)
                                {
                                        // increase lambda and try again
                                        lambdaMult = lambdaMult*m_dNu;
                                        
                                        // solve for phi(lambda/nu)
                                        RescaleDiagonal(lambdaMult);
                                        SVDSolveMarquardtSystem(parLambda);
                                        RestoreDiagonal();
                
                                        ObtainTrialParameters(parameters, parLambda, trialPOne);
                                        chiSqLambdaMult = nlls->ObjectiveFunction(trialPOne);
                                }
                                else
                                {
                                        while(chiSqLambdaMult > chiSqOld)
                                        {
                                                // rescale the multiple of the parameters we're adding 
                                                // until the cost decreases
                                                for(i = 0; i < nParameters; i++)
                                                {
                                                        parLambda[i] = 0.5*parLambda[i];
                                                }
                                                ObtainTrialParameters(parameters,parLambda,trialPOne);
                                                chiSqLambdaMult = nlls->ObjectiveFunction(trialPOne);
                                        }
                                }

                        }
                        cout << "Test III passed." << endl;
                        // accept move, rescale 
                        computeFlag = true;
                        m_bChiSqFlag = ComputeChiSqTol(chiSqOld,chiSqLambdaMult);
                        m_bGradFlag = ComputeGradTol();
                        m_bParFlag = ComputeParTol(parameters,parLambda);
                        AcceptParameters(parameters,trialPOne,chiSqLambdaMult);
                        chiSqOld = chiSqLambdaMult;
                        m_dLambda = lambdaMult;
                        cout << "Move accepted, new cost is " << chiSqLambdaMult << endl;
                }
                if(m_bChiSqFlag || m_bGradFlag || m_bParFlag)
                {
                        if(m_bChiSqFlag)
                        {
                                cout << "Convergence on Chi-Squared criterion." << endl;
                        }
                        if(m_bGradFlag)
                        {
                                cout << "Convergence on ||g|| criterion." << endl;
                        }
                        if(m_bParFlag)
                        {
                                cout << "Convergence on parameter criterion." << endl;
                        }
                        break;
                }

                nIterations++;
        }
        

        delete pMatrixOps;
        delete [] parLambda;
        delete [] parLambdaDiv;
        delete [] trialPOne;
        delete [] trialPTwo;
        delete [] m_pdGrad;
        delete [] m_pdDiagonal;
        delete [] m_pdAlpha[0];
        delete [] m_pdAlpha;

        return chiSqOld;
}

// 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 CScaleInvariantMarquardtMinimizer::SVDSolveMarquardtSystem(double *deltaP)
{
        int i,j,l;
        // 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(j = 0; j < nParameters; j++)
        {
                deltaP[j] = 0.0;
                for(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.  If the problem is underdetermined, we know there 
        // MUST be at least M - N singular values that are identically equal to zero 
        // (the last M - N in the list) even if numerical slop gives us small but nonzero
        // ones in the computed Hessian.  While these may always be so small as to be 
        // truncated, it's best to be safe.

        double cutoff = 1.0e-6*D[0];
        for(i = 0; i < nParameters; i++)
        {
                if(D[i] < cutoff) {D[i] = 0;}
                else {D[i] = 1/D[i];}
        }

        // here are the ones that have to be zero
        int diff = nParameters - m_iNResiduals;
        for(i = 0; i < diff; i++)
        {
                D[(nParameters-1) - i] = 0.0;
        }

        // 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_pdGrad[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 with appropriate rescaling
        for(i = 0; i < nParameters; i++)
        {
                deltaP[i] = S[i]/sqrt(m_pdDiagonal[i]);
        }

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

---