Minpack.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "Minpack.hh"
//#include <Base/Math/tnt/fortran.h>
#include <Base/Common/CompareFloatingPoint.hh>
#include <MathAlgo/minpack/cminpack.h>

using namespace std;

namespace BIAS
{
  long int Powell(minpack_func_nn fun, void *p,
                  Vector<double>& InitialGuess, Vector<double>& Result,
                  double Tolerance)
  {
    // Call function HYBRD1 from minpack

    long int n = InitialGuess.Size();
    long int lwa = (n*(3*n+13))/2;
    double *wa = new double[lwa];
    Result = InitialGuess;
    Vector<double> FuncVec(n);
    long int ret = (long int)hybrd1(fun, p,n, Result.GetData(), FuncVec.GetData(),
                                    Tolerance, wa, lwa);

    // Release memory
    delete[] wa;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }

  long int PowellExtended(minpack_func_nn fun, void *p,
                          Vector<double>& InitialGuess, Vector<double>& Result,
                          double EpsilonFun, double Tolerance)
  {
    // Call function HYBRD from minpack

    long int n = InitialGuess.Size();
    double *wa = new double[4*n];
    Result = InitialGuess;
    Vector<double> FuncVec(n);
    long int maxfev = 200*(n + 1);
    long int ml = n - 1;
    long int mu = n - 1;
    long int mode = 2;
    long int lr = (n*(n + 1))/2;
    double factor = 1.0;
    long int nprint = 0;
    double *diag = new double[n];
    for (long int i = 0; i < n; i++) diag[i] = 1.0;
    //long int nfev;
    int nfev;
    double *fjac = new double[n*n];
    long int ldfjac = n;
    double *r = new double[lr];
    double *qtf = new double[n];
    long int ret = (long int)hybrd(fun, p, n, Result.GetData(), FuncVec.GetData(), Tolerance,
                                   maxfev, ml, mu, EpsilonFun, diag, mode, factor, nprint,
                                   &nfev, fjac, ldfjac, r, lr, qtf,
                                   &wa[0], &wa[n], &wa[2*n], &wa[3*n]);

    // Release memory
    delete[] wa;
    delete[] fjac;
    delete[] r;
    delete[] qtf;
    delete[] diag;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }

  long int LevenbergMarquardt(minpack_funcder_mn fun, void *p,
                              long int m, long int n, Vector<double>& InitialGuess,
                              Vector<double>& Result, double Tolerance)
  {
#ifdef BIAS_DEBUG
    if (m < n) {
      BIASERR("Target function is underdetermined (" << m << " residuals < "
              << n << " parameters!");
    }
#endif

    // Call function LMDER1 from minpack

    Result = InitialGuess;
    Vector<double> FuncVec(m);
    double *fjac = new double[m*n];
    long int ldfjac = n;
    //long int *ipvt = new long int[n];
    int *ipvt = new int[n];
    long int lwa = 5*n+m;
    double *wa=new double[lwa];
    long int ret = (long int)lmder1(fun, p, m, n, Result.GetData(), FuncVec.GetData(), fjac,
                                    ldfjac, Tolerance, ipvt, wa, lwa);

    // Release memory
    delete[] fjac;
    delete[] ipvt;
    delete[] wa;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }


  long int LevenbergMarquardtExtended(minpack_funcder_mn fun,
                                      void *p,
                                      long int m, long int n,
                                      Vector<double>& InitialGuess,
                                      Vector<double>& Result,
                                      double Tolerance,
                                      long int MaxIter)
  {
#ifdef BIAS_DEBUG
    if (m < n) {
      BIASERR("Target function is underdetermined (" << m << " residuals < "
              << n << " parameters!");
    }
#endif

    // Call function LMDER from minpack

    Result = InitialGuess;
    Vector<double> FuncVec(m);
    long int maxfev = (MaxIter == LM_DEF_MAX_ITER) ? 200*(n + 1) : MaxIter;
    // Algorithm terminates when change in residuum is less than ftol
    // or change in solution vector is less than xtol
    double ftol = Tolerance;
    double xtol = Tolerance;
    double gtol = 0.0; // = Tolerance;
    long int mode = 1;
    long int nprint = 0;
    double *diag = new double[n];
    double factor = 1.0;
    int *iptv = new int[n];
    double *qtf = new double[n];
    double *wa = new double[m+3*n];
    //long int nfev, njfev;
    int nfev, njfev;
    double *fjac = new double[m*n];
    long int ldfjac = m;
    long int ret = (long int)lmder(fun, p, m, n, Result.GetData(),
                                   FuncVec.GetData(), fjac, ldfjac,
                                   ftol, xtol, gtol, maxfev, diag,
                                   mode, factor, nprint, &nfev, &njfev, iptv, qtf,
                                   &wa[0], &wa[n], &wa[2*n], &wa[3*n]);

    // Release memory
    delete[] diag;
    delete[] iptv;
    delete[] qtf;
    delete[] wa;
    delete[] fjac;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }

  long int LevenbergMarquardt(minpack_func_mn fun,void *p,
                              long int m, long int n, Vector<double>& InitialGuess,
                              Vector<double>& Result, double Tolerance, long int MaxIter)
  {
#ifdef BIAS_DEBUG
    if (m < n) {
      BIASERR("Target function is underdetermined (" << m << " residuals < "
              << n << " parameters!");
    }
    if (MaxIter != LM_DEF_MAX_ITER) {
      BIASWARN("Parameter MaxIter is not used and should be removed here!");
    }
#endif

    // Call function LMDIF1 from minpack

    Result = InitialGuess;
    Vector<double> FuncVec(m);
    //long int *iwa = new long int[n];
    int *iwa = new int[n];
    long int lwa = m*n+5*n+m;
    double *wa = new double[lwa];
    long int ret = (long int)lmdif1(fun, p, m, n, Result.GetData(), FuncVec.GetData(),
                                    Tolerance, iwa, wa, lwa);

    // Release memory
    delete[] iwa;
    delete[] wa;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }


  long int LevenbergMarquardtExtended(minpack_func_mn fun,
                                      void *p,
                                      long int num_errors,
                                      long int num_vars,
                                      Vector<double>& InitialGuess,
                                      Vector<double>& Result,
                                      double EpsilonFun,
                                      double Tolerance,
                                      long int MaxIter,
                                      double* fjac,
                                      int* ipvt,
                                      double *SumOfSquaredErrors)
  {
#ifdef BIAS_DEBUG
    if (num_errors < num_vars) {
      BIASERR("Target function is underdetermined (" << num_errors
              << " residuals < " << num_vars << " parameters!");
    }
#endif

    // Call function LMDIF from minpack

    long int ret;
    Result = InitialGuess;
    Vector<double> FuncVec(num_errors);
    long int maxfev = (MaxIter == LM_DEF_MAX_ITER) ? 200*(num_vars + 1) : MaxIter;
    // Algorithm terminates when change in residuum is less than ftol
    // or change in solution vector is less than xtol
    double ftol = Tolerance;
    double xtol = Tolerance;
    double gtol = Tolerance; // = 0.0;
    long int mode = 1;
    long int nprint = 0;
    double *diag = new double[num_vars];
    double factor = 1.0;
    const bool ExhaustiveData =
      (fjac != NULL) && (ipvt != NULL) && (SumOfSquaredErrors != NULL);
    if (!ExhaustiveData) {
      fjac = new double[num_vars*num_errors];
      ipvt = new int[num_vars];
    }
    const long int ldfjac  = num_errors;
    double *qtf = new double[num_vars];
    double *wa  = new double[num_errors+3*num_vars];
    //long int nfev;
    int nfev;
    ret = (long int)lmdif(fun, p, num_errors, num_vars, Result.GetData(),
                          FuncVec.GetData(), ftol, xtol,
                          gtol, maxfev, EpsilonFun, diag, mode, factor, nprint,
                          &nfev, fjac, ldfjac, ipvt, qtf, &wa[0], &wa[num_vars],
                          &wa[2*num_vars], &wa[3*num_vars]);

    // Release memory
    delete[] diag;
    if (!ExhaustiveData) {
      delete[] fjac;
      delete[] ipvt;
    } else {
      // Computes sum of squared errors in output parameter
      *SumOfSquaredErrors = 0;
      for (long int i = 0; i < num_vars; i++)
        *SumOfSquaredErrors += qtf[i]*qtf[i];
    }
    delete[] qtf;
    delete[] wa;

    // Return 0 in case of convergence, <0 otherwise
    return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
  }


  long int ComputeCovariance(long int NumErrors, long int NumParams, double *Jac,
                             int *Permutation, double &SumOfSquaredErrors,
                             Matrix<double>& Cov)
  {
    // Compute covariance from Levenberg-Marquardt results

    Matrix<double>mJac(NumParams, NumErrors);
    Vector<int> mperm(NumParams);
    memcpy(mJac.GetData(), Jac, NumErrors*NumParams*sizeof(double));
    memcpy(mperm.GetData(), Permutation, NumParams*sizeof(int));
    return ComputeCovariance(NumErrors, NumParams, mJac, mperm,
                             SumOfSquaredErrors, Cov);
  }


  long int ComputeCovariance(const long int NumErrors, const long int NumParams,
                             const Matrix<double> &Jac,
                             const Vector<int>& Permutation,
                             const double &SumOfSquaredErrors,
                             Matrix<double>& Cov)
  {
    // Compute covariance from Levenberg-Marquardt results

    // Enforce correct size for covariance matrix
    Cov.newsize(NumParams, NumParams);

    // Buffer for intermediate results
    Matrix<double> TMP(NumParams, NumParams), P(NumParams, NumParams);

    // Compute matrix R out of Levenberg-Marquardt Jacobian J(x),
    // citing minpack documentation (where ipvt is Permutation in our code):
    //
    // "fjac is an output m by n array. The upper n by n submatrix
    //  of fjac contains an upper triangular matrix R with
    //  diagonal elements of nonincreasing magnitude such that
    //
    //                T     T           T
    //               P *(Jac *Jac)*P = R *R,
    //
    //  where P is a permutation matrix and Jac is the final
    //  calculated Jacobian. Column j of P is column ipvt(j)
    //  (see below) of the identity matrix. The lower trapezoidal
    //  part of fjac contains information generated during
    //  the computation of R."

    int row, column, k;
    for (row = 0; row < NumParams; row++)
      for (column = 0; column < NumParams; column++)
        Cov[row][column] = (column<row) ? 0.0 : Jac[column][row];

    // Invert upper triangular matrix R
    double s;
    for (row = 0; row < NumParams; row++) {
      // invert main diagonal
      Cov[row][row] = (Equal(Cov[row][row], 0.0)) ? 0.0 : 1.0 / Cov[row][row];
      // upper right triangle part
      for (column = row + 1; column < NumParams; column++) {
        s = 0;
        for (k = row; k < column; k++) {
          s -= Cov[row][k] * Cov[k][column];
        }
        Cov[row][column] =
          (Equal(Cov[row][column], 0.0)) ? 0.0 : s / Cov[column][column];
      }
    }

    // Calculation of R^-1 * R^-T
    for (row = 0; row < NumParams; row++) {
      for (column = 0; column < NumParams; column++) {
        TMP[row][column] = 0;
        for (k = 0; k < NumParams; k++) {
          TMP[row][column] += Cov[row][k] * Cov[column][k];
        }
      }
    }

    // Copy temporary buffer into covariance matrix
    memcpy(Cov.GetData(), TMP.GetData(),
           NumParams * NumParams * sizeof(double) );

    // Construct permutation matrix P
    P.SetZero();
    for (row = 0; row < NumParams; row++) {
      P[Permutation[row]-1][row] = 1;
    }


    // Compute (R^-1 * R^-T) * P^T
    for (row = 0; row < NumParams; row++) {
      for (column = 0; column < NumParams; column++) {
        TMP[row][column] = 0;
        for (k = 0; k < NumParams; k++) {
          TMP[row][column] += Cov[row][k] * P[column][k];
        }
      }
    }

    // Compute normalization scale sigma0^2 for covariance matrix
    double sigma0squared;
    if (NumErrors > NumParams) {
      sigma0squared = SumOfSquaredErrors / (double)(NumErrors - NumParams);
    } else {
      BIASERR("Covariance approximation is not valid, NumErrors = NumParams!");
      sigma0squared = SumOfSquaredErrors;
      BIASABORT;
      return -1;
    }

    // Set Cov = P *  (R^-1 R^-T * P^T)
    P.Mult(TMP, Cov);

    // Normalize scale of covariance matrix
    Cov.MultiplyIP(sigma0squared);

    return 0;
  }

  void CheckJacobian(minpack_funcder_mn fun, void *p,
                     const long int NumErrors, const Vector<double> &x,
                     Vector<double> &error)
  {
    // Check Jacobian matrix J(x) with minpack functions CHKDER

    long int m = NumErrors;
    long int n = (long int)x.size();
    long int ldfjac = n;
    Vector<double> mx = x, xp(n);
    Matrix<double> fjac(NumErrors, n);
    Vector<double> fvec(NumErrors), fvecp(NumErrors);
    error.newsize(NumErrors);

    long int mode = 1;
    long int iflag = 1;

    // Compute xp and Jacobian numerically
    chkder(m, n, mx.GetData(), fvec.GetData(), fjac.GetData(),
           ldfjac, xp.GetData(), fvecp.GetData(), mode, error.GetData());

    // Evaluate the function at x and put into fvec
    fun(p, m, n, mx.GetData(), fvec.GetData(), fjac.GetData(), ldfjac, iflag);

    // Evaluate the function at xp and put into fvecp
    fun(p, m, n, xp.GetData(), fvecp.GetData(), fjac.GetData(), ldfjac,iflag);

    // Compute the Jacobian at x and put into fjac
    iflag = 2;
    fun(p, m, n, mx.GetData(), fvec.GetData(), fjac.GetData(), ldfjac, iflag);

    // Now compare Jacobian and numerically computed Jacobian
    mode = 2;
    chkder(m, n, mx.GetData(), fvec.GetData(), fjac.GetData(), ldfjac,
           xp.GetData(), fvecp.GetData(), mode, error.GetData());
  }

  long int ComputeJacobian(minpack_func_mn fun,void *p,
                           const int NumErrors,
                           const Vector<double> &x, Matrix<double>& fjac,
                           const double gradientEpsilon)
  {
    // Call function FDJAC2 from minpack to compute Jacobian

    long int m = NumErrors;
    long int n = (long int)x.size();
    // The leading dimension in BIAS matrix data is the number of columns
    long int ldfjac = m;
    Vector<double> mx = x;
    Matrix<double> mfjac(n, m);
    Vector<double> fvec(NumErrors);
    long int iflag = 0;
    double eps = gradientEpsilon;
    double *wa = new double[m];
    BIASASSERT(wa);

    // Fill function residual vector fvec
    fun(p, m, n, mx.GetData(), fvec.GetData(), iflag);
    long int ret = fdjac2(fun, p, m, n, mx.GetData(), fvec.GetData(),
                          mfjac.GetData(), ldfjac, eps, wa);
    fjac = mfjac.Transpose();

    // Release memory
    delete[] wa;
    return (ret == 0 ? 0 : -1);
  }

  void QRFrac(const Matrix<double> &A, Matrix<double>& Q, Matrix<double>& R)
  {
    // Call method QRFAC from minpack

    const unsigned int m = A.GetRows();
    const unsigned int n = A.GetCols();
    if((Q.GetCols()!=m)||(Q.GetRows()!=m)){
      Q.newsize(m,m);
    }
    if((R.GetCols()!=n)||(R.GetRows()!=m)){
      R.newsize(m,n);
    }
    Q.SetIdentity();
    R.SetZero();
    double *A_Data = new double[m*n];

    // Note that the matrix is stored in row major in BIAS matrix, but it is stored in
    // in column major in LAPACK. So the return from A.GetData() cannot be directly
    // used as the input of qrfac function. If you do so, you will get the QR
    // decomposition of matrix AT's, but not of A!
    for(unsigned int j=0; j<n; j++){
      for(unsigned int i=0; i<m; i++){
        A_Data[j*m + i] = A[i][j];
      }
    }
    const int lda = m; // the leading dimension of array A_Data is the number of rows
    const int pivot = 0; // pivot is set false, so no column pivoting is done
    int *ipvt = NULL; // pivot is false, so ipvt is not referenced
    const int lipvt = 1; // pivot is false, so lipvt may be as small as 1
    double *rdiag  = new double[n]; // an output array of length n which contains the
                                    //   diagonal elements of R
    double *acnorm = new double[n]; // acnorm is coincide with rdiag which contains the
                                    //   norms of the corresponding columns of input matrix A
    double *wa     = new double[n]; // pivot is false, so wa can coincide with rdiag.
    qrfac(m, n, A_Data, lda, pivot, ipvt, lipvt, rdiag, acnorm, wa);
    
    // On output, the strict upper trapezoidal part of A_Data contains the strict
    // upper trapezoidal part of R, and the lower trapezoidal  part of A_Data contains
    // a factored form of Q.  C.f. "minpack\qrfac.c" for details.

    // First save the result of R
    for(unsigned int j = 0; j < n; j++){
      for(unsigned int i = 0; i <= j && i < m; i++){
        if (i == j) {
          R[i][j] = rdiag[j];
        } else {
          R[i][j] = A_Data[j*m+i];
        }
      }
    }

    // Then save the result of Q
    Vector<double> vec(m);
    Matrix<double> Qk(m,m);
    Matrix<double> I(m,m);
    I.SetIdentity();
    for (unsigned int j = 0; j < n && j < m; j++) {
      vec.SetZero();
      for (unsigned int i = j; i < m; i++) {
        vec[i] = A_Data[j*m+i];
      }
      Qk = I - (1/vec[j]) * vec.OuterProduct(vec);
      Q = Q * Qk.Transpose();
    }
    
    // Release memory
    delete[] A_Data;
    delete[] rdiag;
    delete[] acnorm;
    delete[] wa;
  }

}