|
|
|
00001 #include "TrackingTools/TrackFitters/interface/TrajectoryStateCombiner.h" 00002 #include "DataFormats/Math/interface/invertPosDefMatrix.h" 00003 00004 #include "FWCore/MessageLogger/interface/MessageLogger.h" 00005 00006 TrajectoryStateOnSurface 00007 TrajectoryStateCombiner::combine(const TSOS& Tsos1, const TSOS& Tsos2) const { 00008 00009 double pzSign = Tsos1.localParameters().pzSign(); 00010 AlgebraicVector5 x1(Tsos1.localParameters().vector()); 00011 AlgebraicVector5 x2(Tsos2.localParameters().vector()); 00012 const AlgebraicSymMatrix55 &C1 = (Tsos1.localError().matrix()); 00013 const AlgebraicSymMatrix55 &C2 = (Tsos2.localError().matrix()); 00014 00015 AlgebraicSymMatrix55 Csum = C1 + C2; 00016 bool ok = invertPosDefMatrix(Csum); 00017 AlgebraicMatrix55 K = C1*Csum; 00018 00019 if(!ok) { 00020 edm::LogError("MatrixInversionFailure") 00021 <<"the inversion of the combined error matrix failed. Impossible to get a combined state." 00022 <<"\nmatrix 1:"<<C1 00023 <<"\nmatrix 2:"<<C2; 00024 return TSOS(); 00025 } 00026 00027 AlgebraicVector5 xcomb = x1 + K*(x2 - x1); 00028 //AlgebraicSymMatrix55 Ccomb; Ccomb.assign(K*C2); 00029 AlgebraicSymMatrix55 Ccomb = (AlgebraicMatrix55(K*C2)).LowerBlock(); 00030 00031 TSOS combTsos( LocalTrajectoryParameters(xcomb, pzSign), 00032 LocalTrajectoryError(Ccomb), Tsos1.surface(), 00033 &(Tsos1.globalParameters().magneticField())); 00034 return combTsos; 00035 }
1.7.3