ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Track.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file Track.cc
1 
7 //PHGenFit
8 #include "Track.h"
9 #include "Measurement.h"
10 
11 #include <trackbase/TrkrDefs.h>
12 
13 //GenFit
14 #include <GenFit/AbsFitterInfo.h>
15 #include <GenFit/AbsHMatrix.h>
16 #include <GenFit/AbsMeasurement.h>
17 #include <GenFit/AbsTrackRep.h>
18 #include <GenFit/DetPlane.h>
19 #include <GenFit/Exception.h>
20 #include <GenFit/FitStatus.h>
21 #include <GenFit/KalmanFittedStateOnPlane.h>
22 #include <GenFit/KalmanFitterInfo.h>
23 #include <GenFit/MeasuredStateOnPlane.h>
24 #include <GenFit/MeasurementOnPlane.h>
25 #include <GenFit/SharedPlanePtr.h>
26 #include <GenFit/Tools.h>
27 #include <GenFit/Track.h>
28 #include <GenFit/TrackPoint.h>
29 
30 #include <TMatrixDfwd.h> // for TMatrixD
31 #include <TMatrixDSymfwd.h> // for TMatrixDSym
32 #include <TMatrixT.h> // for TMatrixT
33 #include <TMatrixTSym.h> // for TMatrixTSym
34 #include <TVectorDfwd.h> // for TVectorD
35 #include <TVector3.h> // for TVector3
36 #include <TVectorT.h> // for TVectorT, operator-
37 
38 //STL
39 #include <cassert>
40 #include <cstddef>
41 #include <limits>
42 #include <iostream>
43 #include <utility>
44 
45 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
46 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
47 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
48 
49 #define WILD_DOUBLE -999999
50 
51 //#define _DEBUG_
52 //#define _PRINT_MATRIX_
53 
54 #ifdef _DEBUG_
55 #include <fstream>
56 #include <iostream>
57 ofstream fout_matrix("matrix.txt");
58 #endif
59 
60 namespace PHGenFit
61 {
62 Track::Track(genfit::AbsTrackRep* rep, TVector3 seed_pos, TVector3 seed_mom, TMatrixDSym seed_cov, const int v)
63 {
64  //TODO Add input param check
65 
66  verbosity = v;
67 
68  genfit::MeasuredStateOnPlane seedMSoP(rep);
69  seedMSoP.setPosMomCov(seed_pos, seed_mom, seed_cov);
70  //const genfit::StateOnPlane seedSoP(seedMSoP);
71 
72  TVectorD seedState(6);
73  TMatrixDSym seedCov(6);
74  seedMSoP.get6DStateCov(seedState, seedCov);
75 
76  _track = new genfit::Track(rep, seedState, seedCov);
77  //_track = NEW(genfit::Track)(rep, seedState, seedCov);
78 }
79 
80 Track::Track(const PHGenFit::Track& t)
81 {
82  _track = new genfit::Track(*(t.getGenFitTrack()));
83  verbosity = t.verbosity;
84  _clusterIDs = t.get_cluster_IDs();
85  _clusterkeys = t.get_cluster_keys();
86 }
87 
88 int Track::addMeasurement(PHGenFit::Measurement* measurement)
89 {
90  std::vector<genfit::AbsMeasurement*> msmts;
91  msmts.push_back(measurement->getMeasurement());
92  _track->insertPoint(new genfit::TrackPoint(msmts, _track));
93 
94  _clusterIDs.push_back(measurement->get_cluster_ID());
95  _clusterkeys.push_back(measurement->get_cluster_key());
96 
97  delete measurement;
98 
99  return 0;
100 }
101 
102 int Track::addMeasurements(std::vector<PHGenFit::Measurement*>& measurements)
103 {
104  for (PHGenFit::Measurement* measurement : measurements)
105  {
106  std::vector<genfit::AbsMeasurement*> msmts;
107  msmts.push_back(measurement->getMeasurement());
108  _track->insertPoint(
109  new genfit::TrackPoint(msmts, _track));
110 
111  //_measurements.push_back(measurement);
112  _clusterIDs.push_back(measurement->get_cluster_ID());
113  _clusterkeys.push_back(measurement->get_cluster_key());
114 
115  delete measurement;
116  }
117 
118  //measurements.clear();
119 
120  return 0;
121 }
122 
123 int Track::deleteLastMeasurement()
124 {
125  _track->deletePoint(-1);
126 
127  _clusterIDs.pop_back();
128  _clusterkeys.pop_back();
129 
130  return 0;
131 }
132 
133 Track::~Track()
134 {
135  // std::cout << "DTOR: " << __LINE__ <<std::endl;
136  delete _track;
137 
138  // for(PHGenFit::Measurement* measurement : _measurements)
139  // {
140  // delete measurement;
141  // }
142  // _measurements.clear();
143 
144  _clusterIDs.clear();
145  _clusterkeys.clear();
146 }
147 
148 double Track::extrapolateToPlane(genfit::MeasuredStateOnPlane& state, TVector3 O, TVector3 n, const int tr_point_id) const
149 {
150  double pathlenth = WILD_DOUBLE;
151 
152  genfit::SharedPlanePtr destPlane(new genfit::DetPlane(O, n));
153 
154  genfit::AbsTrackRep* rep = _track->getCardinalRep();
155  genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
156  tr_point_id, rep);
157  if (tp == NULL)
158  {
159  std::cout << "Track has no TrackPoint with fitterInfo! \n";
160  return WILD_DOUBLE;
161  }
162  std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
163  *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
164  // extrapolate back to reference plane.
165  try
166  {
167  pathlenth = rep->extrapolateToPlane(*kfsop, destPlane);
168  }
169  catch (genfit::Exception& e)
170  {
171  std::cerr << "Exception, next track" << std::endl;
172  std::cerr << e.what();
173  //delete kfsop;
174  return WILD_DOUBLE;
175  }
176 
177  state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
178 
179  //delete kfsop;
180 
181  return pathlenth;
182 }
183 
184 genfit::MeasuredStateOnPlane* Track::extrapolateToPlane(TVector3 O, TVector3 n, const int tr_point_id) const
185 {
186  genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
187  double pathlenth = this->extrapolateToPlane(*state, O, n, tr_point_id);
188  if (pathlenth <= WILD_DOUBLE)
189  {
190  delete state;
191  return NULL;
192  }
193  else
194  return state;
195 }
196 
197 double Track::extrapolateToLine(genfit::MeasuredStateOnPlane& state, TVector3 line_point, TVector3 line_direction, const int tr_point_id) const
198 {
199  double pathlenth = WILD_DOUBLE;
200 
201  genfit::AbsTrackRep* rep = _track->getCardinalRep();
202  genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
203  tr_point_id, rep);
204  if (tp == NULL)
205  {
206  std::cout << "Track has no TrackPoint with fitterInfo! \n";
207  return WILD_DOUBLE;
208  }
209  std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
210  *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
211  // extrapolate back to reference plane.
212  try
213  {
214  pathlenth = rep->extrapolateToLine(*kfsop, line_point, line_direction);
215  }
216  catch (genfit::Exception& e)
217  {
218  std::cerr << "Exception, next track" << std::endl;
219  std::cerr << e.what();
220  //delete kfsop;
221  return WILD_DOUBLE;
222  }
223 
224  state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
225 
226  //delete kfsop;
227 
228  return pathlenth;
229 }
230 
231 genfit::MeasuredStateOnPlane* Track::extrapolateToLine(TVector3 line_point, TVector3 line_direction, const int tr_point_id) const
232 {
233  genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
234  double pathlenth = this->extrapolateToLine(*state, line_point, line_direction, tr_point_id);
235  if (pathlenth <= WILD_DOUBLE)
236  {
237  delete state;
238  return NULL;
239  }
240  else
241  return state;
242 }
243 
244 double Track::extrapolateToCylinder(genfit::MeasuredStateOnPlane& state, double radius, TVector3 line_point, TVector3 line_direction, const int tr_point_id, const int direction) const
245 {
246 #ifdef _DEBUG_
247  std::cout << __LINE__ << std::endl;
248  std::cout
249  << __LINE__
250  << ": tr_point_id: " << tr_point_id
251  << ": direction: " << direction
252  << std::endl;
253 #endif
254  assert(direction == 1 or direction == -1);
255 
256  double pathlenth = WILD_DOUBLE;
257 
258  genfit::AbsTrackRep* rep = _track->getCardinalRep();
259 
260  // genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
261  // tr_point_id, rep);
262  // if (tp == NULL) {
263  // std::cout << "Track has no TrackPoint with fitterInfo! \n";
264  // return WILD_DOUBLE;
265  // }
266 
267  bool have_tp_with_fit_info = false;
268  std::unique_ptr<genfit::MeasuredStateOnPlane> kfsop = NULL;
269  if (_track->getNumPointsWithMeasurement() > 0)
270  {
271 #ifdef _DEBUG_
272 // std::cout<<__LINE__ <<std::endl;
273 #endif
274  genfit::TrackPoint* tp = _track->getPointWithMeasurement(tr_point_id);
275  if (tp == NULL)
276  {
277  LogError("tp == NULL!");
278  return WILD_DOUBLE;
279  }
280  if (dynamic_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep)))
281  {
282  if (direction == 1)
283  {
284  if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
285  rep))
286  ->getForwardUpdate())
287  {
288  have_tp_with_fit_info = true;
289  kfsop =
290  std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::KalmanFittedStateOnPlane(
291  *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
292  rep))
293  ->getForwardUpdate())));
294  }
295  }
296  else
297  {
298  if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
299  rep))
300  ->getBackwardUpdate())
301  {
302  have_tp_with_fit_info = true;
303  kfsop =
304  std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::KalmanFittedStateOnPlane(
305  *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
306  rep))
307  ->getBackwardUpdate())));
308  }
309  }
310  }
311  }
312 
313  if (!have_tp_with_fit_info)
314  {
315 #ifdef _DEBUG_
316  std::cout << __LINE__ << ": !have_tp_with_fit_info" << std::endl;
317 #endif
318  kfsop = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
319  rep->setPosMomCov(*kfsop, _track->getStateSeed(), _track->getCovSeed());
320  }
321 
322  if (!kfsop) return pathlenth;
323  // extrapolate back to reference plane.
324  try
325  {
326  //rep->extrapolateToLine(*kfsop, line_point, line_direction);
327  pathlenth = rep->extrapolateToCylinder(*kfsop, radius, line_point, line_direction);
328  }
329  catch (genfit::Exception& e)
330  {
331  if (verbosity > 1)
332  {
333  LogWarning("Can't extrapolate to cylinder!");
334  std::cerr << e.what();
335  }
336  return WILD_DOUBLE;
337  }
338 
339  state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
340 
341  //delete kfsop;
342 
343  return pathlenth;
344 }
345 
346 genfit::MeasuredStateOnPlane* Track::extrapolateToCylinder(double radius, TVector3 line_point, TVector3 line_direction, const int tr_point_id, const int direction) const
347 {
348  assert(direction == 1 or direction == -1);
349  genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
350  double pathlenth = this->extrapolateToCylinder(*state, radius, line_point, line_direction, tr_point_id, direction);
351  if (pathlenth <= WILD_DOUBLE)
352  {
353  delete state;
354  return NULL;
355  }
356  else
357  return state;
358 }
359 
360 int Track::updateOneMeasurementKalman(
361  const std::vector<PHGenFit::Measurement*>& measurements,
362  std::map<double, std::shared_ptr<PHGenFit::Track> >& incr_chi2s_new_tracks,
363  const int base_tp_idx,
364  const int direction,
365  const float blowup_factor,
366  const bool use_fitted_state) const
367 {
368 #ifdef _DEBUG_
369  std::cout
370  << __LINE__
371  << " : base_tp_idx: " << base_tp_idx
372  << " : direction: " << direction
373  << " : blowup_factor: " << blowup_factor
374  << " : use_fitted_state: " << use_fitted_state
375  << std::endl;
376 #endif
377 
378  if (measurements.size() == 0) return -1;
379 
380  for (PHGenFit::Measurement* measurement : measurements)
381  {
382  std::shared_ptr<PHGenFit::Track> new_track = NULL;
383 
384  new_track = std::shared_ptr<PHGenFit::Track>(new PHGenFit::Track(*this));
385 
386  // if(incr_chi2s_new_tracks.size() == 0)
387  // new_track = const_cast<PHGenFit::Track*>(this);
388  // else
389  // new_track = new PHGenFit::Track(*this);
390 
391  genfit::Track* track = new_track->getGenFitTrack();
392  genfit::AbsTrackRep* rep = track->getCardinalRep();
393 
394  bool newFi(true);
395  genfit::TrackPoint* tp_base = NULL;
396  std::unique_ptr<genfit::MeasuredStateOnPlane> currentState = NULL;
397  genfit::SharedPlanePtr plane = NULL;
398 
399  if (track->getNumPointsWithMeasurement() > 0)
400  {
401  tp_base = track->getPointWithMeasurement(base_tp_idx);
402  newFi = !(tp_base->hasFitterInfo(rep));
403  //tp_base->Print();
404  }
405 #ifdef _DEBUG_
406  std::cout << __LINE__ << ": "
407  << "newFi: " << newFi << std::endl;
408 #endif
409  if (newFi)
410  {
411  currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
412  rep->setPosMomCov(*currentState, track->getStateSeed(),
413  track->getCovSeed());
414  }
415  else
416  {
417  try
418  {
419  genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
420  if (!kfi)
421  {
422 #ifdef _DEBUG_
423  LogDebug("!kfi");
424 #endif
425  continue;
426  }
427  //#ifdef _DEBUG_
428  // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
429  // kfi->Print();
430  // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
431  //#endif
432  if (use_fitted_state)
433  {
434  const genfit::MeasuredStateOnPlane* tempFS = &(kfi->getFittedState(true));
435  if (!tempFS)
436  {
437 #ifdef _DEBUG_
438  LogDebug("!tempFS");
439 #endif
440  continue;
441  }
442  currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*tempFS));
443  }
444  else
445  {
446  genfit::MeasuredStateOnPlane* tempUpdate = kfi->getUpdate(direction);
447  if (!tempUpdate)
448  {
449 #ifdef _DEBUG_
450  LogDebug("!tempUpdate");
451 #endif
452  continue;
453  }
454  currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*tempUpdate));
455  }
456 
457 #ifdef _DEBUG_
458 // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
459 // kfi->Print();
460 // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
461 // tempFS->Print();
462 // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
463 // tempUpdate->Print();
464 // std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
465 #endif
466 
467  if (blowup_factor > 1)
468  {
469  currentState->blowUpCov(blowup_factor, true, 1e6);
470  }
471  }
472  catch (genfit::Exception &e)
473  {
474 #ifdef _DEBUG_
475  std::cout
476  << __LINE__
477  << ": Fitted state not found!"
478  << std::endl;
479  std::cerr << e.what() << std::endl;
480 #endif
481  currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
482  rep->setPosMomCov(*currentState, track->getStateSeed(),
483  track->getCovSeed());
484  }
485  }
486 #ifdef _DEBUG_
487  std::cout << __LINE__ << std::endl;
488 #endif
489  //std::vector<genfit::AbsMeasurement*> msmts;
490  //msmts.push_back(measurement->getMeasurement());
491 
492  //genfit::TrackPoint *tp = new genfit::TrackPoint(msmts, track);
493  //track->insertPoint(tp); // genfit
494 
499  new_track->addMeasurement(measurement);
500 
501 #ifdef _DEBUG_
502  std::cout << __LINE__ << ": clusterIDs size: " << new_track->get_cluster_IDs().size() << std::endl;
503  std::cout << __LINE__ << ": clusterkeyss size: " << new_track->get_cluster_keys().size() << std::endl;
504 #endif
505 
507  genfit::TrackPoint* tp = new_track->getGenFitTrack()->getPoint(-1);
508 #ifdef _DEBUG_
509  std::cout << __LINE__ << std::endl;
510 #endif
511  genfit::KalmanFitterInfo* fi = new genfit::KalmanFitterInfo(tp, rep);
512  tp->setFitterInfo(fi);
513 #ifdef _DEBUG_
514  std::cout
515  << __LINE__
516  << ": track->getPointWithMeasurement(): " << track->getPointWithMeasurement(-1)
517  << std::endl;
518 #endif
519 // if (track->getNumPointsWithMeasurement() > 0) {
520 // tp_base = track->getPointWithMeasurement(-1);
521 // if (tp_base->hasFitterInfo(rep)) {
522 // std::cout << "TP has FI!" << std::endl;
523 // }
524 // }
525 #ifdef _DEBUG_
526  std::cout << __LINE__ << std::endl;
527 #endif
528  const std::vector<genfit::AbsMeasurement*>& rawMeasurements =
529  tp->getRawMeasurements();
530  // construct plane with first measurement
531  plane = rawMeasurements[0]->constructPlane(*currentState);
532 
533  //double extLen = rep->extrapolateToPlane(*state, plane);
534 
535  try
536  {
537  rep->extrapolateToPlane(*currentState, plane);
538  }
539  catch (...)
540  {
541  if (verbosity > 1)
542  {
543  LogWarning("Can not extrapolate to measuremnt with cluster_ID and cluster key: ") << measurement->get_cluster_ID()
544  << " " << measurement->get_cluster_key()
545  << std::endl;
546  }
547  continue;
548  }
549 #ifdef _DEBUG_
550  std::cout << __LINE__ << std::endl;
551 #endif
552  fi->setPrediction(currentState->clone(), direction);
553  genfit::MeasuredStateOnPlane* state = fi->getPrediction(direction);
554 #ifdef _DEBUG_
555  std::cout << __LINE__ << std::endl;
556 #endif
557  TVectorD stateVector(state->getState());
558  TMatrixDSym cov(state->getCov());
559 #ifdef _DEBUG_
560  {
561  std::cout << __LINE__ << std::endl;
562  // TMatrixDSym cov6d = state->get6DCov();
563  // float err_rphi = sqrt(
564  // cov6d[0][0] + cov6d[1][1] + cov6d[0][1] + cov6d[1][0]);
565  // float err_z = sqrt(cov6d[2][2]);
566  // std::cout << err_phi << "\t" << err_z << "\t";
567  }
568 #endif
569  for (std::vector<genfit::AbsMeasurement*>::const_iterator it =
570  rawMeasurements.begin();
571  it != rawMeasurements.end(); ++it)
572  {
573  fi->addMeasurementsOnPlane(
574  (*it)->constructMeasurementsOnPlane(*state));
575  }
576 
577  double chi2inc = 0;
578  double ndfInc = 0;
579 #ifdef _DEBUG_
580  std::cout << __LINE__ << std::endl;
581 #endif
582  // update(s)
583  const std::vector<genfit::MeasurementOnPlane*>& measurements_on_plane = fi->getMeasurementsOnPlane();
584 #ifdef _DEBUG_
585  std::cout
586  << __LINE__
587  << ": size of fi's MeasurementsOnPlane: " << measurements_on_plane.size()
588  << std::endl;
589 #endif
590  for (std::vector<genfit::MeasurementOnPlane*>::const_iterator it =
591  measurements_on_plane.begin();
592  it != measurements_on_plane.end(); ++it)
593  {
594  const genfit::MeasurementOnPlane& mOnPlane = **it;
595  //const double weight = mOnPlane.getWeight();
596 
597  const TVectorD& measurement(mOnPlane.getState());
598  const genfit::AbsHMatrix* H(mOnPlane.getHMatrix());
599  // (weighted) cov
600  const TMatrixDSym& V(mOnPlane.getCov()); //Covariance of measurement noise v_{k}
601 
602  TVectorD res(measurement - H->Hv(stateVector));
603 #ifdef _DEBUG_
604  {
605  std::cout << __LINE__ << std::endl;
606  // std::cout
607  // <<res(0) <<"\t"
608  // <<res(1) <<"\t";
609  }
610 #endif
611  // If hit, do Kalman algebra.
612  {
613  // calculate kalman gain ------------------------------
614  // calculate covsum (V + HCH^T) and invert
615  TMatrixDSym covSumInv(cov);
616  H->HMHt(covSumInv);
617  covSumInv += V;
618  try
619  {
620  genfit::tools::invertMatrix(covSumInv);
621  }
622  catch (genfit::Exception &e)
623  {
624 #ifdef _DEBUG_
625  LogDebug("cannot invert matrix.");
626 #endif
627  continue;
628  }
629 
630  TMatrixD CHt(H->MHt(cov));
631 #ifdef _PRINT_MATRIX_
632  std::cout << __LINE__ << ": V_{k}:" << std::endl;
633  V.Print();
634  std::cout << __LINE__ << ": R_{k}^{-1}:" << std::endl;
635  covSumInv.Print();
636  std::cout << __LINE__ << ": C_{k|k-1}:" << std::endl;
637  cov.Print();
638  std::cout << __LINE__ << ": C_{k|k-1} H_{k}^{T} :" << std::endl;
639  CHt.Print();
640  std::cout << __LINE__ << ": K_{k} :" << std::endl;
641  TMatrixD Kk(CHt, TMatrixD::kMult, covSumInv);
642  Kk.Print();
643  std::cout << __LINE__ << ": res:" << std::endl;
644  res.Print();
645 #endif
646  TVectorD update(
647  TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
648  //TMatrixD(CHt, TMatrixD::kMult, covSumInv).Print();
649 
650  stateVector += update; // x_{k|k} = x_{k|k-1} + K_{k} r_{k|k-1}
651  covSumInv.Similarity(CHt); // with (C H^T)^T = H C^T = H C (C is symmetric)
652  cov -= covSumInv; //C_{k|k}
653 #ifdef _DEBUG_
654  {
655  std::cout << __LINE__ << std::endl;
656  // TMatrixDSym cov6d = state->get6DCov();
657  // float err_rphi = sqrt(cov6d[0][0] + cov6d[1][1] + cov6d[0][1] + cov6d[1][0]);
658  // float err_z = sqrt(cov6d[2][2]);
659  // std::cout
660  // <<err_phi <<"\t"
661  // <<err_z <<"\t";
662  }
663 #endif
664  }
665 
666  TVectorD resNew(measurement - H->Hv(stateVector));
667 
668  // Calculate chi2
669  TMatrixDSym HCHt(cov); //C_{k|k}
670  H->HMHt(HCHt);
671  HCHt -= V;
672  HCHt *= -1;
673 
674  try
675  {
676  genfit::tools::invertMatrix(HCHt);
677  }
678  catch (genfit::Exception &e)
679  {
680 #ifdef _DEBUG_
681  LogDebug("cannot invert matrix.");
682 #endif
683  continue;
684  }
685  chi2inc += HCHt.Similarity(resNew);
686 
687  ndfInc += measurement.GetNrows();
688 
689 #ifdef _PRINT_MATRIX_
690  std::cout << __LINE__ << ": V - HCHt:" << std::endl;
691  HCHt.Print();
692  std::cout << __LINE__ << ": resNew:" << std::endl;
693  resNew.Print();
694 #endif
695 
696 #ifdef _DEBUG_
697  std::cout << __LINE__ << ": ndfInc: " << ndfInc << std::endl;
698  std::cout << __LINE__ << ": chi2inc: " << chi2inc << std::endl;
699 #endif
700 
701  currentState->setStateCovPlane(stateVector, cov, plane);
702  currentState->setAuxInfo(state->getAuxInfo());
703 
704  genfit::KalmanFittedStateOnPlane* updatedSOP =
705  new genfit::KalmanFittedStateOnPlane(*currentState, chi2inc,
706  ndfInc);
707  fi->setUpdate(updatedSOP, direction);
708  } //loop measurements_on_plane
709 
710  //FIXME why chi2 could be smaller than 0?
711  if (chi2inc > 0)
712  incr_chi2s_new_tracks.insert(std::make_pair(chi2inc, new_track));
713 
714  } //loop measurments
715 
716  return 0;
717 }
718 
719 double Track::extrapolateToPoint(genfit::MeasuredStateOnPlane& state, TVector3 P, const int tr_point_id) const
720 {
721  double pathlenth = WILD_DOUBLE;
722  genfit::AbsTrackRep* rep = _track->getCardinalRep();
723  genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
724  tr_point_id, rep);
725  if (tp == NULL)
726  {
727  std::cout << "Track has no TrackPoint with fitterInfo! \n";
728  return WILD_DOUBLE;
729  }
730  std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
731  *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
732  // extrapolate back to reference plane.
733  try
734  {
735  pathlenth = rep->extrapolateToPoint(*kfsop, P);
736  }
737  catch (genfit::Exception& e)
738  {
739  std::cerr << "Exception, next track" << std::endl;
740  std::cerr << e.what();
741  //delete kfsop;
742  return WILD_DOUBLE;
743  }
744 
745  state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
746 
747  //delete kfsop;
748 
749  return pathlenth;
750 }
751 
752 genfit::MeasuredStateOnPlane* Track::extrapolateToPoint(TVector3 P, const int tr_point_id) const
753 {
754  genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
755  double pathlenth = this->extrapolateToPoint(*state, P, tr_point_id);
756  if (pathlenth <= WILD_DOUBLE)
757  {
758  delete state;
759  return NULL;
760  }
761  else
762  return state;
763 }
764 
765 double Track::get_chi2() const
766 {
767  double chi2 = std::numeric_limits<double>::quiet_NaN();
768 
769  genfit::AbsTrackRep* rep = _track->getCardinalRep();
770  if (rep)
771  {
772  genfit::FitStatus* fs = _track->getFitStatus(rep);
773  if (fs)
774  chi2 = fs->getChi2();
775  }
776  return chi2;
777 }
778 
779 double Track::get_ndf() const
780 {
781  double ndf = std::numeric_limits<double>::quiet_NaN();
782 
783  genfit::AbsTrackRep* rep = _track->getCardinalRep();
784  if (rep)
785  {
786  genfit::FitStatus* fs = _track->getFitStatus(rep);
787  if (fs)
788  ndf = fs->getNdf();
789  }
790  return ndf;
791 }
792 
793 double Track::get_charge() const
794 {
795  double charge = std::numeric_limits<double>::quiet_NaN();
796 
797  if (!_track) return charge;
798 
799  try
800  {
801  genfit::TrackPoint* tp_base = nullptr;
802 
803  if (_track->getNumPointsWithMeasurement() > 0)
804  {
805  tp_base = _track->getPointWithMeasurement(0);
806  }
807 
808  if (!tp_base) return charge;
809 
810  genfit::AbsTrackRep* rep = _track->getCardinalRep();
811  if (rep)
812  {
813  genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
814 
815  if (!kfi) return charge;
816 
817  const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(true));
818 
819  //std::unique_ptr<genfit::StateOnPlane> state (this->extrapolateToLine(TVector3(0, 0, 0), TVector3(1, 0, 0)));
820 
821  if (state)
822  charge = rep->getCharge(*state);
823  }
824  }
825  catch (...)
826  {
827  if (verbosity >= 1)
828  std::cerr << "Track::get_charge - Error - obtaining charge failed. Returning NAN as charge." << std::endl;
829  }
830 
831  return charge;
832 }
833 
834 TVector3 Track::get_mom() const
835 {
836  TVector3 mom(0, 0, 0);
837 
838  if (!_track) return mom;
839 
840  genfit::TrackPoint* tp_base = nullptr;
841 
842  if (_track->getNumPointsWithMeasurement() > 0)
843  {
844  tp_base = _track->getPointWithMeasurement(0);
845  }
846 
847  if (!tp_base) return mom;
848 
849  genfit::AbsTrackRep* rep = _track->getCardinalRep();
850  if (rep)
851  {
852  genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
853 
854  if (!kfi) return mom;
855 
856  const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(true));
857 
858  if (state)
859  return state->getMom();
860  }
861 
862  return mom;
863 }
864 
865 } // namespace PHGenFit