PFTracking/src/CollinearFitAtTM.cc

0001 #include "RecoParticleFlow/PFTracking/interface/CollinearFitAtTM.h"
0002 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0003
0004 CollinearFitAtTM::CollinearFitAtTM() {
0005   //
0006   // Jacobian
0007   //
0008   for (int i = 0; i < 12; ++i) {
0009     for (int j = 0; j < 6; ++j)
0010       jacobian_(i, j) = 0;
0011   }
0012   for (int i = 1; i < 5; ++i) {
0013     jacobian_(i, ParQpOut + i) = jacobian_(i + 5, ParQpOut + i) = 1;
0014   }
0015   jacobian_(0, ParQpIn) = 1.;
0016   jacobian_(5, ParQpOut) = 1.;
0017 }
0018
0019 bool CollinearFitAtTM::fit(const TrajectoryMeasurement& tm,
0020                            ResultVector& parameters,
0021                            ResultMatrix& covariance,
0022                            double& chi2) {
0023   //
0024   // check input
0025   //
0026   if (!tm.forwardPredictedState().isValid() || !tm.backwardPredictedState().isValid() || !tm.updatedState().isValid()) {
0027     edm::LogWarning("CollinearFitAtTM") << "Invalid state in TrajectoryMeasurement";
0028     return false;
0029   }
0030   //
0031   // prepare fit
0032   //
0033   AlgebraicVector5 fwdPar = tm.forwardPredictedState().localParameters().vector();
0034   AlgebraicSymMatrix55 fwdCov = tm.forwardPredictedState().localError().matrix();
0035   AlgebraicVector5 bwdPar = tm.backwardPredictedState().localParameters().vector();
0036   AlgebraicSymMatrix55 bwdCov = tm.backwardPredictedState().localError().matrix();
0037
0038   LocalPoint hitPos(0., 0., 0.);
0039   LocalError hitErr(-1., -1., -1.);
0040   if (tm.recHit()->isValid()) {
0041     hitPos = tm.recHit()->localPosition();
0042     hitErr = tm.recHit()->localPositionError();
0043   }
0044
0045   return fit(fwdPar, fwdCov, bwdPar, bwdCov, hitPos, hitErr, parameters, covariance, chi2);
0046 }
0047
0048 bool CollinearFitAtTM::fit(const AlgebraicVector5& fwdParameters,
0049                            const AlgebraicSymMatrix55& fwdCovariance,
0050                            const AlgebraicVector5& bwdParameters,
0051                            const AlgebraicSymMatrix55& bwdCovariance,
0052                            const LocalPoint& hitPos,
0053                            const LocalError& hitErr,
0054                            ResultVector& parameters,
0055                            ResultMatrix& covariance,
0056                            double& chi2) {
0057   if (hitErr.xx() > 0)
0058     jacobian_(10, ParX) = jacobian_(11, ParY) = 1;
0059   else
0060     jacobian_(10, ParX) = jacobian_(11, ParY) = 0;
0061
0062   for (int i = 0; i < 12; ++i) {
0063     for (int j = 0; j < 12; ++j)
0064       weightMatrix_(i, j) = 0;
0065   }
0066
0067   for (int i = 0; i < 5; ++i)
0068     measurements_(i) = fwdParameters(i);
0069   weightMatrix_.Place_at(fwdCovariance, 0, 0);
0070   for (int i = 0; i < 5; ++i)
0071     measurements_(i + 5) = bwdParameters(i);
0072   weightMatrix_.Place_at(bwdCovariance, 5, 5);
0073   if (hitErr.xx() > 0) {
0074     measurements_(10) = hitPos.x();
0075     measurements_(11) = hitPos.y();
0076     weightMatrix_(10, 10) = hitErr.xx();
0077     weightMatrix_(10, 11) = weightMatrix_(11, 10) = hitErr.xy();
0078     weightMatrix_(11, 11) = hitErr.yy();
0079   } else {
0080     measurements_(10) = measurements_(11) = 0.;
0081     weightMatrix_(10, 10) = weightMatrix_(11, 11) = 1.;
0082     weightMatrix_(10, 11) = weightMatrix_(11, 10) = 0.;
0083   }
0084   //
0085   // invert covariance matrix
0086   //
0087   if (!weightMatrix_.Invert()) {
0088     edm::LogWarning("CollinearFitAtTM") << "Inversion of input covariance matrix failed";
0089     return false;
0090   }
0091
0092   //
0093   projectedMeasurements_ = ROOT::Math::Transpose(jacobian_) * (weightMatrix_ * measurements_);
0094   //
0095   // Fitted parameters and covariance matrix
0096   //
0097   covariance = ROOT::Math::SimilarityT(jacobian_, weightMatrix_);
0098   if (!covariance.Invert()) {
0099     edm::LogWarning("CollinearFitAtTM") << "Inversion of resulting weight matrix failed";
0100     return false;
0101   }
0102
0103   parameters = covariance * projectedMeasurements_;
0104
0105   //
0106   // chi2
0107   //
0108   chi2 =
0109       ROOT::Math::Similarity(measurements_, weightMatrix_) - ROOT::Math::Similarity(projectedMeasurements_, covariance);
0110
0111   return true;
0112 }