Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2023-10-25 10:05:23

0001 //
0002 //
0003 // File: src/Chisq_Constrainer.cc
0004 // Purpose: Minimize a chisq subject to a set of constraints.
0005 //          Based on the SQUAW algorithm.
0006 // Created: Jul, 2000, sss, based on run 1 mass analysis code.
0007 //
0008 // CMSSW File      : src/Chisq_Constrainer.cc
0009 // Original Author : Scott Stuart Snyder <snyder@bnl.gov> for D0
0010 // Imported to CMSSW by Haryo Sumowidagdo <Suharyo.Sumowidagdo@cern.ch>
0011 //
0012 
0013 /**
0014     @file Chisq_Constrainer.cc
0015 
0016     @brief Minimize a \f$\chi^{2}\f$ subject to a set of constraints,
0017     based on the SQUAW algorithm.  See the documentation for header
0018     file Chisq_Constrainer.h for details.
0019 
0020     @par Creation date.
0021     July 2000.
0022 
0023     @author
0024     Scott Stuart Snyder <snyder@bnl.gov>.
0025 
0026     @par Modification History
0027     Apr 2009: Haryo Sumowidagdo <Suharyo.Sumowidagdo@cern.ch>:
0028     Imported to CMSSW.<br>
0029     Oct 2009: Haryo Sumowidagdo <Suharyo.Sumowidagdo@cern.ch>:
0030     Added Doxygen tags for automatic generation of documentation.
0031 
0032     @par Terms of Usage.
0033     With consent from the original author (Scott Snyder).
0034 
0035 */
0036 
0037 #include "TopQuarkAnalysis/TopHitFit/interface/Chisq_Constrainer.h"
0038 #include "TopQuarkAnalysis/TopHitFit/interface/Defaults.h"
0039 #include <cmath>
0040 #include <cassert>
0041 #include <iostream>
0042 #include <iomanip>
0043 
0044 using std::abs;
0045 using std::cout;
0046 using std::fixed;
0047 using std::ios_base;
0048 using std::ostream;
0049 using std::resetiosflags;
0050 using std::setiosflags;
0051 using std::sqrt;
0052 
0053 //*************************************************************************
0054 
0055 namespace hitfit {
0056 
0057   Chisq_Constrainer_Args::Chisq_Constrainer_Args(const Defaults& defs)
0058       //
0059       // Purpose: Constructor.
0060       //
0061       // Inputs:
0062       //   defs -        The Defaults instance from which to initialize.
0063       //
0064       : _base_constrainer_args(defs) {
0065     _use_G = defs.get_bool("use_G");
0066     _printfit = defs.get_bool("printfit");
0067     _constraint_sum_eps = defs.get_float("constraint_sum_eps");
0068     _chisq_diff_eps = defs.get_float("chisq_diff_eps");
0069     _maxit = defs.get_int("maxit");
0070     _max_cut = defs.get_int("maxcut");
0071     _cutsize = defs.get_float("cutsize");
0072     _min_tot_cutsize = defs.get_float("min_tot_cutsize");
0073     _chisq_test_eps = defs.get_float("chisq_test_eps");
0074   }
0075 
0076   bool Chisq_Constrainer_Args::printfit() const
0077   //
0078   // Purpose: Return the printfit parameter.
0079   //          See the header for documentation.
0080   //
0081   {
0082     return _printfit;
0083   }
0084 
0085   bool Chisq_Constrainer_Args::use_G() const
0086   //
0087   // Purpose: Return the use_G parameter.
0088   //          See the header for documentation.
0089   //
0090   {
0091     return _use_G;
0092   }
0093 
0094   double Chisq_Constrainer_Args::constraint_sum_eps() const
0095   //
0096   // Purpose: Return the constraint_sum_eps parameter.
0097   //          See the header for documentation.
0098   //
0099   {
0100     return _constraint_sum_eps;
0101   }
0102 
0103   double Chisq_Constrainer_Args::chisq_diff_eps() const
0104   //
0105   // Purpose: Return the chisq_diff_eps parameter.
0106   //          See the header for documentation.
0107   //
0108   {
0109     return _chisq_diff_eps;
0110   }
0111 
0112   unsigned Chisq_Constrainer_Args::maxit() const
0113   //
0114   // Purpose: Return the maxit parameter.
0115   //          See the header for documentation.
0116   //
0117   {
0118     return _maxit;
0119   }
0120 
0121   unsigned Chisq_Constrainer_Args::max_cut() const
0122   //
0123   // Purpose: Return the max_cut parameter.
0124   //          See the header for documentation.
0125   //
0126   {
0127     return _max_cut;
0128   }
0129 
0130   double Chisq_Constrainer_Args::cutsize() const
0131   //
0132   // Purpose: Return the cutsize parameter.
0133   //          See the header for documentation.
0134   //
0135   {
0136     return _cutsize;
0137   }
0138 
0139   double Chisq_Constrainer_Args::min_tot_cutsize() const
0140   //
0141   // Purpose: Return the min_tot_cutsize parameter.
0142   //          See the header for documentation.
0143   //
0144   {
0145     return _min_tot_cutsize;
0146   }
0147 
0148   double Chisq_Constrainer_Args::chisq_test_eps() const
0149   //
0150   // Purpose: Return the chisq_test_eps parameter.
0151   //          See the header for documentation.
0152   //
0153   {
0154     return _chisq_test_eps;
0155   }
0156 
0157   const Base_Constrainer_Args& Chisq_Constrainer_Args::base_constrainer_args() const
0158   //
0159   // Purpose: Return the contained subobject parameters.
0160   //          See the header for documentation.
0161   //
0162   {
0163     return _base_constrainer_args;
0164   }
0165 
0166 }  // namespace hitfit
0167 
0168 //*************************************************************************
0169 
0170 namespace {
0171 
0172   using namespace hitfit;
0173 
0174   /**
0175      Solve the linear system<br>
0176      \f$\left( \begin{array}{rr} -H & B \cdot t \\ B & Y \end{array}\right)
0177      \left(\begin{array}{r} alpha \\ d \end{array} \right) =
0178      \left(\begin{array}{r}     r \\ 0 \end{array} \right)\f$ for <i>alpha</i> and <i>d</i>.
0179 
0180      Also returns the inverse matrices<br>
0181      \f$
0182      \left(\begin{array}{rr} W & V \cdot t \\ V & U\end{array}\right) =
0183      \left(\begin{array}{rr}-H & B \cdot t \\ B & Y\end{array}\right)^{-1}
0184      \f$
0185 
0186      @par Return:
0187      <b>true</b> if successful.<br>
0188      <b>false</b> if fail.
0189 */
0190   bool solve_linear_system(const Matrix& H,
0191                            const Diagonal_Matrix& Y,
0192                            const Matrix& By,
0193                            const Row_Vector& r,
0194                            Column_Vector& alpha,
0195                            Column_Vector& d,
0196                            Matrix& W,
0197                            Matrix& U,
0198                            Matrix& V)
0199   //
0200   // Purpose: Solve the system
0201   //
0202   //   [ -H  B.t ]   [ alpha ]     [ r ]
0203   //   [         ] * [       ]  =  [   ]
0204   //   [  B  Y   ]   [   d   ]     [ 0 ]
0205   //
0206   //  for alpha and d.
0207   //
0208   //  Also returns the inverse matrices:
0209   //
0210   //   [ W  V.t ]     [ -H  B.t ]
0211   //   [        ]  =  [         ] ^ -1
0212   //   [ V  U   ]     [  B  Y   ]
0213   //
0214   //  Returns true if successful, false if not.
0215   //
0216   {
0217     int nconstraints = H.num_row();
0218     int nbadvars = Y.num_row();
0219 
0220     // Form the matrix on the LHS from H, By, and Y.
0221     Matrix A(nconstraints + nbadvars, nconstraints + nbadvars);
0222     A.sub(1, 1, -H);
0223     if (nbadvars > 0) {
0224       A.sub(nconstraints + 1, nconstraints + 1, Y);
0225       A.sub(1, nconstraints + 1, By.T());
0226       A.sub(nconstraints + 1, 1, By);
0227     }
0228 
0229     // Form the RHS vector from r.
0230     Column_Vector yy(nconstraints + nbadvars, 0);
0231     yy.sub(1, r.T());
0232 
0233     // Invert the matrix.
0234     // Try to handle singularities correctly.
0235     Matrix Ai;
0236     int ierr = 0;
0237     do {
0238       Ai = A.inverse(ierr);
0239       if (ierr) {
0240         int allzero = 0;
0241         for (int i = 1; i <= nconstraints; i++) {
0242           allzero = 1;
0243           for (int j = 1; j <= nconstraints; j++) {
0244             if (A(i, j) != 0) {
0245               allzero = 0;
0246               break;
0247             }
0248           }
0249           if (allzero) {
0250             A(i, i) = 1;
0251             break;
0252           }
0253         }
0254         if (!allzero)
0255           return false;
0256       }
0257     } while (ierr != 0);
0258 
0259     // Solve the system of equations.
0260     Column_Vector xx = Ai * yy;
0261 
0262     // Extract the needed pieces from the inverted matrix
0263     // and the solution vector.
0264     W = Ai.sub(1, nconstraints, 1, nconstraints);
0265     if (nbadvars > 0) {
0266       U = Ai.sub(nconstraints + 1, nconstraints + nbadvars, nconstraints + 1, nconstraints + nbadvars);
0267       V = Ai.sub(nconstraints + 1, nconstraints + nbadvars, 1, nconstraints);
0268       d = xx.sub(nconstraints + 1, nconstraints + nbadvars);
0269     }
0270 
0271     alpha = xx.sub(1, nconstraints);
0272 
0273     return true;
0274   }
0275 
0276 }  // unnamed namespace
0277 
0278 namespace hitfit {
0279 
0280   //*************************************************************************
0281 
0282   Chisq_Constrainer::Chisq_Constrainer(const Chisq_Constrainer_Args& args)
0283       //
0284       // Purpose: Constructor.
0285       //
0286       // Inputs:
0287       //   args -        The parameter settings for this instance.
0288       //
0289       : Base_Constrainer(args.base_constrainer_args()), _args(args) {}
0290 
0291   double Chisq_Constrainer::fit(Constraint_Calculator& constraint_calculator,
0292                                 const Column_Vector& xm,
0293                                 Column_Vector& x,
0294                                 const Column_Vector& ym,
0295                                 Column_Vector& y,
0296                                 const Matrix& G_i,
0297                                 const Diagonal_Matrix& Y,
0298                                 Column_Vector& pullx,
0299                                 Column_Vector& pully,
0300                                 Matrix& Q,
0301                                 Matrix& R,
0302                                 Matrix& S)
0303   //
0304   // Purpose: Do a constrained fit.
0305   //
0306   // Call the number of well-measured variables Nw, the number of
0307   // poorly-measured variables Np, and the number of constraints Nc.
0308   //
0309   // Inputs:
0310   //   constraint_calculator - The object that will be used to evaluate
0311   //                   the constraints.
0312   //   xm(Nw)      - The measured values of the well-measured variables.
0313   //   ym(Np)      - The measured values of the poorly-measured variables.
0314   //   x(Nw)       - The starting values for the well-measured variables.
0315   //   y(Np)       - The starting values for the poorly-measured variables.
0316   //   G_i(Nw,Nw)  - The error matrix for the well-measured variables.
0317   //   Y(Np,Np)    - The inverse error matrix for the poorly-measured variables.
0318   //
0319   // Outputs:
0320   //   x(Nw)       - The fit values of the well-measured variables.
0321   //   y(Np)       - The fit values of the poorly-measured variables.
0322   //   pullx(Nw)   - The pull quantities for the well-measured variables.
0323   //   pully(Nw)   - The pull quantities for the poorly-measured variables.
0324   //   Q(Nw,Nw)    - The final error matrix for the well-measured variables.
0325   //   R(Np,Np)    - The final error matrix for the poorly-measured variables.
0326   //   S(Nw,Np)    - The final cross error matrix for the two sets of variables.
0327   //
0328   // Returns:
0329   //   The minimum chisq satisfying the constraints.
0330   //   Returns a value < 0 if the fit failed to converge.
0331   //
0332   {
0333     // Check that the various matrices we've been passed have consistent
0334     // dimensionalities.
0335     int nvars = x.num_row();
0336     assert(nvars == G_i.num_col());
0337     assert(nvars == xm.num_row());
0338 
0339     int nbadvars = y.num_row();
0340     assert(nbadvars == Y.num_col());
0341     assert(nbadvars == ym.num_row());
0342 
0343     // If we're going to check the chisq calculation by explicitly using G,
0344     // calculate it now from its inverse G_i.
0345     Matrix G(nvars, nvars);
0346     if (_args.use_G()) {
0347       int ierr = 0;
0348       G = G_i.inverse(ierr);
0349       assert(!ierr);
0350     }
0351 
0352     int nconstraints = constraint_calculator.nconstraints();
0353 
0354     // Results of the constraint evaluation function.
0355     Row_Vector F(nconstraints);         // Constraint vector.
0356     Matrix Bx(nvars, nconstraints);     // Gradients wrt x
0357     Matrix By(nbadvars, nconstraints);  // Gradients wrt y
0358 
0359     // (2) Evaluate the constraints at the starting point.
0360     // If the starting point is rejected as invalid,
0361     // give up and return an error.
0362     if (!call_constraint_fcn(constraint_calculator, x, y, F, Bx, By)) {
0363       //    cout << "Bad initial values!";
0364       //    return -1000;
0365       return -999.0;
0366     }
0367 
0368     // (3) Initialize variables for the fitting loop.
0369     double constraint_sum_last = -1000;
0370     double chisq_last = -1000;
0371     bool near_convergence = false;
0372     double last_step_cutsize = 1;
0373 
0374     unsigned nit = 0;
0375 
0376     // Initialize the displacement vectors c and d.
0377     Column_Vector c = x - xm;
0378     Column_Vector d = y - ym;
0379 
0380     Matrix E(nvars, nconstraints);
0381     Matrix W(nconstraints, nconstraints);
0382     Matrix U(nbadvars, nbadvars);
0383     Matrix V(nbadvars, nconstraints);
0384 
0385     // (4) Fitting loop:
0386     do {
0387       // (5) Calculate E, H, and r.
0388       E = G_i * Bx;
0389       Matrix H = E.T() * Bx;
0390       Row_Vector r = c.T() * Bx + d.T() * By - F;
0391 
0392       // (6) Solve the linearized system for the new values
0393       // of the Lagrange multipliers
0394       // $\alpha$ and the new value for the displacements d.
0395       Column_Vector alpha(nvars);
0396       Column_Vector d1(nbadvars);
0397       if (!solve_linear_system(H, Y, By, r, alpha, d1, W, U, V)) {
0398         ///      cout << "singular matrix!";
0399         //      return -1000;
0400         return -998.0;
0401       }
0402 
0403       // (7) Compute the new values for the displacements c and the chisq.
0404       Column_Vector c1 = -E * alpha;
0405       double chisq = -scalar(r * alpha);
0406 
0407       double psi_cut = 0;
0408 
0409       // (8) Find where this step is going to be taking us.
0410       x = c1 + xm;
0411       y = d1 + ym;
0412 
0413       // (9) Set up for cutting this step, should we have to.
0414       Matrix save_By = By;
0415       Row_Vector save_negF = -F;
0416       double this_step_cutsize = 1;
0417       double constraint_sum = -1;
0418       unsigned ncut = 0;
0419 
0420       // (10) Evaluate the constraints at the new point.
0421       // If the point is rejected, we have to try to cut the step.
0422       // We accept the step if:
0423       //  The constraint sum is below the convergence threshold
0424       //    constraint_sum_eps, or
0425       //  This is the first iteration, or
0426       //  The constraint sum has decreased since the last iteration.
0427       // Otherwise, the constraints have gotten worse, and we
0428       // try to cut the step.
0429       while (!call_constraint_fcn(constraint_calculator, x, y, F, Bx, By) ||
0430              ((constraint_sum = norm_infinity(F)) > _args.constraint_sum_eps() && nit > 0 &&
0431               constraint_sum > constraint_sum_last)) {
0432         // Doing step cutting...
0433         if (nit > 0 && _args.printfit() && ncut == 0) {
0434           cout << "(" << chisq << " " << chisq_last << ") ";
0435         }
0436 
0437         // (10a) If this is the first time we've tried to cut this step,
0438         // test to see if the chisq is stationary.  If it hasn't changed
0439         // since the last iteration, try a directed step.
0440         if (ncut == 0 && abs(chisq - chisq_last) < _args.chisq_diff_eps()) {
0441           // Trying a directed step now.
0442           // Try to make the smallest step which satisfies the
0443           // (linearized) constraints.
0444           if (_args.printfit())
0445             cout << " directed step ";
0446 
0447           // (10a.i) Solve the linearized system for $\beta$ and
0448           // the y-displacement vector $\delta$.
0449           Column_Vector beta(nconstraints);
0450           Column_Vector delta(nbadvars);
0451           solve_linear_system(H, Y, save_By, save_negF, beta, delta, W, U, V);
0452 
0453           // (10a.ii) Get the x-displacement vector $\gamma$.
0454           Column_Vector gamma = -E * beta;
0455 
0456           // (10a.iii) Find the destination of the directed step.
0457           x = c + xm + gamma;
0458           y = d + ym + delta;
0459 
0460           // (10a.iv) Accept this point if it's not rejected by the constraint
0461           // function, and the constraints improve.
0462           if (call_constraint_fcn(constraint_calculator, x, y, F, Bx, By) && (constraint_sum = norm_infinity(F)) > 0 &&
0463               (constraint_sum < constraint_sum_last)) {
0464             // Accept this step.  Calculate the chisq and new displacement
0465             // vectors.
0466             chisq = chisq_last - scalar((-save_negF + r * 2) * beta);
0467             c1 = x - xm;
0468             d1 = y - ym;
0469 
0470             // Exit from step cutting loop.
0471             break;
0472           }
0473         }
0474 
0475         // If this is the first time we're cutting the step,
0476         // initialize $\psi$.
0477         if (ncut == 0)
0478           psi_cut = scalar((save_negF - r) * alpha);
0479 
0480         // (10b) Give up if we've tried to cut this step too many times.
0481         if (++ncut > _args.max_cut()) {
0482           //    cout << " Too many cut steps ";
0483           //    return -1000;
0484           return -997.0;
0485         }
0486 
0487         // (10c) Set up the size by which we're going to cut this step.
0488         // Normally, this is cutsize.  But if this is the first time we're
0489         // cutting this step and the last step was also cut, set the cut
0490         // size to twice the final cut size from the last step (provided
0491         // that it is less than cutsize).
0492         double this_cutsize = _args.cutsize();
0493         if (ncut == 1 && last_step_cutsize < 1) {
0494           this_cutsize = 2 * last_step_cutsize;
0495           if (this_cutsize > _args.cutsize())
0496             this_cutsize = _args.cutsize();
0497         }
0498 
0499         // (10d) Keep track of the total amount by which we've cut this step.
0500         this_step_cutsize *= this_cutsize;
0501 
0502         // If it falls below min_tot_cutsize, give up.
0503         if (this_step_cutsize < _args.min_tot_cutsize()) {
0504           //    cout << "Cut size underflow ";
0505           //    return -1000;
0506           return -996.0;
0507         }
0508 
0509         // (10e) Cut the step: calculate the new displacement vectors.
0510         double cutleft = 1 - this_cutsize;
0511         c1 = c1 * this_cutsize + c * cutleft;
0512         d1 = d1 * this_cutsize + d * cutleft;
0513 
0514         // (10f) Calculate the new chisq.
0515         if (chisq_last >= 0) {
0516           chisq = this_cutsize * this_cutsize * chisq + cutleft * cutleft * chisq_last +
0517                   2 * this_cutsize * cutleft * psi_cut;
0518           psi_cut = this_cutsize * psi_cut + cutleft * chisq_last;
0519         } else
0520           chisq = chisq_last;
0521 
0522         // Log what we've done.
0523         if (_args.printfit()) {
0524           cout << constraint_sum << " cut " << ncut << " size " << setiosflags(ios_base::scientific) << this_cutsize
0525                << " tot size " << this_step_cutsize << resetiosflags(ios_base::scientific) << " " << chisq << "\n";
0526         }
0527 
0528         // Find the new step destination.
0529         x = c1 + xm;
0530         y = d1 + ym;
0531 
0532         // Now, go and test the step again for acceptability.
0533       }
0534 
0535       // (11) At this point, we have an acceptable step.
0536       // Shuffle things around to prepare for the next step.
0537       last_step_cutsize = this_step_cutsize;
0538 
0539       // If requested, calculate the chisq using G to test for
0540       // possible loss of precision.
0541       double chisq_b = 0;
0542       if (_args.use_G()) {
0543         chisq_b = scalar(c1.T() * G * c1) + scalar(d1.T() * Y * d1);
0544         if (chisq >= 0 && abs((chisq - chisq_b) / chisq) > _args.chisq_test_eps()) {
0545           cout << chisq << " " << chisq_b << "lost precision?\n";
0546           abort();
0547         }
0548       }
0549 
0550       // Log what we're doing.
0551       if (_args.printfit()) {
0552         cout << chisq << " ";
0553         if (_args.use_G())
0554           cout << chisq_b << " ";
0555       }
0556 
0557       double z2 = abs(chisq - chisq_last);
0558 
0559       if (_args.printfit()) {
0560         cout << constraint_sum << " " << z2 << "\n";
0561       }
0562 
0563       c = c1;
0564       d = d1;
0565       chisq_last = chisq;
0566       constraint_sum_last = constraint_sum;
0567 
0568       // (12) Test for convergence.  The conditions must be satisfied
0569       // for two iterations in a row.
0570       if (chisq >= 0 && constraint_sum < _args.constraint_sum_eps() && z2 < _args.chisq_diff_eps()) {
0571         if (near_convergence)
0572           break;  // Converged!  Exit loop.
0573         near_convergence = true;
0574       } else
0575         near_convergence = false;
0576 
0577       // (13) Give up if we've done this too many times.
0578       if (++nit > _args.maxit()) {
0579         //      cout << "too many iterations";
0580         //      return -1000;
0581         return -995.0;
0582       }
0583 
0584     } while (true);
0585 
0586     // (15) Ok, we have a successful fit!
0587 
0588     // Calculate the error matrices.
0589     Q = E * W * E.T();
0590     S = -E * V.T();
0591     R = U;
0592 
0593     // And the vectors of pull functions.
0594     pullx = Column_Vector(nvars);
0595     for (int i = 1; i <= nvars; i++) {
0596       double a = Q(i, i);
0597       if (a < 0)
0598         pullx(i) = c(i) / sqrt(-a);
0599       else {
0600         pullx(i) = 0;
0601         //      cout << " bad pull fcn for var " << i << " (" << a << ") ";
0602       }
0603     }
0604 
0605     pully = Column_Vector(nbadvars);
0606     for (int i = 1; i <= nbadvars; i++) {
0607       double a = 1 - Y(i, i) * R(i, i);
0608       if (a > 0)
0609         pully(i) = d(i) * sqrt(Y(i, i) / a);
0610       else {
0611         pully(i) = 0;
0612         //      cout << " bad pull fcn for badvar " << i << " ";
0613       }
0614     }
0615 
0616     // Finish calculation of Q.
0617     Q = Q + G_i;
0618 
0619     // Return the final chisq.
0620     return chisq_last;
0621   }
0622 
0623   std::ostream& Chisq_Constrainer::print(std::ostream& s) const
0624   //
0625   // Purpose: Print our state.
0626   //
0627   // Inputs:
0628   //   s -           The stream to which to write.
0629   //
0630   // Returns:
0631   //   The stream S.
0632   //
0633   {
0634     Base_Constrainer::print(s);
0635     s << " printfit: " << _args.printfit() << "  use_G: " << _args.use_G() << "\n";
0636     s << " constraint_sum_eps: " << _args.constraint_sum_eps() << "  chisq_diff_eps: " << _args.chisq_diff_eps()
0637       << "  chisq_test_eps: " << _args.chisq_test_eps() << "\n";
0638     s << " maxit: " << _args.maxit() << "  max_cut: " << _args.max_cut()
0639       << "  min_tot_cutsize: " << _args.min_tot_cutsize() << "  cutsize: " << _args.cutsize() << "\n";
0640     return s;
0641   }
0642 
0643 }  // namespace hitfit