TrackFitters/src/TrajectoryStateCombiner.cc

0001 #include "TrackingTools/TrackFitters/interface/TrajectoryStateCombiner.h"
0002 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0003
0004 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0005
0006 TrajectoryStateOnSurface TrajectoryStateCombiner::combine(const TSOS& Tsos1, const TSOS& Tsos2) const {
0007   auto pzSign = Tsos1.localParameters().pzSign();
0008   AlgebraicVector5&& x1 = Tsos1.localParameters().vector();
0009   AlgebraicVector5&& x2 = Tsos2.localParameters().vector();
0010   const AlgebraicSymMatrix55& C1 = (Tsos1.localError().matrix());
0011   const AlgebraicSymMatrix55& C2 = (Tsos2.localError().matrix());
0012
0013   AlgebraicSymMatrix55&& Csum = C1 + C2;
0014   bool ok = invertPosDefMatrix(Csum);
0015   AlgebraicMatrix55&& K = C1 * Csum;
0016
0017   if (!ok) {
0018     if (!(C1(0, 0) == 0.0 && C2(0, 0) == 0.0))  //do not make noise about obviously bad input
0019       edm::LogWarning("MatrixInversionFailure")
0020           << "the inversion of the combined error matrix failed. Impossible to get a combined state."
0021           << "\nmatrix 1:" << C1 << "\nmatrix 2:" << C2;
0022     return TSOS();
0023   }
0024
0025   AlgebraicVector5&& xcomb = x1 + K * (x2 - x1);
0026   //AlgebraicSymMatrix55 Ccomb; Ccomb.assign(K*C2);
0027   AlgebraicSymMatrix55&& Ccomb = (AlgebraicMatrix55(K * C2)).LowerBlock();
0028
0029   return TSOS(LocalTrajectoryParameters(xcomb, pzSign),
0030               LocalTrajectoryError(Ccomb),
0031               Tsos1.surface(),
0032               &(Tsos1.globalParameters().magneticField()));
0033 }