vnl_levenberg_marquardt.cxx

DTMK软件开发包,此为开源软件,是一款很好的医学图像开发资源.

// This is core/vnl/algo/vnl_levenberg_marquardt.cxx
#ifdef VCL_NEEDS_PRAGMA_INTERFACE
#pragma implementation
#endif
//:
// \file
// \author Andrew W. Fitzgibbon, Oxford RRG
// \date 31 Aug 96
//
//-----------------------------------------------------------------------------

#include "vnl_levenberg_marquardt.h"

#include <vcl_cassert.h>
#include <vcl_iostream.h>

#include <vnl/vnl_vector.h>
#include <vnl/vnl_matrix.h>
#include <vnl/vnl_fastops.h>
#include <vnl/vnl_vector_ref.h>
#include <vnl/vnl_matrix_ref.h>
#include <vnl/vnl_least_squares_function.h>
#include <vnl/algo/vnl_netlib.h> // lmdif_()

// see header
vnl_vector<double> vnl_levenberg_marquardt_minimize(vnl_least_squares_function& f,
                                                    vnl_vector<double> const& initial_estimate)
{
  vnl_vector<double> x = initial_estimate;
  vnl_levenberg_marquardt lm(f);
  lm.minimize(x);
  return x;
}

// ctor
void vnl_levenberg_marquardt::init(vnl_least_squares_function* f)
{
  f_ = f;

  // If changing these defaults, check the help comments in vnl_levenberg_marquardt.h,
  // and MAKE SURE they're consistent.
  xtol = 1e-8;           // Termination tolerance on X (solution vector)
  maxfev = 400 * f->get_number_of_unknowns(); // Termination maximum number of iterations.
  ftol = xtol * 0.01;    // Termination tolerance on F (sum of squared residuals)
  gtol = 1e-5;           // Termination tolerance on Grad(F)' * F = 0
  epsfcn = xtol * 0.001; // Step length for FD Jacobian

  unsigned int m = f_->get_number_of_residuals(); // I  Number of residuals, must be > #unknowns
  unsigned int n = f_->get_number_of_unknowns();  // I  Number of unknowns

  set_covariance_ = false;
  fdjac_.set_size(n,m);
  fdjac_.fill(0.0);
  ipvt_.set_size(n);
  ipvt_.fill(0);
  inv_covar_.set_size(n,n);
  inv_covar_.fill(0.0);
  //fdjac_ = new vnl_matrix<double>(n,m);
  //ipvt_ = new vnl_vector<int>(n);
  //covariance_ = new vnl_matrix<double>(n,n);
}

vnl_levenberg_marquardt::~vnl_levenberg_marquardt()
{
  //delete covariance_;
  //delete fdjac_;
  //delete ipvt_;
}

//--------------------------------------------------------------------------------

#ifdef VCL_SUNPRO_CC
extern "C"
#endif
void vnl_levenberg_marquardt::lmdif_lsqfun(long* n,     // I   Number of residuals
                                           long* p,     // I   Number of unknowns
                                           double* x,  // I   Solution vector, size n
                                           double* fx, // O   Residual vector f(x)
                                           long* iflag, // IO  0 ==> print, -1 ==> terminate
                                           void* userdata)
{
  vnl_levenberg_marquardt* self =
    static_cast<vnl_levenberg_marquardt*>(userdata);
  vnl_least_squares_function* f = self->f_;
  assert(*p == (int)f->get_number_of_unknowns());
  assert(*n == (int)f->get_number_of_residuals());
  vnl_vector_ref<double> ref_x(*p, const_cast<double*>(x));
  vnl_vector_ref<double> ref_fx(*n, fx);

  if (*iflag == 0) {
    if (self->trace)
      vcl_cerr << "lmdif: iter " << self->num_iterations_ << " err ["
               << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", "
               << x[4] << ", ... ] = " << ref_fx.magnitude() << '\n';

    f->trace(self->num_iterations_, ref_x, ref_fx);
    ++(self->num_iterations_);
  } else {
    f->f(ref_x, ref_fx);
  }

  if (self->start_error_ == 0)
    self->start_error_ = ref_fx.rms();

  if (f->failure) {
    f->clear_failure();
    *iflag = -1; // fsm
  }
}


// This function shouldn't be inlined, because (1) modification of the
// body will not cause the timestamp on the header to change, and so
// others will not be forced to recompile, and (2) the cost of making
// one more function call is far, far less than the cost of actually
// performing the minimisation, so the inline doesn't gain you
// anything.
//
bool vnl_levenberg_marquardt::minimize(vnl_vector<double>& x)
{
  if ( f_->has_gradient() )
    return minimize_using_gradient(x);
  else
    return minimize_without_gradient(x);
}


//
bool vnl_levenberg_marquardt::minimize_without_gradient(vnl_vector<double>& x)
{
  //fsm
  if (f_->has_gradient()) {
    vcl_cerr << __FILE__ " : WARNING. calling minimize_without_gradient(), but f_ has gradient.\n";
  }

  // e04fcf
  long m = f_->get_number_of_residuals(); // I  Number of residuals, must be > #unknowns
  long n = f_->get_number_of_unknowns();  // I  Number of unknowns

  if (m < n) {
    vcl_cerr << "vnl_levenberg_marquardt: Number of unknowns("<<n<<") greater than number of data ("<<m<<")\n";
    failure_code_ = ERROR_DODGY_INPUT;
    return false;
  }

  if (int(x.size()) != n) {
    vcl_cerr << "vnl_levenberg_marquardt: Input vector length ("<<x.size()<<") not equal to num unknowns ("<<n<<")\n";
    failure_code_ = ERROR_DODGY_INPUT;
    return false;
  }

  vnl_vector<double> fx(m, 0.0);    // W m   Storage for target vector
  vnl_vector<double> diag(n);  // I     Multiplicative scale factors for variables
  long user_provided_scale_factors = 1;  // 1 is no, 2 is yes
  double factor = 100;
  long nprint = 1;

  vnl_vector<double> qtf(n);
  vnl_vector<double> wa1(n);
  vnl_vector<double> wa2(n);
  vnl_vector<double> wa3(n);
  vnl_vector<double> wa4(m);

#ifdef DEBUG
  vcl_cerr << "STATUS: " << failure_code_ << '\n';
#endif

  num_iterations_ = 0;
  set_covariance_ = false;
  long info;
  start_error_ = 0; // Set to 0 so first call to lmdif_lsqfun will know to set it.
  v3p_netlib_lmdif_(
         lmdif_lsqfun, &m, &n,
         x.data_block(),
         fx.data_block(),
         &ftol, &xtol, &gtol, &maxfev, &epsfcn,
         &diag[0],
         &user_provided_scale_factors, &factor, &nprint,
         &info, &num_evaluations_,
         fdjac_.data_block(), &m, ipvt_.data_block(),
         &qtf[0],
         &wa1[0], &wa2[0], &wa3[0], &wa4[0],
         this);
  failure_code_ = (ReturnCodes) info;

  // One more call to compute final error.
  lmdif_lsqfun(&m,              // I    Number of residuals
               &n,              // I    Number of unknowns
               x.data_block(),  // I    Solution vector, size n
               fx.data_block(), // O    Residual vector f(x)
               &info, this);
  end_error_ = fx.rms();

#ifdef _SGI_CC_6_
  // Something fundamentally odd about the switch below on SGI native... FIXME
  vcl_cerr << "vnl_levenberg_marquardt: termination code = " << failure_code_ << vcl_endl;
  // diagnose_outcome(vcl_cerr);
  return 1;
#endif

  // Translate status code
  switch ((int)failure_code_) {
  case 1: // ftol
  case 2: // xtol
  case 3: // both
  case 4: // gtol
    return true;
  default:
    if (verbose_) diagnose_outcome();
    return false;
  }
}

//--------------------------------------------------------------------------------

#ifdef VCL_SUNPRO_CC
extern "C"
#endif
void vnl_levenberg_marquardt::lmder_lsqfun(long* n,     // I   Number of residuals
                                           long* p,     // I   Number of unknowns
                                           double* x,  // I   Solution vector, size n
                                           double* fx, // O   Residual vector f(x)
                                           double* fJ, // O   m * n Jacobian f(x)
                                           long*,
                                           long* iflag, // I   1 -> calc fx, 2 -> calc fjac
                                           void* userdata)
{
  vnl_levenberg_marquardt* self =
    static_cast<vnl_levenberg_marquardt*>(userdata);
  vnl_least_squares_function* f = self->f_;
  assert(*p == (int)f->get_number_of_unknowns());
  assert(*n == (int)f->get_number_of_residuals());
  vnl_vector_ref<double> ref_x(*p, (double*)x); // const violation!
  vnl_vector_ref<double> ref_fx(*n, fx);
  vnl_matrix_ref<double> ref_fJ(*n, *p, fJ);