Chisq_Constrainer.cc

Go to the documentation of this file.

//
//
// File: src/Chisq_Constrainer.cc
// Purpose: Minimize a chisq subject to a set of constraints.
//          Based on the SQUAW algorithm.
// Created: Jul, 2000, sss, based on run 1 mass analysis code.
//
// CMSSW File      : src/Chisq_Constrainer.cc
// Original Author : Scott Stuart Snyder <snyder@bnl.gov> for D0
// Imported to CMSSW by Haryo Sumowidagdo <Suharyo.Sumowidagdo@cern.ch>
//

#include "TopQuarkAnalysis/TopHitFit/interface/Chisq_Constrainer.h"
#include "TopQuarkAnalysis/TopHitFit/interface/Defaults.h"
#include <cmath>
#include <cassert>
#include <iostream>
#include <iomanip>

using std::abs;
using std::cout;
using std::fixed;
using std::ios_base;
using std::ostream;
using std::resetiosflags;
using std::setiosflags;
using std::sqrt;


//*************************************************************************


namespace hitfit {


Chisq_Constrainer_Args::Chisq_Constrainer_Args (const Defaults& defs)
//
// Purpose: Constructor.
//
// Inputs:
//   defs -        The Defaults instance from which to initialize.
//
  : _base_constrainer_args (defs)
{
  _use_G = defs.get_bool ("use_G");
  _printfit = defs.get_bool ("printfit");
  _constraint_sum_eps = defs.get_float ("constraint_sum_eps");
  _chisq_diff_eps = defs.get_float ("chisq_diff_eps");
  _maxit = defs.get_int ("maxit");
  _max_cut = defs.get_int ("maxcut");
  _cutsize = defs.get_float ("cutsize");
  _min_tot_cutsize = defs.get_float ("min_tot_cutsize");
  _chisq_test_eps = defs.get_float ("chisq_test_eps");
}


bool Chisq_Constrainer_Args::printfit () const
//
// Purpose: Return the printfit parameter.
//          See the header for documentation.
//
{
  return _printfit;
}


bool Chisq_Constrainer_Args::use_G () const
//
// Purpose: Return the use_G parameter.
//          See the header for documentation.
//
{
  return _use_G;
}


double Chisq_Constrainer_Args::constraint_sum_eps () const
//
// Purpose: Return the constraint_sum_eps parameter.
//          See the header for documentation.
//
{
  return _constraint_sum_eps;
}


double Chisq_Constrainer_Args::chisq_diff_eps () const
//
// Purpose: Return the chisq_diff_eps parameter.
//          See the header for documentation.
//
{
  return _chisq_diff_eps;
}


unsigned  Chisq_Constrainer_Args::maxit () const
//
// Purpose: Return the maxit parameter.
//          See the header for documentation.
//
{
  return _maxit;
}


unsigned  Chisq_Constrainer_Args::max_cut () const
//
// Purpose: Return the max_cut parameter.
//          See the header for documentation.
//
{
  return _max_cut;
}


double Chisq_Constrainer_Args::cutsize () const
//
// Purpose: Return the cutsize parameter.
//          See the header for documentation.
//
{
  return _cutsize;
}


double Chisq_Constrainer_Args::min_tot_cutsize () const
//
// Purpose: Return the min_tot_cutsize parameter.
//          See the header for documentation.
//
{
  return _min_tot_cutsize;
}


double Chisq_Constrainer_Args::chisq_test_eps () const
//
// Purpose: Return the chisq_test_eps parameter.
//          See the header for documentation.
//
{
  return _chisq_test_eps;
}


const Base_Constrainer_Args&
Chisq_Constrainer_Args::base_constrainer_args () const
//
// Purpose: Return the contained subobject parameters.
//          See the header for documentation.
//
{
  return _base_constrainer_args;
}


} // namespace hitfit


//*************************************************************************


namespace {


using namespace hitfit;


bool solve_linear_system (const Matrix& H,
                          const Diagonal_Matrix& Y,
                          const Matrix& By,
                          const Row_Vector& r,
                          Column_Vector& alpha,
                          Column_Vector& d,
                          Matrix& W,
                          Matrix& U,
                          Matrix& V)
//
// Purpose: Solve the system
//
//   [ -H  B.t ]   [ alpha ]     [ r ]
//   [         ] * [       ]  =  [   ]
//   [  B  Y   ]   [   d   ]     [ 0 ]
// 
//  for alpha and d.
// 
//  Also returns the inverse matrices:
// 
//   [ W  V.t ]     [ -H  B.t ]
//   [        ]  =  [         ] ^ -1
//   [ V  U   ]     [  B  Y   ]
// 
//  Returns true if successful, false if not.
//
{
  int nconstraints = H.num_row();
  int nbadvars = Y.num_row();

  // Form the matrix on the LHS from H, By, and Y.
  Matrix A (nconstraints+nbadvars, nconstraints+nbadvars);
  A.sub (1, 1, -H);
  if (nbadvars > 0) {
    A.sub (nconstraints+1, nconstraints+1, Y);
    A.sub (1, nconstraints+1, By.T());
    A.sub (nconstraints+1, 1, By);
  }

  // Form the RHS vector from r.
  Column_Vector yy(nconstraints + nbadvars, 0);
  yy.sub (1, r.T());

  // Invert the matrix.
  // Try to handle singularities correctly.
  Matrix Ai;
  int ierr = 0;
  do {
    Ai = A.inverse (ierr);
    if (ierr) {
      int allzero = 0;
      for (int i=1; i<=nconstraints; i++) {
    allzero = 1;
    for (int j=1; j<=nconstraints; j++) {
      if (A(i,j) != 0) {
        allzero = 0;
        break;
      }
    }
    if (allzero) {
      A(i,i) = 1;
      break;
    }
      }
      if (!allzero) return false;
    }
  } while (ierr != 0);

  // Solve the system of equations.
  Column_Vector xx = Ai * yy;

  // Extract the needed pieces from the inverted matrix
  // and the solution vector.
  W = Ai.sub (1, nconstraints, 1, nconstraints);
  if (nbadvars > 0) {
    U = Ai.sub (nconstraints+1, nconstraints+nbadvars,
                nconstraints+1, nconstraints+nbadvars);
    V = Ai.sub (nconstraints+1, nconstraints+nbadvars, 1, nconstraints);
    d = xx.sub (nconstraints+1, nconstraints+nbadvars);
  }

  alpha = xx.sub (1, nconstraints);

  return true;
}


} // unnamed namespace


namespace hitfit {


//*************************************************************************


Chisq_Constrainer::Chisq_Constrainer (const Chisq_Constrainer_Args& args)
//
// Purpose: Constructor.
//
// Inputs:
//   args -        The parameter settings for this instance.
//
  : Base_Constrainer (args.base_constrainer_args()),
    _args (args)
{
}


double Chisq_Constrainer::fit (Constraint_Calculator& constraint_calculator,
                               const Column_Vector& xm,
                               Column_Vector& x,
                               const Column_Vector& ym,
                               Column_Vector& y,
                               const Matrix& G_i,
                               const Diagonal_Matrix& Y,
                               Column_Vector& pullx,
                               Column_Vector& pully,
                               Matrix& Q,
                               Matrix& R,
                               Matrix& S)
//
// Purpose: Do a constrained fit.
//
// Call the number of well-measured variables Nw, the number of
// poorly-measured variables Np, and the number of constraints Nc.
//
// Inputs:
//   constraint_calculator - The object that will be used to evaluate
//                   the constraints.
//   xm(Nw)      - The measured values of the well-measured variables.
//   ym(Np)      - The measured values of the poorly-measured variables.
//   x(Nw)       - The starting values for the well-measured variables.
//   y(Np)       - The starting values for the poorly-measured variables.
//   G_i(Nw,Nw)  - The error matrix for the well-measured variables.
//   Y(Np,Np)    - The inverse error matrix for the poorly-measured variables.
//
// Outputs:
//   x(Nw)       - The fit values of the well-measured variables.
//   y(Np)       - The fit values of the poorly-measured variables.
//   pullx(Nw)   - The pull quantities for the well-measured variables.
//   pully(Nw)   - The pull quantities for the poorly-measured variables.
//   Q(Nw,Nw)    - The final error matrix for the well-measured variables.
//   R(Np,Np)    - The final error matrix for the poorly-measured variables.
//   S(Nw,Np)    - The final cross error matrix for the two sets of variables.
//
// Returns:
//   The minimum chisq satisfying the constraints.
//   Returns a value < 0 if the fit failed to converge.
//
{
  // Check that the various matrices we've been passed have consistent
  // dimensionalities.
  int nvars = x.num_row();
  assert (nvars == G_i.num_col());
  assert (nvars == xm.num_row());

  int nbadvars = y.num_row();
  assert (nbadvars == Y.num_col());
  assert (nbadvars == ym.num_row());

  // If we're going to check the chisq calculation by explicitly using G,
  // calculate it now from its inverse G_i.
  Matrix G (nvars, nvars);
  if (_args.use_G()) {
    int ierr = 0;
    G = G_i.inverse (ierr);
    assert (!ierr);
  }

  int nconstraints = constraint_calculator.nconstraints ();

  // Results of the constraint evaluation function.
  Row_Vector F (nconstraints);             // Constraint vector.
  Matrix Bx (nvars, nconstraints);         // Gradients wrt x
  Matrix By (nbadvars, nconstraints);      // Gradients wrt y

  // (2) Evaluate the constraints at the starting point.
  // If the starting point is rejected as invalid,
  // give up and return an error.
  if (! call_constraint_fcn (constraint_calculator, x, y, F, Bx, By)) {
      //    cout << "Bad initial values!";
      //    return -1000;
      return -999.0;
  }

  // (3) Initialize variables for the fitting loop.
  double constraint_sum_last = -1000;
  double chisq_last = -1000;
  bool near_convergence = false;
  double last_step_cutsize = 1;

  unsigned nit = 0;

  // Initialize the displacement vectors c and d.
  Column_Vector c = x - xm;
  Column_Vector d = y - ym;

  Matrix E (nvars, nconstraints);
  Matrix W (nconstraints, nconstraints);
  Matrix U (nbadvars, nbadvars);
  Matrix V (nbadvars, nconstraints);

  // (4) Fitting loop:
  do {
    // (5) Calculate E, H, and r.
    E = G_i * Bx;
    Matrix H = E.T() * Bx;
    Row_Vector r = c.T() * Bx + d.T() * By - F;

    // (6) Solve the linearized system for the new values
    // of the Lagrange multipliers
    // $\alpha$ and the new value for the displacements d.
    Column_Vector alpha (nvars);
    Column_Vector d1 (nbadvars);
    if (!solve_linear_system (H, Y, By, r,
                              alpha, d1, W, U, V)) {
        //      return -1000;
        return -998.0;
    }

    // (7) Compute the new values for the displacements c and the chisq.
    Column_Vector c1 = -E * alpha;
    double chisq =  - scalar (r * alpha);

    double psi_cut = 0;

    // (8) Find where this step is going to be taking us.
    x = c1 + xm;
    y = d1 + ym;

    // (9) Set up for cutting this step, should we have to.
    Matrix save_By = By;
    Row_Vector save_negF = - F;
    double this_step_cutsize = 1;
    double constraint_sum = -1;
    unsigned ncut = 0;

    // (10) Evaluate the constraints at the new point.
    // If the point is rejected, we have to try to cut the step.
    // We accept the step if:
    //  The constraint sum is below the convergence threshold
    //    constraint_sum_eps, or
    //  This is the first iteration, or
    //  The constraint sum has decreased since the last iteration.
    // Otherwise, the constraints have gotten worse, and we
    // try to cut the step.
    while (! call_constraint_fcn (constraint_calculator, x, y, F, Bx, By) ||
       ((constraint_sum = norm_infinity (F))
              > _args.constraint_sum_eps() &&
        nit > 0 &&
        constraint_sum > constraint_sum_last))
    {

      // Doing step cutting...
      if (nit > 0 && _args.printfit() && ncut == 0) {
    cout << "(" << chisq << " " << chisq_last << ") ";
      }

      // (10a) If this is the first time we've tried to cut this step,
      // test to see if the chisq is stationary.  If it hasn't changed
      // since the last iteration, try a directed step.
      if (ncut == 0 &&
      abs (chisq - chisq_last) < _args.chisq_diff_eps()) {

    // Trying a directed step now.
    // Try to make the smallest step which satisfies the
    // (linearized) constraints.
    if (_args.printfit())
      cout << " directed step ";

    // (10a.i) Solve the linearized system for $\beta$ and
    // the y-displacement vector $\delta$.
    Column_Vector beta (nconstraints);
    Column_Vector delta (nbadvars);
    solve_linear_system (H, Y, save_By, save_negF,
                 beta, delta, W, U, V);

    // (10a.ii) Get the x-displacement vector $\gamma$.
    Column_Vector gamma = -E * beta;

    // (10a.iii) Find the destination of the directed step.
    x = c + xm + gamma;
    y = d + ym + delta;

    // (10a.iv) Accept this point if it's not rejected by the constraint
    // function, and the constraints improve.
    if (call_constraint_fcn (constraint_calculator, x, y, F, Bx, By) &&
        (constraint_sum = norm_infinity (F)) > 0 &&
        (constraint_sum < constraint_sum_last)) {

      // Accept this step.  Calculate the chisq and new displacement
      // vectors.
      chisq = chisq_last - scalar ((-save_negF + r*2) * beta);
      c1 = x - xm;
      d1 = y - ym;

      // Exit from step cutting loop.
      break;
    }
      }

      // If this is the first time we're cutting the step,
      // initialize $\psi$.
      if (ncut == 0)
    psi_cut = scalar ((save_negF - r) * alpha);

      // (10b) Give up if we've tried to cut this step too many times.
      if (++ncut > _args.max_cut()) {
          //    cout << " Too many cut steps ";
          //    return -1000;
          return -997.0;
      }

      // (10c) Set up the size by which we're going to cut this step.
      // Normally, this is cutsize.  But if this is the first time we're
      // cutting this step and the last step was also cut, set the cut
      // size to twice the final cut size from the last step (provided
      // that it is less than cutsize).
      double this_cutsize = _args.cutsize();
      if (ncut == 1 && last_step_cutsize < 1) {
    this_cutsize = 2 * last_step_cutsize;
    if (this_cutsize > _args.cutsize())
      this_cutsize = _args.cutsize();
      }

      // (10d) Keep track of the total amount by which we've cut this step.
      this_step_cutsize *= this_cutsize;

      // If it falls below min_tot_cutsize, give up.
      if (this_step_cutsize < _args.min_tot_cutsize()) {
          //    cout << "Cut size underflow ";
          //    return -1000;
          return -996.0;
      }

      // (10e) Cut the step: calculate the new displacement vectors.
      double cutleft = 1 - this_cutsize;
      c1 = c1 * this_cutsize + c * cutleft;
      d1 = d1 * this_cutsize + d * cutleft;

      // (10f) Calculate the new chisq.
      if (chisq_last >= 0) {
    chisq = this_cutsize*this_cutsize * chisq +
            cutleft*cutleft * chisq_last +
               2*this_cutsize*cutleft * psi_cut;
    psi_cut = this_cutsize * psi_cut + cutleft * chisq_last;
      }
      else
    chisq = chisq_last;

      // Log what we've done.
      if (_args.printfit()) {
        cout << constraint_sum << " cut " << ncut << " size "
             << setiosflags (ios_base::scientific)
             << this_cutsize << " tot size " << this_step_cutsize
             << resetiosflags (ios_base::scientific)
             << " " << chisq << "\n";
      }

      // Find the new step destination.
      x = c1 + xm;
      y = d1 + ym;

      // Now, go and test the step again for acceptability.
    }

    // (11) At this point, we have an acceptable step.
    // Shuffle things around to prepare for the next step.
    last_step_cutsize = this_step_cutsize;

    // If requested, calculate the chisq using G to test for
    // possible loss of precision.
    double chisq_b = 0;
    if (_args.use_G()) {
      chisq_b = scalar (c1.T() * G * c1) + scalar (d1.T() * Y * d1);
      if (chisq >= 0 &&
      abs ((chisq - chisq_b) / chisq) > _args.chisq_test_eps()) {
    cout << chisq << " " << chisq_b
         << "lost precision?\n";
    abort ();
      }
    }

    // Log what we're doing.
    if (_args.printfit()) {
      cout << chisq << " ";
      if (_args.use_G())
    cout << chisq_b << " ";
    }

    double z2 = abs (chisq - chisq_last);

    if (_args.printfit()) {
      cout << constraint_sum << " " << z2 << "\n";
    }

    c = c1;
    d = d1;
    chisq_last = chisq;
    constraint_sum_last = constraint_sum;

    // (12) Test for convergence.  The conditions must be satisfied
    // for two iterations in a row.
    if (chisq >= 0 && constraint_sum < _args.constraint_sum_eps() &&
    z2 < _args.chisq_diff_eps())
    {
      if (near_convergence) break;  // Converged!  Exit loop.
      near_convergence = true;
    }
    else
      near_convergence = false;

    // (13) Give up if we've done this too many times.
    if (++nit > _args.maxit()) {
        //      cout << "too many iterations";
        //      return -1000;
        return -995.0;
    }

  } while (true);

  // (15) Ok, we have a successful fit!


  // Calculate the error matrices.
  Q = E * W * E.T();
  S = - E * V.T();
  R = U;

  // And the vectors of pull functions.
  pullx = Column_Vector (nvars);
  for (int i=1; i<=nvars; i++) {
    double a = Q(i,i);
    if (a < 0)
      pullx(i) = c(i) / sqrt (-a);
    else {
      pullx(i) = 0;
      //      cout << " bad pull fcn for var " << i << " (" << a << ") ";
    }
  }

  pully = Column_Vector (nbadvars);
  for (int i=1; i<=nbadvars; i++) {
    double a = 1 - Y(i,i)*R(i,i);
    if (a > 0)
      pully(i) = d(i) * sqrt (Y(i,i) / a);
    else {
      pully(i) = 0;
      //      cout << " bad pull fcn for badvar " << i << " ";
    }
  }

  // Finish calculation of Q.
  Q = Q + G_i;

  // Return the final chisq.
  return chisq_last;
}


std::ostream& Chisq_Constrainer::print (std:: ostream& s) const
//
// Purpose: Print our state.
//
// Inputs:
//   s -           The stream to which to write.
//
// Returns:
//   The stream S.
//
{
  Base_Constrainer::print (s);
  s << " printfit: " << _args.printfit()
    << "  use_G: " << _args.use_G() << "\n";
  s << " constraint_sum_eps: " << _args.constraint_sum_eps()
    << "  chisq_diff_eps: " << _args.chisq_diff_eps()
    << "  chisq_test_eps: " << _args.chisq_test_eps() << "\n";
  s << " maxit: " << _args.maxit()
    << "  max_cut: " << _args.max_cut()
    << "  min_tot_cutsize: " << _args.min_tot_cutsize()
    << "  cutsize: " << _args.cutsize() << "\n";
  return s;
}


} // namespace hitfit