KalmanUpdators/src/Chi2MeasurementEstimator.cc

0001 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
0002 #include "TrackingTools/KalmanUpdators/interface/Chi2MeasurementEstimator.h"
0003 #include "TrackingTools/PatternTools/interface/MeasurementExtractor.h"
0004 #include "DataFormats/TrackingRecHit/interface/KfComponentsHolder.h"
0005 #include "DataFormats/GeometrySurface/interface/Plane.h"
0006 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0007
0008 namespace {
0009   template <unsigned int D>
0010   double lestimate(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit) {
0011     typedef typename AlgebraicROOTObject<D, 5>::Matrix MatD5;
0012     typedef typename AlgebraicROOTObject<5, D>::Matrix Mat5D;
0013     typedef typename AlgebraicROOTObject<D, D>::SymMatrix SMatDD;
0014     typedef typename AlgebraicROOTObject<D>::Vector VecD;
0015     using ROOT::Math::SMatrixNoInit;
0016
0017     VecD r, rMeas;
0018     SMatDD R(SMatrixNoInit{}), RMeas(SMatrixNoInit{});
0019     ProjectMatrix<double, 5, D> dummyProjFunc;
0020     auto&& v = tsos.localParameters().vector();
0021     auto&& m = tsos.localError().matrix();
0022     KfComponentsHolder holder;
0023     holder.template setup<D>(&r, &R, &dummyProjFunc, &rMeas, &RMeas, v, m);
0024     aRecHit.getKfComponents(holder);
0025
0026     R += RMeas;
0027     invertPosDefMatrix(R);
0028     return ROOT::Math::Similarity(r - rMeas, R);
0029   }
0030 }  // namespace
0031
0032 std::pair<bool, double> Chi2MeasurementEstimator::estimate(const TrajectoryStateOnSurface& tsos,
0033                                                            const TrackingRecHit& aRecHit) const {
0034   switch (aRecHit.dimension()) {
0035     case 1:
0036       return returnIt(lestimate<1>(tsos, aRecHit));
0037     case 2:
0038       return returnIt(lestimate<2>(tsos, aRecHit));
0039     case 3:
0040       return returnIt(lestimate<3>(tsos, aRecHit));
0041     case 4:
0042       return returnIt(lestimate<4>(tsos, aRecHit));
0043     case 5:
0044       return returnIt(lestimate<5>(tsos, aRecHit));
0045   }
0046   throw cms::Exception("RecHit of invalid size (not 1,2,3,4,5)");
0047 }