10 #include <tracking/eventTimeExtraction/utilities/TimeExtractionUtils.h>
11 #include <tracking/trackFitting/fitter/base/TrackFitter.h>
12 #include <tracking/dataobjects/RecoTrack.h>
14 #include <genfit/KalmanFitStatus.h>
15 #include <genfit/KalmanFitterInfo.h>
17 #include <genfit/Tools.h>
19 #include <cdc/dataobjects/CDCRecoHit.h>
20 #include <framework/logging/Logger.h>
21 #include <TDecompChol.h>
33 void setSubOnDiagonal(TMatrixTSym <T>& target,
int iRow,
34 const TMatrixTSym <T>& source)
36 const int nColsTarget = target.GetNrows();
37 const int nColsSource = source.GetNrows();
39 T* pt = target.GetMatrixArray();
40 const T* ps = source.GetMatrixArray();
42 for (
int i = 0; i < nColsSource; ++i) {
43 for (
int j = 0; j < nColsSource; ++j) {
44 pt[(iRow + i) * nColsTarget + (iRow + j)] = ps[i * nColsSource + j];
54 void setSubOffDiagonal(TMatrixTSym <T>& target,
int iRow,
int iCol,
55 const TMatrixT <T>& source)
57 const int nColsTarget = target.GetNcols();
58 const int nRowsSource = source.GetNrows();
59 const int nColsSource = source.GetNcols();
61 T* pt = target.GetMatrixArray();
62 const T* ps = source.GetMatrixArray();
64 for (
int i = 0; i < nRowsSource; ++i) {
65 for (
int j = 0; j < nColsSource; ++j) {
66 pt[(iRow + i) * nColsTarget + (iCol + j)] = ps[i * nColsSource + j];
69 for (
int i = 0; i < nRowsSource; ++i) {
70 for (
int j = 0; j < nColsSource; ++j) {
71 pt[(iCol + j) * nColsTarget + (iRow + i)] = ps[i * nColsSource + j];
80 std::vector<EventT0::EventT0Component>& eventT0WithQualityIndex)
82 if (not eventT0->hasEventT0()) {
83 B2DEBUG(50,
"No event t0 is set. Not testing.");
89 const double quality = chi2_with_ndf.second;
91 if (std::isnan(quality)) {
92 B2DEBUG(50,
"The calculated quality is nan. Not using this EventT0 of " << eventT0->getEventT0());
96 B2DEBUG(50,
"The iteration gave a result of " << quality <<
" for " << eventT0->getEventT0());
98 eventT0Component.
quality = quality;
99 eventT0WithQualityIndex.emplace_back(eventT0Component);
100 eventT0->addTemporaryEventT0(eventT0Component);
101 eventT0->setEventT0(eventT0Component);
109 double summedChi2 = 0;
110 double summedNDF = 0;
111 unsigned int numberOfFittableRecoTracks = 0;
113 for (
RecoTrack* recoTrack : recoTracks) {
115 recoTrack->setDirtyFlag();
118 if (not trackFitter.
fit(*recoTrack)) {
123 const double ndf = recoTrack->getTrackFitStatus()->getNdf();
125 if (std::isnan(chi2)) {
129 numberOfFittableRecoTracks++;
134 if (numberOfFittableRecoTracks == 0) {
138 return {summedChi2 / numberOfFittableRecoTracks, summedNDF / numberOfFittableRecoTracks};
147 double sumFirstDerivatives = 0;
148 double sumSecondDerivatives = 0;
149 unsigned int numberOfFittableRecoTracks = 0;
151 for (
RecoTrack* recoTrack : recoTracks) {
153 recoTrack->setDirtyFlag();
156 if (not trackFitter.
fit(*recoTrack)) {
161 const double dchi2da = chi2Derivatives.first;
162 const double d2chi2da2 = chi2Derivatives.second;
164 if (std::isnan(dchi2da) or std::isnan(d2chi2da2)) {
168 if (d2chi2da2 > 20) {
169 B2DEBUG(50,
"Track with bad second derivative");
173 numberOfFittableRecoTracks++;
174 sumFirstDerivatives += dchi2da;
175 sumSecondDerivatives += d2chi2da2;
178 if (numberOfFittableRecoTracks == 0) {
182 const double extractedEventT0 = sumFirstDerivatives / sumSecondDerivatives;
183 const double extractedEventT0Uncertainty = std::sqrt(2 / sumSecondDerivatives);
185 return {extractedEventT0, extractedEventT0Uncertainty};
205 B2DEBUG(200,
"No CDC hits in track.");
214 B2WARNING(
"Skipping pruned track");
222 TMatrixDSym fullCovariance;
228 TMatrixDSym fullResidualCovariance;
229 TMatrixDSym inverseFullMeasurementCovariance;
231 inverseFullMeasurementCovariance);
234 TVectorD residualsTimeDerivative;
240 const double dchi2da = 2. * residualsTimeDerivative * (inverseFullMeasurementCovariance * residuals);
244 const double d2chi2da2 = 2. * fullResidualCovariance.Similarity(inverseFullMeasurementCovariance * residualsTimeDerivative);
247 if (d2chi2da2 > 20) {
248 B2DEBUG(200,
"Track with bad second derivative");
251 return {dchi2da, d2chi2da2};
253 B2DEBUG(50,
"Failed time extraction - skipping track");
263 std::vector<int> vDimMeas;
264 vDimMeas.reserve(hitPoints.size());
266 for (
const auto& hit : hitPoints) {
268 vDimMeas.push_back(hit->getRawMeasurement(0)->getDim());
276 TMatrixDSym& fullCovariance)
281 B2ERROR(
"Track not fitted with a Kalman fitter.");
285 if (!kfs->isFitConverged()) {
286 B2ERROR(
"Track fit didn't converge.");
290 if (!kfs->isFittedWithReferenceTrack()) {
291 B2ERROR(
"No reference track.");
296 const unsigned int nPoints = hitPoints.size();
298 const int nDim = rep->
getDim();
300 fullCovariance.ResizeTo(nPoints * nDim, nPoints * nDim);
301 std::vector<TMatrixD> vFitterGain;
302 vFitterGain.reserve(nPoints);
303 for (
unsigned int i = 0; i < nPoints; ++i) {
307 B2DEBUG(50,
"Missing KalmanFitterInfo - skipping track");
314 setSubOnDiagonal(fullCovariance, i * nDim, mop.getCov());
317 if (i + 1 < nPoints) {
321 B2DEBUG(50,
"Missing next KalmanFitterInfo - skipping track");
333 TDecompChol decomp(pred->getCov());
334 TMatrixD F = rsop->getForwardTransportMatrix();
335 decomp.MultiSolve(F);
340 vFitterGain.emplace_back(TMatrixD(update->getCov(),
341 TMatrixD::kMultTranspose, F));
343 B2DEBUG(150,
"No reference state, substituting empty fitter gain matrix.");
344 vFitterGain.emplace_back(TMatrixD(5, 5));
349 TMatrixD offDiag = mop.getCov();
350 for (
int j = i - 1; j >= 0; --j) {
351 offDiag = TMatrixD(vFitterGain[j], TMatrixD::kMult, offDiag);
352 setSubOffDiagonal(fullCovariance, j * nDim, i * nDim, offDiag);
360 const std::vector<int>& vDimMeas,
361 const TMatrixDSym& fullCovariance,
362 TMatrixDSym& fullResidualCovariance,
363 TMatrixDSym& inverseFullMeasurementCovariance)
366 const unsigned int nPoints = hitPoints.size();
368 const int nDim = rep->
getDim();
369 int measurementDimensions = std::accumulate(vDimMeas.begin(), vDimMeas.end(), 0);
370 fullResidualCovariance.ResizeTo(measurementDimensions, measurementDimensions);
371 inverseFullMeasurementCovariance.ResizeTo(measurementDimensions, measurementDimensions);
374 std::vector<TMatrixD> HMatrices;
375 HMatrices.reserve(nPoints);
376 for (
unsigned int i = 0, index = 0; i < nPoints; ++i) {
381 int nDimMeas = vDimMeas[i];
382 TMatrixDSym cov = fullCovariance.GetSub(i * nDim, (i + 1) * nDim - 1,
383 i * nDim, (i + 1) * nDim - 1);
385 setSubOnDiagonal(fullResidualCovariance, index, cov);
387 int indexOffDiag = index;
388 for (
int j = i - 1; j >= 0; --j) {
389 int nDimMeasJ = HMatrices[j].GetNrows();
390 indexOffDiag -= nDimMeasJ;
392 TMatrixD offDiag(HMatrices[j], TMatrixD::kMult,
393 TMatrixD(fullCovariance.GetSub(nDim * j, nDim * (j + 1) - 1,
394 nDim * i, nDim * (i + 1) - 1),
395 TMatrixD::kMultTranspose, H));
397 setSubOffDiagonal(fullResidualCovariance, indexOffDiag, index, offDiag);
401 HMatrices.push_back(H);
406 fullResidualCovariance *= -1;
407 inverseFullMeasurementCovariance = 0;
408 for (
unsigned int i = 0, index = 0; i < nPoints; ++i) {
412 TMatrixDSym cov = mop.getCov();
413 const int dim = cov.GetNrows();
414 setSubOnDiagonal(fullResidualCovariance, index,
415 cov + fullResidualCovariance.GetSub(index, index + dim - 1,
416 index, index + dim - 1));
418 genfit::tools::invertMatrix(cov);
419 setSubOnDiagonal(inverseFullMeasurementCovariance, index, cov);
429 const std::vector<int>& vDimMeas,
431 TVectorD& residualTimeDerivative)
435 int measurementDimensions = std::accumulate(vDimMeas.begin(), vDimMeas.end(), 0);
436 residuals.ResizeTo(measurementDimensions);
437 residualTimeDerivative.ResizeTo(measurementDimensions);
440 const unsigned int nPoints = hitPoints.size();
441 for (
unsigned int i = 0, index = 0; i < nPoints; ++i) {
445 const std::vector<double>& weights = fi->getWeights();
446 TVectorD weightedResidual(vDimMeas[i]);
447 for (
size_t iMeas = 0; iMeas < fi->getNumMeasurements(); ++iMeas) {
448 weightedResidual += weights[iMeas] * fi->getResidual(iMeas).getState();
450 residuals.SetSub(index, weightedResidual);
451 if (
dynamic_cast<const CDCRecoHit*
>(tp->getRawMeasurement(0))) {
453 std::vector<double> deriv = hit->timeDerivativesMeasurementsOnPlane(fi->getFittedState());
455 double weightedDeriv = weights[0] * deriv[0] + weights[1] * deriv[1];
456 residualTimeDerivative[index] = weightedDeriv;
458 index += vDimMeas[i];