---

RobustLevenbergMarquardtMinimizer.cpp

Go to the documentation of this file.

// RobustLevenbergMarquardtMinimizer.cpp: implementation of the CRobustLevenbergMarquardtMinimizer class.
//

#include "RobustLevenbergMarquardtMinimizer.h"

// Construction/Destruction

CRobustLevenbergMarquardtMinimizer::CRobustLevenbergMarquardtMinimizer()
{
        m_dMarquardt = 0.00001;
        m_dChiSqTol = 0.001;
        m_dGradTol = 0.001;
        m_iNIterations = 1000;
}

CRobustLevenbergMarquardtMinimizer::CRobustLevenbergMarquardtMinimizer(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;
}

CRobustLevenbergMarquardtMinimizer::~CRobustLevenbergMarquardtMinimizer()
{

}

double CRobustLevenbergMarquardtMinimizer::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;

        // 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_pdBeta = new double[nParameters];
        m_pdCurrentResiduals = new double[m_iNResiduals];
        m_pdAlpha = new double *[nParameters];
        m_pdAlpha[0] = new double[nParameters*nParameters];
        m_pdJacobian = new double *[m_iNResiduals];
        m_pdJacobian[0] = new double[m_iNResiduals*nParameters];
        m_pdScalingMatrix = new double[nParameters];
        for(int nPar = 1; nPar < nParameters; nPar++)
        {
                m_pdAlpha[nPar] = &(m_pdAlpha[0][nPar*nParameters]);
        }
        for(int nR = 1; nR < m_iNResiduals; nR++)
        {
                m_pdJacobian[nR] = &(m_pdJacobian[0][nR*nParameters]);
        }
        
        // 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];
                }
        
                QRSolveMarquardtSystem(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];
                }
                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;     

                        // 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;
        delete [] m_pdJacobian;
        delete [] m_pdJacobian[0];
        delete [] m_pdScalingMatrix;
        delete [] m_pdCurrentResiduals;

        return chiSqOld;
}

void CRobustLevenbergMarquardtMinimizer::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 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;
                }
                                
                // 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;
                        m_pdJacobian[j][i] = -1*delta;
                }
        }
        
        // 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] += m_pdJacobian[j][i]*m_pdJacobian[j][k];
                        }
                        m_pdAlpha[k][i] = m_pdAlpha[i][k];
                }
        }

        delete [] fxPlus;
        delete [] fx;

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

        return;
}

void CRobustLevenbergMarquardtMinimizer::QRSolveMarquardtSystem(double *deltaP)
{
        int M = m_iNResiduals;
        int N = nParameters;
        int minMN = __min(M,N);
        int maxMN = __max(M,N);
        int INFO = 0;
        int LWORK = 4*N;
        int LDA = M;
        double *A = new double[M*N];
        double *WORK = new double[LWORK];
        double *TAU = new double[minMN];
        int *JPVT = new int[N];

        // copy the jacobian into the temp array, in column-major order
        int counter = 0;
        for(int j = 0; j < N; j++)
        {
                for(int i = 0; i < M; i++)
                {
                        A[counter] = m_pdJacobian[i][j];
                        counter++;
                }
        }

        // decomposition and solution is done slightly differently depending upon
        // the shape of J
        if( M < N )
        {
                // XXX debug
                cout << "Using decomposition procedure for M < N . . . " << endl;
                // QR decompose J
                DGEQPF(&M, &N, A, &LDA, JPVT, TAU, WORK, &INFO);
                if(INFO != 0)
                {
                        cout << "ERROR : INFO = " << INFO << " in DGEQPF (JP = QR)" << endl;
                }
                exit(1);
        }
        else
        {

        }
}

---