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