vnl_levenberg_marquardt.cxx

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

  if (*iflag == 0) {
    if (self->trace)
      vcl_cerr << "lmder: 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);
  }
  else if (*iflag == 1) {
    f->f(ref_x, ref_fx);
    if (self->start_error_ == 0)
      self->start_error_ = ref_fx.rms();
    ++(self->num_iterations_);
  }
  else if (*iflag == 2) {
    f->gradf(ref_x, ref_fJ);
    ref_fJ.inplace_transpose();

    // check derivative?
    if ( self->check_derivatives_ > 0 )
    {
      self->check_derivatives_--;

      // use finite difference to compute Jacobian
      vnl_vector<double> feval( *n );
      vnl_matrix<double> finite_jac( *p, *n, 0.0 );
      vnl_vector<double> wa1( *n );
      long info=1;
      double diff;
      f->f( ref_x, feval );
      v3p_netlib_fdjac2_(
              lmdif_lsqfun, n, p, x,
              feval.data_block(),
              finite_jac.data_block(),
              n,
              &info,
              &(self->epsfcn),
              wa1.data_block(),
              self);
      // compute difference
      for ( unsigned i=0; i<ref_fJ.cols(); ++i )
        for ( unsigned j=0; j<ref_fJ.rows(); ++j ) {
          diff = ref_fJ(j,i) - finite_jac(j,i);
          diff = diff*diff;
          if ( diff > self->epsfcn ) {
            vcl_cout << "Jac(" << i << ", " << j << ") diff: " << ref_fJ(j,i) 
                     << "  " << finite_jac(j,i)
                     << "  " << ref_fJ(j,i)-finite_jac(j,i) << '\n';
          }
        }
    }
  }

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


//
bool vnl_levenberg_marquardt::minimize_using_gradient(vnl_vector<double>& x)
{
  //fsm
  if (! f_->has_gradient()) {
    vcl_cerr << __FILE__ ": called method minimize_using_gradient(), but f_ has no gradient.\n";
    return false;
  }

  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 << __FILE__ ": Number of unknowns("<<n<<") greater than number of data ("<<m<<")\n";
    failure_code_ = ERROR_DODGY_INPUT;
    return false;
  }

  vnl_vector<double> fx(m, 0.0);    // W m   Explicitly set target to 0.0
  vnl_vector<double> wa1(5*n + m);

  num_iterations_ = 0;
  set_covariance_ = false;
  long info;
  long size = wa1.size();
  start_error_ = 0; // Set to 0 so first call to lmder_lsqfun will know to set it.
  v3p_netlib_lmder1_(
          lmder_lsqfun, &m, &n,
          x.data_block(),
          fx.data_block(),
          fdjac_.data_block(), &m,
          &ftol,
          &info,
          ipvt_.data_block(),
          wa1.data_block(),
          &size, this);
  num_evaluations_ = num_iterations_; // for lmder, these are the same.
  if (info<0)
    info = ERROR_FAILURE;
  failure_code_ = (ReturnCodes) info;
  end_error_ = fx.rms();

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

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

void vnl_levenberg_marquardt::diagnose_outcome() const
{
  diagnose_outcome(vcl_cerr);
}

// fsm: should this function be a method on vnl_nonlinear_minimizer?
// if not, the return codes should be moved into LM.
void vnl_levenberg_marquardt::diagnose_outcome(vcl_ostream& s) const
{
#define whoami "vnl_levenberg_marquardt"
  //if (!verbose_) return;
  switch (failure_code_) {
    //  case -1:
    // have already warned.
    //    return;
  case ERROR_FAILURE:
    s << (whoami ": OIOIOI -- failure in leastsquares function\n");
    break;
  case ERROR_DODGY_INPUT:
    s << (whoami ": OIOIOI -- lmdif dodgy input\n");
    break;
  case CONVERGED_FTOL: // ftol
    s << (whoami ": converged to ftol\n");
    break;
  case CONVERGED_XTOL: // xtol
    s << (whoami ": converged to xtol\n");
    break;
  case CONVERGED_XFTOL: // both
    s << (whoami ": converged nicely\n");
    break;
  case CONVERGED_GTOL:
    s << (whoami ": converged via gtol\n");
    break;
  case FAILED_TOO_MANY_ITERATIONS:
    s << (whoami ": too many iterations\n");
    break;
  case FAILED_FTOL_TOO_SMALL:
    s << (whoami ": ftol is too small. no further reduction in the sum of squares is possible.\n");
    break;
  case FAILED_XTOL_TOO_SMALL:
    s << (whoami ": xtol is too small. no further improvement in the approximate solution x is possible.\n");
    break;
  case FAILED_GTOL_TOO_SMALL:
    s << (whoami ": gtol is too small. Fx is orthogonal to the columns of the jacobian to machine precision.\n");
    break;
  default:
    s << (whoami ": OIOIOI: unkown info code from lmder.\n");
    break;
  }
  unsigned int m = f_->get_number_of_residuals();
  s << whoami ": " << num_iterations_ << " iterations, "
    << num_evaluations_ << " evaluations, "<< m <<" residuals.  RMS error start/end "
    << get_start_error() << '/' << get_end_error() << vcl_endl;
#undef whoami
}

// 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.

// fdjac is target m*n

//: Get INVERSE of covariance at last minimum.
// Code thanks to Joss Knight ([e-mail])
vnl_matrix<double> const& vnl_levenberg_marquardt::get_JtJ()
{
  if (!set_covariance_)
  {
    vcl_cerr << __FILE__ ": get_covariance() not confirmed tested  yet\n";
    unsigned int n = fdjac_.rows();

    // matrix in FORTRAN is column-wise.
    // transpose it to get C style order
    vnl_matrix<double> r = fdjac_.extract(n,n).transpose();
    // r is upper triangular matrix according to documentation.
    // But the lower part has non-zeros somehow.
    // clear the lower part
    for (unsigned int i=0; i<n; ++i)
      for (unsigned int j=0; j<i; ++j)
        r(i,j) = 0.0;

    // compute r^T * r
    vnl_matrix<double> rtr;
    vnl_fastops::AtA(rtr, r);
    vnl_matrix<double> rtrpt (n, n);

    // Permute. First order columns.
    // Note, *ipvt_ contains 1 to n, not 0 to n-1
    vnl_vector<int> jpvt(n);
    for (unsigned int j = 0; j < n; ++j) {
      unsigned int i = 0;
      for (; i < n; i++) {
        if (ipvt_[i] == (int)j+1) {
          jpvt (j) = i;
          break;
        }
      }
      rtrpt.set_column(j, rtr.get_column(i));
    }

    // Now order rows
    for (unsigned int j = 0; j < n; ++j) {
      inv_covar_.set_row (j, rtrpt.get_row (jpvt(j)));
    }

    set_covariance_ = true;
  }
  return inv_covar_;
}