ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PHGenFitTrkFitter.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file PHGenFitTrkFitter.cc
1 
8 #include "PHGenFitTrkFitter.h"
9 
10 #include <trackbase/TrkrCluster.h> // for TrkrCluster
12 #include <trackbase/TrkrDefs.h>
13 
21 #include <trackbase_historic/SvtxTrackState.h> // for SvtxTrackState
22 #include <trackbase_historic/SvtxVertex.h> // for SvtxVertex
23 #include <trackbase_historic/SvtxVertexMap.h> // for SvtxVertexMap
24 
25 #include <intt/InttDefs.h>
26 #include <intt/CylinderGeomIntt.h>
27 
30 
31 #include <mvtx/MvtxDefs.h>
32 #include <mvtx/CylinderGeom_Mvtx.h>
33 
34 #include <g4detectors/PHG4CylinderGeom.h> // for PHG4CylinderGeom
36 
37 #include <g4main/PHG4Particle.h>
38 #include <g4main/PHG4Particlev2.h>
40 #include <g4main/PHG4VtxPoint.h> // for PHG4VtxPoint
41 #include <g4main/PHG4VtxPointv1.h>
42 
43 #include <phgenfit/Fitter.h>
44 #include <phgenfit/Measurement.h> // for Measurement
45 #include <phgenfit/PlanarMeasurement.h>
46 #include <phgenfit/SpacepointMeasurement.h>
47 #include <phgenfit/Track.h>
48 
50 #include <fun4all/PHTFileServer.h>
51 #include <fun4all/SubsysReco.h> // for SubsysReco
52 
53 #include <phool/PHCompositeNode.h>
54 #include <phool/PHIODataNode.h>
55 #include <phool/PHNode.h> // for PHNode
56 #include <phool/PHNodeIterator.h>
57 #include <phool/PHObject.h> // for PHObject
58 #include <phool/getClass.h>
59 #include <phool/phool.h>
60 
61 #include <phfield/PHFieldUtility.h>
62 #include <phgeom/PHGeomUtility.h>
63 
64 #include <GenFit/AbsMeasurement.h> // for AbsMeasurement
65 #include <GenFit/EventDisplay.h> // for EventDisplay
66 #include <GenFit/Exception.h> // for Exception
67 #include <GenFit/GFRaveConverters.h>
68 #include <GenFit/GFRaveTrackParameters.h> // for GFRaveTrackParame...
69 #include <GenFit/GFRaveVertex.h>
70 #include <GenFit/GFRaveVertexFactory.h>
71 #include <GenFit/KalmanFitterInfo.h>
72 #include <GenFit/MeasuredStateOnPlane.h>
73 #include <GenFit/RKTrackRep.h>
74 #include <GenFit/Track.h>
75 #include <GenFit/TrackPoint.h> // for TrackPoint
76 
77 //Rave
78 #include <rave/ConstantMagneticField.h>
79 #include <rave/VacuumPropagator.h> // for VacuumPropagator
80 #include <rave/VertexFactory.h>
81 
82 #include <TClonesArray.h>
83 #include <TRotation.h>
84 #include <TTree.h>
85 #include <TVector3.h>
86 #include <TMath.h> // for ATan2
87 #include <TMatrixDSymfwd.h> // for TMatrixDSym
88 #include <TMatrixFfwd.h> // for TMatrixF
89 #include <TMatrixT.h> // for TMatrixT, operator*
90 #include <TMatrixTSym.h> // for TMatrixTSym
91 #include <TMatrixTUtils.h> // for TMatrixTRow
92 #include <TVectorDfwd.h> // for TVectorD
93 #include <TVectorT.h> // for TVectorT
94 
95 #include <cmath> // for sqrt, NAN
96 #include <iostream>
97 #include <map>
98 #include <memory>
99 #include <utility>
100 #include <vector>
101 
102 class PHField;
103 class TGeoManager;
104 namespace genfit { class AbsTrackRep; }
105 
106 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
107 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
108 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
109 
110 static constexpr float WILD_FLOAT = -9999;
111 
112 #define _DEBUG_MODE_ 0
113 
114 //#define _DEBUG_
115 
116 using namespace std;
117 
118 //______________________________________________________
119 namespace {
120 
121  // square
122  template< class T > T square( T x ) { return x*x; }
123 
124  // convert gf state to SvtxTrackState_v1
125  SvtxTrackState_v1 create_track_state( float pathlength, const genfit::MeasuredStateOnPlane* gf_state )
126  {
127 
128  SvtxTrackState_v1 out( pathlength );
129  out.set_x(gf_state->getPos().x());
130  out.set_y(gf_state->getPos().y());
131  out.set_z(gf_state->getPos().z());
132 
133  out.set_px(gf_state->getMom().x());
134  out.set_py(gf_state->getMom().y());
135  out.set_pz(gf_state->getMom().z());
136 
137  for (int i = 0; i < 6; i++)
138  {
139  for (int j = i; j < 6; j++)
140  { out.set_error(i, j, gf_state->get6DCov()[i][j]); }
141  }
142 
143  return out;
144 
145  }
146 
147 }
148 
149 /*
150  * Constructor
151  */
153  : SubsysReco(name)
154 {
155  Verbosity(0);
162 }
163 
164 /*
165  * Init
166  */
168 {
169  // CreateNodes(topNode);
170 
172 }
173 
174 /*
175  * Init run
176  */
178 {
179  CreateNodes(topNode);
180 
181  TGeoManager* tgeo_manager = PHGeomUtility::GetTGeoManager(topNode);
182  PHField* field = PHFieldUtility::GetFieldMapNode(nullptr, topNode);
183 
184  //_fitter = new PHGenFit::Fitter("sPHENIX_Geo.root","sPHENIX.2d.root", 1.4 / 1.5);
185  _fitter.reset( PHGenFit::Fitter::getInstance(tgeo_manager,
187  "RKTrackRep", _do_evt_display) );
188  _fitter->set_verbosity(Verbosity());
189 
190  if (!_fitter)
191  {
192  cerr << PHWHERE << endl;
194  }
195 
196  //LogDebug(genfit::FieldManager::getInstance()->getFieldVal(TVector3(0, 0, 0)).Z());
197 
198  _vertex_finder.reset( new genfit::GFRaveVertexFactory(Verbosity()) );
199  _vertex_finder->setMethod(_vertexing_method.data());
200 
201 
202  if (!_vertex_finder)
203  {
204  cerr << PHWHERE << endl;
206  }
207 
208  if (_do_eval)
209  {
210  if (Verbosity() > 1)
211  cout << PHWHERE << " Openning file: " << _eval_outname << endl;
212  PHTFileServer::get().open(_eval_outname, "RECREATE");
213  init_eval_tree();
214  }
215 
216  // print disabled layers
217  for( const auto& layer:_disabled_layers )
218  { std::cout << PHWHERE << " Layer " << layer << " is disabled." << std::endl; }
219 
221 }
222 /*
223  * process_event():
224  * Call user instructions for every event.
225  * This function contains the analysis structure.
226  *
227  */
229 {
230  _event++;
231 
232  if (Verbosity() > 1)
233  std::cout << PHWHERE << "Events processed: " << _event << std::endl;
234  // if (_event % 1000 == 0)
235  // cout << PHWHERE << "Events processed: " << _event << endl;
236 
237  //cout << "Start PHGenFitTrkfitter::process_event" << endl;
238 
239  GetNodes(topNode);
240 
242  vector<genfit::Track*> rf_gf_tracks;
243  vector<std::shared_ptr<PHGenFit::Track> > rf_phgf_tracks;
244 
245  map<unsigned int, unsigned int> svtxtrack_genfittrack_map;
246 
247  if (_trackmap_refit)
249 
250  // _trackmap is SvtxTrackMap from the node tree
251  for (SvtxTrackMap::Iter iter = _trackmap->begin(); iter != _trackmap->end();
252  ++iter)
253  {
254  SvtxTrack* svtx_track = iter->second;
255  if(Verbosity() > 10){
256  cout << " process SVTXTrack " << iter->first << endl;
257  svtx_track->identify();
258  }
259  if (!svtx_track)
260  continue;
261  if (!(svtx_track->get_pt() > _fit_min_pT))
262  continue;
263 
264  // This is the final track (re)fit. It does not include the collision vertex. If fit_primary_track is set, a refit including the vertex is done below.
266  std::shared_ptr<PHGenFit::Track> rf_phgf_track = ReFitTrack(topNode, svtx_track);
267  if (rf_phgf_track)
268  {
269  svtxtrack_genfittrack_map[svtx_track->get_id()] = rf_phgf_tracks.size();
270  rf_phgf_tracks.push_back(rf_phgf_track);
271  if (rf_phgf_track->get_ndf() > _vertex_min_ndf)
272  rf_gf_tracks.push_back(rf_phgf_track->getGenFitTrack());
273  if(Verbosity() > 10) cout << "Done refitting input track" << svtx_track->get_id() << " or rf_phgf_track " << rf_phgf_tracks.size() << endl;
274  }
275  else{
276  if(Verbosity() >= 1)
277  cout << "failed refitting input track# " << iter->first << endl;
278  }
279 
280  }
281 
282  /*
283  * add tracks to event display
284  * needs to make copied for smart ptrs will be destroied even
285  * there are still references in TGeo::EventView
286  */
287  if (_do_evt_display)
288  {
289  vector<genfit::Track*> copy;
290  for (genfit::Track* t : rf_gf_tracks)
291  {
292  copy.push_back(new genfit::Track(*t));
293  }
294  _fitter->getEventDisplay()->addEvent(copy);
295  }
296 
298  std::vector<genfit::GFRaveVertex*> rave_vertices;
299 
300  if (rf_gf_tracks.size() >= 2)
301  {
302  if(Verbosity() > 10)
303  {cout << "Call Rave vertex finder" << endl;
304  cout << " rf_gf_tracks size " << rf_gf_tracks.size() << endl;
305  for(long unsigned int i=0;i<rf_gf_tracks.size();++i)
306  {
307  cout << " track " << i << " num points " << rf_gf_tracks[i]->getNumPointsWithMeasurement() << endl;
308  }
309  }
310  try
311  {
312  _vertex_finder->findVertices(&rave_vertices, rf_gf_tracks);
313  }
314  catch (...)
315  {
316  if (Verbosity() > 1)
317  std::cout << PHWHERE << "GFRaveVertexFactory::findVertices failed!";
318  }
319  }
320 
321  if(Verbosity() > 10 && rave_vertices.size() == 0)
322  {
323  cout << PHWHERE << " Rave found no vertices - SvtxVertexMapRefit will be empty" << endl;
324  }
325 
326  // Creates new SvtxVertex objects and copies in the rave vertex info and associated rave tracks. Then adds the vertices to _svtxmap
327  // also makes a map of (trackid/vertex), _rave_vertex_gf_track_map for later use
328  FillSvtxVertexMap(rave_vertices, rf_gf_tracks);
329 
330  // Finds the refitted rf_phgf_track corresponding to each SvtxTrackMap entry
331  // Converts it to an SvtxTrack in MakeSvtxTrack
332  // MakeSvtxTrack takes a vertex that it gets from the map made in FillSvtxVertex
333  // If the refit was succesful, the track on the node tree is replaced with the new one
334  // If not, the track is erased from the node tree
335  for (SvtxTrackMap::Iter iter = _trackmap->begin(); iter != _trackmap->end();)
336  {
337  std::shared_ptr<PHGenFit::Track> rf_phgf_track;
338 
339  // find the genfit track that corresponds to this one on the node tree
340  unsigned int itrack =0;
341  if (svtxtrack_genfittrack_map.find(iter->second->get_id()) != svtxtrack_genfittrack_map.end())
342  {
343  itrack =
344  svtxtrack_genfittrack_map[iter->second->get_id()];
345  rf_phgf_track = rf_phgf_tracks[itrack];
346  }
347 
348  if (rf_phgf_track)
349  {
350  SvtxVertex* vertex = nullptr;
351 
352  unsigned int ivert = 0;
353  ivert = _rave_vertex_gf_track_map[itrack];
354 
355  if (_vertexmap_refit->size() > 0)
356  {
357  vertex = _vertexmap_refit->get(ivert);
358 
359  if(Verbosity() > 20) cout << PHWHERE << " gf track " << itrack << " will add to track: _vertexmap_refit vertex " << ivert
360  << " with position x,y,z = " << vertex->get_x() << " " << vertex->get_y() << " " << vertex->get_z() << endl;
361  }
362  std::shared_ptr<SvtxTrack> rf_track = MakeSvtxTrack(iter->second, rf_phgf_track,
363  vertex);
364 
365 #ifdef _DEBUG_
366  cout << __LINE__ << endl;
367 #endif
368  if (!rf_track)
369  {
370  //if (_output_mode == OverwriteOriginalNode)
371 #ifdef _DEBUG_
372  LogDebug("!rf_track, continue.");
373 #endif
375  {
376  auto key = iter->first;
377  ++iter;
378  _trackmap->erase(key);
379  continue;
380  } else {
381  ++iter;
382  continue;
383  }
384  }
385 
386  // delete vertex;//DEBUG
387 
388  // rf_phgf_tracks.push_back(rf_phgf_track);
389  // rf_gf_tracks.push_back(rf_phgf_track->getGenFitTrack());
390 
392  if (_trackmap_refit)
393  { _trackmap_refit->insert(rf_track.get()); }
394 
396  { iter->second->CopyFrom( rf_track.get() ); }
397  }
398  else
399  {
401  {
402  auto key = iter->first;
403  ++iter;
404  _trackmap->erase(key);
405  continue;
406  }
407  }
408 
409  ++iter;
410  }
411 
412 #ifdef _DEBUG_
413  cout << __LINE__ << endl;
414 #endif
415 
416  // Need to keep tracks if _do_evt_display
417  if (!_do_evt_display)
418  {
419  rf_phgf_tracks.clear();
420  }
421 
422 #ifdef _DEBUG_
423  cout << __LINE__ << endl;
424 #endif
425 
429  if (_fit_primary_tracks && rave_vertices.size() > 0)
430  {
432 
433  //FIXME figure out which vertex to use.
434  SvtxVertex* vertex = nullptr;
435  if (_vertexmap_refit->size() > 0)
436  vertex = _vertexmap_refit->get(0);
437 
438  // fix this, have to get vertex ID from track
439 
440  if (vertex)
441  {
442  for (SvtxTrackMap::ConstIter iter = _trackmap->begin();
443  iter != _trackmap->end(); ++iter)
444  {
445  SvtxTrack* svtx_track = iter->second;
446  if (!svtx_track)
447  continue;
448  if (!(svtx_track->get_pt() > _fit_min_pT))
449  continue;
453  std::shared_ptr<PHGenFit::Track> rf_phgf_track = ReFitTrack(topNode, svtx_track,
454  vertex);
455  if (rf_phgf_track)
456  {
457  // //FIXME figure out which vertex to use.
458  // SvtxVertex* vertex = nullptr;
459  // if (_vertexmap_refit->size() > 0)
460  // vertex = _vertexmap_refit->get(0);
461 
462  std::shared_ptr<SvtxTrack> rf_track = MakeSvtxTrack(svtx_track,
463  rf_phgf_track, vertex);
464  //delete rf_phgf_track;
465  if (!rf_track)
466  {
467 #ifdef _DEBUG_
468  LogDebug("!rf_track, continue.");
469 #endif
470  continue;
471  }
472  _primary_trackmap->insert(rf_track.get());
473  }
474  }
475  }
476  else
477  {
478  LogError("No vertex in SvtxVertexMapRefit!");
479  }
480  }
481 #ifdef _DEBUG_
482  cout << __LINE__ << endl;
483 #endif
484  for (genfit::GFRaveVertex* vertex : rave_vertices)
485  {
486  delete vertex;
487  }
488  rave_vertices.clear();
489 
490  if (_do_eval)
491  {
492  fill_eval_tree(topNode);
493  }
494 #ifdef _DEBUG_
495  cout << __LINE__ << endl;
496 #endif
498 }
499 
500 /*
501  * End
502  */
504 {
505  if (_do_eval)
506  {
507  if (Verbosity() > 1)
508  cout << PHWHERE << " Writing to file: " << _eval_outname << endl;
510  _eval_tree->Write();
511  _cluster_eval_tree->Write();
512  }
513 
514  if (_do_evt_display)
515  _fitter->displayEvent();
516 
518 }
519 
520 /*
521  * fill_eval_tree():
522  */
524 {
527 
528  if( _truth_container )
529  {
530  int i = 0;
531  for (auto itr = _truth_container->GetPrimaryParticleRange().first; itr != _truth_container->GetPrimaryParticleRange().second; ++itr, ++i)
532  { new ((*_tca_particlemap)[i])(PHG4Particlev2)( *dynamic_cast<PHG4Particlev2*>(itr->second)); }
533  }
534 
535 
536  if( _truth_container )
537  {
538  int i = 0;
539  for (auto itr = _truth_container->GetPrimaryVtxRange().first; itr != _truth_container->GetPrimaryVtxRange().second; ++itr, ++i)
540  { new ((*_tca_vtxmap)[i])(PHG4VtxPointv1)(*dynamic_cast<PHG4VtxPointv1*>(itr->second)); }
541  }
542 
543  if( _trackmap )
544  {
545  int i = 0;
546  for ( const auto& pair:*_trackmap )
547  { new ((*_tca_trackmap)[i++])(SvtxTrack_v2)( *pair.second ); }
548  }
549 
550  if (_vertexmap)
551  {
552  int i = 0;
553  for ( const auto& pair:*_vertexmap )
554  { new ((*_tca_vertexmap)[i++])(SvtxVertex_v1)( *dynamic_cast<SvtxVertex_v1*>(pair.second) ); }
555  }
556 
557  if (_trackmap_refit)
558  {
559  int i = 0;
560  for (const auto& pair:*_trackmap_refit )
561  { new ((*_tca_trackmap_refit)[i++])(SvtxTrack_v2)(*pair.second); }
562  }
563 
565  {
566  int i = 0;
567  for ( const auto& pair:*_primary_trackmap )
568  { new ((*_tca_primtrackmap)[i++])(SvtxTrack_v2)(*pair.second); }
569  }
570 
571  if (_vertexmap_refit)
572  {
573  int i = 0;
574  for( const auto& pair:*_vertexmap_refit )
575  { new ((*_tca_vertexmap_refit)[i++])(SvtxVertex_v1)( *dynamic_cast<SvtxVertex_v1*>(pair.second)); }
576  }
577 
578  _eval_tree->Fill();
579 
580  return;
581 }
582 
583 /*
584  * init_eval_tree
585  */
587 {
588  if (!_tca_particlemap)
589  _tca_particlemap = new TClonesArray("PHG4Particlev2");
590  if (!_tca_vtxmap)
591  _tca_vtxmap = new TClonesArray("PHG4VtxPointv1");
592 
593  if (!_tca_trackmap)
594  _tca_trackmap = new TClonesArray("SvtxTrack_v2");
595  if (!_tca_vertexmap)
596  _tca_vertexmap = new TClonesArray("SvtxVertex_v1");
597  if (!_tca_trackmap_refit)
598  _tca_trackmap_refit = new TClonesArray("SvtxTrack_v2");
600  if (!_tca_primtrackmap)
601  _tca_primtrackmap = new TClonesArray("SvtxTrack_v2");
603  _tca_vertexmap_refit = new TClonesArray("SvtxVertex_v1");
604 
606  _eval_tree = new TTree("T", "PHGenFitTrkFitter Evaluation");
607 
608  _eval_tree->Branch("PrimaryParticle", _tca_particlemap);
609  _eval_tree->Branch("TruthVtx", _tca_vtxmap);
610 
611  _eval_tree->Branch("SvtxTrack", _tca_trackmap);
612  _eval_tree->Branch("SvtxVertex", _tca_vertexmap);
613  _eval_tree->Branch("SvtxTrackRefit", _tca_trackmap_refit);
615  _eval_tree->Branch("PrimSvtxTrack", _tca_primtrackmap);
616  _eval_tree->Branch("SvtxVertexRefit", _tca_vertexmap_refit);
617 
618  _cluster_eval_tree = new TTree("cluster_eval", "cluster eval tree");
619  _cluster_eval_tree->Branch("x", &_cluster_eval_tree_x, "x/F");
620  _cluster_eval_tree->Branch("y", &_cluster_eval_tree_y, "y/F");
621  _cluster_eval_tree->Branch("z", &_cluster_eval_tree_z, "z/F");
622  _cluster_eval_tree->Branch("gx", &_cluster_eval_tree_gx, "gx/F");
623  _cluster_eval_tree->Branch("gy", &_cluster_eval_tree_gy, "gy/F");
624  _cluster_eval_tree->Branch("gz", &_cluster_eval_tree_gz, "gz/F");
625 }
626 
627 /*
628  * reset_eval_variables():
629  * Reset all the tree variables to their default values.
630  * Needs to be called at the start of every event
631  */
633 {
634  _tca_particlemap->Clear();
635  _tca_vtxmap->Clear();
636 
637  _tca_trackmap->Clear();
638  _tca_vertexmap->Clear();
639  _tca_trackmap_refit->Clear();
641  _tca_primtrackmap->Clear();
642  _tca_vertexmap_refit->Clear();
643 
650 }
651 
653 {
654  // create nodes...
655  PHNodeIterator iter(topNode);
656 
657  PHCompositeNode* dstNode = static_cast<PHCompositeNode*>(iter.findFirst(
658  "PHCompositeNode", "DST"));
659  if (!dstNode)
660  {
661  cerr << PHWHERE << "DST Node missing, doing nothing." << endl;
663  }
664  PHNodeIterator iter_dst(dstNode);
665 
666  // Create the SVTX node
667  PHCompositeNode* tb_node = dynamic_cast<PHCompositeNode*>(iter_dst.findFirst(
668  "PHCompositeNode", "SVTX"));
669  if (!tb_node)
670  {
671  tb_node = new PHCompositeNode("SVTX");
672  dstNode->addNode(tb_node);
673  if (Verbosity() > 0)
674  cout << "SVTX node added" << endl;
675  }
676 
678  {
681  _trackmap_refit, "SvtxTrackMapRefit", "PHObject");
682  tb_node->addNode(tracks_node);
683  if (Verbosity() > 0)
684  cout << "Svtx/SvtxTrackMapRefit node added" << endl;
685  }
686 
688  {
690  PHIODataNode<PHObject>* primary_tracks_node =
691  new PHIODataNode<PHObject>(_primary_trackmap, "PrimaryTrackMap",
692  "PHObject");
693  tb_node->addNode(primary_tracks_node);
694  if (Verbosity() > 0)
695  cout << "Svtx/PrimaryTrackMap node added" << endl;
696  }
697 
698  // always write final vertex results to SvtxVertexMapRefit
700  PHIODataNode<PHObject>* vertexes_node = new PHIODataNode<PHObject>(
701  _vertexmap_refit, "SvtxVertexMapRefit", "PHObject");
702  tb_node->addNode(vertexes_node);
703  if (Verbosity() > 0)
704  cout << "Svtx/SvtxVertexMapRefit node added" << endl;
705 
707 }
708 
709 //______________________________________________________
710 void PHGenFitTrkFitter::disable_layer( int layer, bool disabled )
711 {
712  if( disabled ) _disabled_layers.insert( layer );
713  else _disabled_layers.erase( layer );
714 }
715 
716 //______________________________________________________
717 void PHGenFitTrkFitter::set_disabled_layers( const std::set<int>& layers )
718 { _disabled_layers = layers; }
719 
720 //______________________________________________________
722 { _disabled_layers.clear(); }
723 
724 //______________________________________________________
725 const std::set<int>& PHGenFitTrkFitter::get_disabled_layers() const
726 { return _disabled_layers; }
727 
728 /*
729  * GetNodes():
730  * Get all the all the required nodes off the node tree
731  */
733 {
734  //DST objects
735  //Truth container
736  _truth_container = findNode::getClass<PHG4TruthInfoContainer>(topNode, "G4TruthInfo");
737 
738  // Input Svtx Clusters
739  //_clustermap = findNode::getClass<SvtxClusterMap>(topNode, "SvtxClusterMap");
740  _clustermap = findNode::getClass<TrkrClusterContainer>(topNode, "TRKR_CLUSTER");
741  if (!_clustermap && _event < 2)
742  {
743  cout << PHWHERE << " TRKR_CLUSTER node not found on node tree"
744  << endl;
746  }
747 
748  // Input Svtx Tracks
749  _trackmap = findNode::getClass<SvtxTrackMap>(topNode, "SvtxTrackMap");
750  if (!_trackmap && _event < 2)
751  {
752  cout << PHWHERE << " SvtxTrackMap node not found on node tree"
753  << endl;
755  }
756 
757  // Input Svtx Vertices
758  _vertexmap = findNode::getClass<SvtxVertexMap>(topNode, "SvtxVertexMap");
759  if (!_vertexmap && _event < 2)
760  {
761  cout << PHWHERE << " SvtxVertexrMap node not found on node tree"
762  << endl;
764  }
765 
766  // Output Svtx Tracks
768  {
769  _trackmap_refit = findNode::getClass<SvtxTrackMap>(topNode,
770  "SvtxTrackMapRefit");
771  if (!_trackmap_refit && _event < 2)
772  {
773  cout << PHWHERE << " SvtxTrackMapRefit node not found on node tree"
774  << endl;
776  }
777  }
778 
779  // Output Primary Svtx Tracks
781  {
782  _primary_trackmap = findNode::getClass<SvtxTrackMap>(topNode,
783  "PrimaryTrackMap");
784  if (!_primary_trackmap && _event < 2)
785  {
786  cout << PHWHERE << " PrimaryTrackMap node not found on node tree"
787  << endl;
789  }
790  }
791 
792  // Output Svtx Vertices
793  _vertexmap_refit = findNode::getClass<SvtxVertexMap>(topNode,
794  "SvtxVertexMapRefit");
795  if (!_vertexmap_refit && _event < 2)
796  {
797  cout << PHWHERE << " SvtxVertexMapRefit node not found on node tree"
798  << endl;
800  }
801 
803 }
804 
805 //struct CompMeasurementByR {
806 // bool operator() (PHGenFit::Measurement *m1,PHGenFit::Measurement *m2) {
807 // float x1 = m1->getMeasurement()
808 //
809 // return (i<j);}
810 //} myobject;
811 
812 /*
813  * fit track with SvtxTrack as input seed.
814  * \param intrack Input SvtxTrack
815  * \param invertex Input Vertex, if fit track as a primary vertex
816  */
817 //PHGenFit::Track* PHGenFitTrkFitter::ReFitTrack(PHCompositeNode *topNode, const SvtxTrack* intrack,
818 std::shared_ptr<PHGenFit::Track> PHGenFitTrkFitter::ReFitTrack(PHCompositeNode* topNode, const SvtxTrack* intrack,
819  const SvtxVertex* invertex)
820 {
821  //std::shared_ptr<PHGenFit::Track> empty_track(nullptr);
822  if (!intrack)
823  {
824  cerr << PHWHERE << " Input SvtxTrack is nullptr!" << endl;
825  return nullptr;
826  }
827 
828  auto geom_container_intt = findNode::getClass<PHG4CylinderGeomContainer>(topNode, "CYLINDERGEOM_INTT");
829  assert( geom_container_intt );
830 
831  auto geom_container_mvtx = findNode::getClass<PHG4CylinderGeomContainer>(topNode, "CYLINDERGEOM_MVTX");
832  assert( geom_container_mvtx );
833 
834  /* no need to check for the container validity here. The check is done if micromegas clusters are actually found in the track */
835  auto geom_container_micromegas = findNode::getClass<PHG4CylinderGeomContainer>(topNode, "CYLINDERGEOM_MICROMEGAS_FULL");
836 
837  // prepare seed
838  TVector3 seed_mom(100, 0, 0);
839  TVector3 seed_pos(0, 0, 0);
840  TMatrixDSym seed_cov(6);
841  for (int i = 0; i < 6; i++)
842  {
843  for (int j = 0; j < 6; j++)
844  {
845  seed_cov[i][j] = 100.;
846  }
847  }
848 
849  // Create measurements
850  std::vector<PHGenFit::Measurement*> measurements;
851 
856  /*
857  if(invertex and Verbosity() >= 2)
858  {
859  LogDebug(invertex->size_tracks());
860  LogDebug(invertex->get_chisq());
861  LogDebug(invertex->get_ndof());
862  for (unsigned int i = 0; i < 3; i++)
863  for (unsigned int j = 0; j < 3; j++)
864  {
865  LogDebug(invertex->get_error(i,j));
866  }
867  }
868  */
869 
873 #if _DEBUG_MODE_ == 1
874  if (invertex
875  // and invertex->size_tracks() == 1
876  )
877  {
878  TRandom3 rand(0);
879  double dxy = 0.0007; //7 um
880  double dz = 0.003; //30 um
881 
882  TVector3 pos(invertex->get_x(), invertex->get_y(), invertex->get_z());
883  TMatrixDSym cov(3);
884 
885  // Use smeared position instead of reco'd one.
886  double x = rand.Gaus(0, dxy);
887  double y = rand.Gaus(0, dxy);
888  double z = rand.Gaus(0, dz);
889  pos.SetXYZ(x, y, z);
890 
891  for (int i = 0; i < 3; i++)
892  for (int j = 0; j < 3; j++)
893  cov[i][j] = 0;
894 
895  cov[0][0] = dxy * dxy;
896  cov[1][1] = dxy * dxy;
897  cov[2][2] = dz * dz;
898 
900  pos, cov);
901  measurements.push_back(meas);
902  }
903 #else
904 
905  const double vertex_chi2_over_dnf_cut = 1000;
906  const double vertex_cov_element_cut = 10000; //arbitrary cut cm*cm
907 
908  if (invertex and invertex->size_tracks() > 1 and invertex->get_chisq() / invertex->get_ndof() < vertex_chi2_over_dnf_cut)
909  {
910  TVector3 pos(invertex->get_x(), invertex->get_y(), invertex->get_z());
911  TMatrixDSym cov(3);
912  cov.Zero();
913  bool is_vertex_cov_sane = true;
914  for (unsigned int i = 0; i < 3; i++)
915  for (unsigned int j = 0; j < 3; j++)
916  {
917  cov(i, j) = invertex->get_error(i, j);
918 
919  if (i == j)
920  {
921  if (!(invertex->get_error(i, j) > 0 and invertex->get_error(i, j) < vertex_cov_element_cut))
922  is_vertex_cov_sane = false;
923  }
924  }
925 
926  if (is_vertex_cov_sane)
927  {
929  pos, cov);
930  measurements.push_back(meas);
931  if(Verbosity() >= 2)
932  {
933  meas->getMeasurement()->Print();
934  }
935  }
936  }
937 #endif
938 
939  // sort clusters with radius before fitting
940  if(Verbosity() > 10) intrack->identify();
941  std::map<float, TrkrDefs::cluskey> m_r_cluster_id;
942  for (auto iter = intrack->begin_cluster_keys();
943  iter != intrack->end_cluster_keys(); ++iter){
944  TrkrDefs::cluskey cluster_key = *iter;
945  TrkrCluster* cluster = _clustermap->findCluster(cluster_key);
946  float x = cluster->getPosition(0);
947  float y = cluster->getPosition(1);
948  float r = sqrt(square(x) + square(y));
949  m_r_cluster_id.insert(std::pair<float, TrkrDefs::cluskey>(r, cluster_key));
950  int layer_out = TrkrDefs::getLayer(cluster_key);
951  if(Verbosity() > 10) cout << " Layer " << layer_out << " cluster " << cluster_key << " radius " << r << endl;
952  }
953 
954  for (auto iter = m_r_cluster_id.begin();
955  iter != m_r_cluster_id.end();
956  ++iter)
957  {
958  TrkrDefs::cluskey cluster_key = iter->second;
959  const int layer = TrkrDefs::getLayer(cluster_key);
960 
961  // skip disabled layers
962  if( _disabled_layers.find( layer ) != _disabled_layers.end() )
963  { continue; }
964 
965  TrkrCluster* cluster = _clustermap->findCluster(cluster_key);
966  if (!cluster)
967  {
968  LogError("No cluster Found!");
969  continue;
970  }
971 
972 #ifdef _DEBUG_
973  int output_layer = TrkrDefs::getLayer(cluster_key);
974  cout
975  << __LINE__
976  << ": ID: " << cluster_key
977  << ": layer: " << output_layer
978  << endl;
979 #endif
980 
981  TVector3 pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
982 
983  seed_mom.SetPhi(pos.Phi());
984  seed_mom.SetTheta(pos.Theta());
985 
986  // by default assume normal to local surface is along cluster azimuthal position
987  TVector3 n(cluster->getPosition(0), cluster->getPosition(1), 0);
988 
989  // replace normal by proper vector for specified subsystems
990  switch( TrkrDefs::getTrkrId(cluster_key) )
991  {
992 
993  case TrkrDefs::mvtxId:
994  {
995  int stave_index = MvtxDefs::getStaveId(cluster_key);
996  int chip_index = MvtxDefs::getChipId(cluster_key);
997 
998  double ladder_location[3] = {0.0, 0.0, 0.0};
999  auto geom = static_cast<CylinderGeom_Mvtx*>(geom_container_mvtx->GetLayerGeom(layer));
1000  // returns the center of the sensor in world coordinates - used to get the ladder phi location
1001  geom->find_sensor_center(stave_index, 0,
1002  0, chip_index, ladder_location);
1003 
1004  //cout << " MVTX stave phi tilt = " << geom->get_stave_phi_tilt()
1005  // << " seg.X " << ladder_location[0] << " seg.Y " << ladder_location[1] << " seg.Z " << ladder_location[2] << endl;
1006  n.SetXYZ(ladder_location[0], ladder_location[1], 0);
1007  n.RotateZ(geom->get_stave_phi_tilt());
1008  break;
1009  }
1010 
1011  case TrkrDefs::inttId:
1012  {
1013  auto geom = static_cast<CylinderGeomIntt*>(geom_container_intt->GetLayerGeom(layer));
1014  double hit_location[3] = {0.0, 0.0, 0.0};
1015  geom->find_segment_center(InttDefs::getLadderZId(cluster_key),
1016  InttDefs::getLadderPhiId(cluster_key), hit_location);
1017 
1018  //cout << " Intt strip phi tilt = " << geom->get_strip_phi_tilt()
1019  // << " seg.X " << hit_location[0] << " seg.Y " << hit_location[1] << " seg.Z " << hit_location[2] << endl;
1020  n.SetXYZ(hit_location[0], hit_location[1], 0);
1021  n.RotateZ(geom->get_strip_phi_tilt());
1022  break;
1023  }
1024 
1026  {
1027  // get geometry
1028  /* a situation where micromegas clusters are found, but not the geometry, should not happen */
1029  assert( geom_container_micromegas );
1030  auto geom = static_cast<CylinderGeomMicromegas*>(geom_container_micromegas->GetLayerGeom(layer));
1031  const auto tileid = MicromegasDefs::getTileId( cluster_key );
1032 
1033  // in local coordinate, n is along y axis
1034  // convert to global coordinates
1035  n = geom->get_world_from_local_vect( tileid, TVector3( 0, 1, 0 ) );
1036  }
1037 
1038  default: break;
1039  }
1040 
1041  // create measurement
1042  auto meas = new PHGenFit::PlanarMeasurement(pos, n, cluster->getRPhiError(), cluster->getZError());
1043 
1044  if(Verbosity() > 10)
1045  {
1046  cout << "Add meas layer " << layer << " cluskey " << cluster_key
1047  << endl
1048  << " pos.X " << pos.X() << " pos.Y " << pos.Y() << " pos.Z " << pos.Z()
1049  << " n.X " << n.X() << " n.Y " << n.Y()
1050  << " RPhiErr " << cluster->getRPhiError()
1051  << " ZErr " << cluster->getZError()
1052  << endl;
1053  }
1054  measurements.push_back(meas);
1055  }
1056 
1057 
1066  //TODO Add multiple TrackRep choices.
1067  //int pid = 211;
1068  genfit::AbsTrackRep* rep = new genfit::RKTrackRep(_primary_pid_guess);
1069  std::shared_ptr<PHGenFit::Track> track(new PHGenFit::Track(rep, seed_pos, seed_mom,
1070  seed_cov));
1071 
1072  //TODO unsorted measurements, should use sorted ones?
1073  track->addMeasurements(measurements);
1074 
1080  if (_fitter->processTrack(track.get(), false) != 0)
1081  {
1082  if (Verbosity() >= 1)
1083  {
1084  LogWarning("Track fitting failed");
1085  /*
1086  cout << " track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
1087  << " mom.X " << track->get_mom().X()
1088  << " mom.Y " << track->get_mom().Y()
1089  << " mom.Z " << track->get_mom().Z()
1090  << endl;
1091  */
1092  }
1093  //delete track;
1094  return nullptr;
1095  }
1096 
1097  if(Verbosity() > 10)
1098  cout << " track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
1099  << " mom.X " << track->get_mom().X()
1100  << " mom.Y " << track->get_mom().Y()
1101  << " mom.Z " << track->get_mom().Z()
1102  << endl;
1103 
1104  return track;
1105 }
1106 
1107 /*
1108  * Make SvtxTrack from PHGenFit::Track and SvtxTrack
1109  */
1110 //SvtxTrack* PHGenFitTrkFitter::MakeSvtxTrack(const SvtxTrack* svtx_track,
1111 std::shared_ptr<SvtxTrack> PHGenFitTrkFitter::MakeSvtxTrack(const SvtxTrack* svtx_track,
1112  const std::shared_ptr<PHGenFit::Track>& phgf_track, const SvtxVertex* vertex)
1113 {
1114  double chi2 = phgf_track->get_chi2();
1115  double ndf = phgf_track->get_ndf();
1116 
1117  TVector3 vertex_position(0, 0, 0);
1118  TMatrixF vertex_cov(3, 3);
1119  double dvr2 = 0;
1120  double dvz2 = 0;
1121 
1122  if (_use_truth_vertex )
1123  {
1124  if( _truth_container )
1125  {
1127  vertex_position.SetXYZ(first_point->get_x(), first_point->get_y(), first_point->get_z());
1128  if (Verbosity() > 1)
1129  {
1130  std::cout << PHWHERE << "Using: truth vertex: {" << vertex_position.X() << ", " << vertex_position.Y() << ", " << vertex_position.Z() << "} " << std::endl;
1131  }
1132  } else {
1133  std::cout << PHWHERE << "PHG4TruthInfoContainer not found. Cannot use truth vertex." << std::endl;
1134  }
1135  }
1136  else if (vertex)
1137  {
1138  vertex_position.SetXYZ(vertex->get_x(), vertex->get_y(),
1139  vertex->get_z());
1140  dvr2 = vertex->get_error(0, 0) + vertex->get_error(1, 1);
1141  dvz2 = vertex->get_error(2, 2);
1142 
1143  for (int i = 0; i < 3; i++)
1144  for (int j = 0; j < 3; j++)
1145  vertex_cov[i][j] = vertex->get_error(i, j);
1146  }
1147 
1148  std::unique_ptr<genfit::MeasuredStateOnPlane> gf_state_beam_line_ca;
1149  try
1150  {
1151  gf_state_beam_line_ca.reset(phgf_track->extrapolateToLine(vertex_position,
1152  TVector3(0., 0., 1.)));
1153  }
1154  catch (...)
1155  {
1156  if (Verbosity() >= 2)
1157  LogWarning("extrapolateToLine failed!");
1158  }
1159  if (!gf_state_beam_line_ca) return nullptr;
1160 
1168  double u = gf_state_beam_line_ca->getState()[3];
1169  double v = gf_state_beam_line_ca->getState()[4];
1170 
1171  double du2 = gf_state_beam_line_ca->getCov()[3][3];
1172  double dv2 = gf_state_beam_line_ca->getCov()[4][4];
1173  //cout << PHWHERE << " u " << u << " v " << v << " du2 " << du2 << " dv2 " << dv2 << " dvr2 " << dvr2 << endl;
1174  //delete gf_state_beam_line_ca;
1175 
1176  // create new track
1177  auto out_track = std::make_shared<SvtxTrack_v2>(*svtx_track);
1178 
1179  // clear states and insert empty one for vertex position
1180  out_track->clear_states();
1181  {
1182  /*
1183  insert first, dummy state, as done in constructor,
1184  so that the track state list is never empty. Note that insert_state, despite taking a pointer as argument,
1185  does not take ownership of the state
1186  */
1187  SvtxTrackState_v1 first(0.0);
1188  out_track->insert_state( &first );
1189  }
1190 
1191  out_track->set_dca2d(u);
1192  out_track->set_dca2d_error(sqrt(du2 + dvr2));
1193 
1194  std::unique_ptr<genfit::MeasuredStateOnPlane> gf_state_vertex_ca;
1195  try
1196  {
1197  gf_state_vertex_ca.reset( phgf_track->extrapolateToPoint(vertex_position) );
1198  }
1199  catch (...)
1200  {
1201  if (Verbosity() >= 2)
1202  LogWarning("extrapolateToPoint failed!");
1203  }
1204  if (!gf_state_vertex_ca)
1205  {
1206  //delete out_track;
1207  return nullptr;
1208  }
1209 
1210  TVector3 mom = gf_state_vertex_ca->getMom();
1211  TVector3 pos = gf_state_vertex_ca->getPos();
1212  TMatrixDSym cov = gf_state_vertex_ca->get6DCov();
1213 
1214  // genfit::MeasuredStateOnPlane* gf_state_vertex_ca =
1215  // phgf_track->extrapolateToLine(vertex_position,
1216  // TVector3(0., 0., 1.));
1217 
1218  u = gf_state_vertex_ca->getState()[3];
1219  v = gf_state_vertex_ca->getState()[4];
1220 
1221  du2 = gf_state_vertex_ca->getCov()[3][3];
1222  dv2 = gf_state_vertex_ca->getCov()[4][4];
1223 
1224  double dca3d = sqrt(u * u + v * v);
1225  double dca3d_error = sqrt(du2 + dv2 + dvr2 + dvz2);
1226 
1227  out_track->set_dca(dca3d);
1228  out_track->set_dca_error(dca3d_error);
1229 
1234  /*
1235  // Rotate from u,v,n to r: n X Z, Z': n X r, n using 5D state/cov
1236  // commented on 2017-10-09
1237 
1238  TMatrixF pos_in(3,1);
1239  TMatrixF cov_in(3,3);
1240  pos_in[0][0] = gf_state_vertex_ca->getState()[3];
1241  pos_in[1][0] = gf_state_vertex_ca->getState()[4];
1242  pos_in[2][0] = 0.;
1243 
1244  cov_in[0][0] = gf_state_vertex_ca->getCov()[3][3];
1245  cov_in[0][1] = gf_state_vertex_ca->getCov()[3][4];
1246  cov_in[0][2] = 0.;
1247  cov_in[1][0] = gf_state_vertex_ca->getCov()[4][3];
1248  cov_in[1][1] = gf_state_vertex_ca->getCov()[4][4];
1249  cov_in[1][2] = 0.;
1250  cov_in[2][0] = 0.;
1251  cov_in[2][1] = 0.;
1252  cov_in[2][2] = 0.;
1253 
1254  TMatrixF pos_out(3,1);
1255  TMatrixF cov_out(3,3);
1256 
1257  TVector3 vu = gf_state_vertex_ca->getPlane().get()->getU();
1258  TVector3 vv = gf_state_vertex_ca->getPlane().get()->getV();
1259  TVector3 vn = vu.Cross(vv);
1260 
1261  pos_cov_uvn_to_rz(vu, vv, vn, pos_in, cov_in, pos_out, cov_out);
1262 
1264  TMatrixF vertex_cov_out(3,3);
1265 
1266  get_vertex_error_uvn(vu,vv,vn, vertex_cov, vertex_cov_out);
1267 
1268  float dca3d_xy = pos_out[0][0];
1269  float dca3d_z = pos_out[1][0];
1270 
1271  float dca3d_xy_error = sqrt(cov_out[0][0] + vertex_cov_out[0][0]);
1272  float dca3d_z_error = sqrt(cov_out[1][1] + vertex_cov_out[1][1]);
1273 
1274  //Begin DEBUG
1275 // LogDebug("rotation debug---------- ");
1276 // gf_state_vertex_ca->Print();
1277 // LogDebug("dca rotation---------- ");
1278 // pos_out = pos_in;
1279 // cov_out = cov_in;
1280 // pos_in.Print();
1281 // cov_in.Print();
1282 // pos_out.Print();
1283 // cov_out.Print();
1284 // cout
1285 // <<"dca3d_xy: "<<dca3d_xy <<" +- "<<dca3d_xy_error*dca3d_xy_error
1286 // <<"; dca3d_z: "<<dca3d_z<<" +- "<< dca3d_z_error*dca3d_z_error
1287 // <<"\n";
1288 // gf_state_vertex_ca->get6DCov().Print();
1289 // LogDebug("vertex rotation---------- ");
1290 // vertex_position.Print();
1291 // vertex_cov.Print();
1292 // vertex_cov_out.Print();
1293  //End DEBUG
1294  */
1295 
1296  //
1297  // in: X, Y, Z; out; r: n X Z, Z X r, Z
1298 
1299  float dca3d_xy = NAN;
1300  float dca3d_z = NAN;
1301  float dca3d_xy_error = NAN;
1302  float dca3d_z_error = NAN;
1303 
1304  try
1305  {
1306  TMatrixF pos_in(3, 1);
1307  TMatrixF cov_in(3, 3);
1308  TMatrixF pos_out(3, 1);
1309  TMatrixF cov_out(3, 3);
1310 
1311  TVectorD state6(6); // pos(3), mom(3)
1312  TMatrixDSym cov6(6, 6); //
1313 
1314  gf_state_vertex_ca->get6DStateCov(state6, cov6);
1315 
1316  TVector3 vn(state6[3], state6[4], state6[5]);
1317 
1318  // mean of two multivariate gaussians Pos - Vertex
1319  pos_in[0][0] = state6[0] - vertex_position.X();
1320  pos_in[1][0] = state6[1] - vertex_position.Y();
1321  pos_in[2][0] = state6[2] - vertex_position.Z();
1322 
1323  for (int i = 0; i < 3; ++i)
1324  {
1325  for (int j = 0; j < 3; ++j)
1326  {
1327  cov_in[i][j] = cov6[i][j] + vertex_cov[i][j];
1328  }
1329  }
1330 
1331  // vn is momentum vector, pos_in is position vector (of what?)
1332  pos_cov_XYZ_to_RZ(vn, pos_in, cov_in, pos_out, cov_out);
1333 
1334  if(Verbosity() > 30)
1335  {
1336  cout << " vn.X " << vn.X() << " vn.Y " << vn.Y() << " vn.Z " << vn.Z() << endl;
1337  cout << " pos_in.X " << pos_in[0][0] << " pos_in.Y " << pos_in[1][0] << " pos_in.Z " << pos_in[2][0] << endl;
1338  cout << " pos_out.X " << pos_out[0][0] << " pos_out.Y " << pos_out[1][0] << " pos_out.Z " << pos_out[2][0] << endl;
1339  }
1340 
1341 
1342  dca3d_xy = pos_out[0][0];
1343  dca3d_z = pos_out[2][0];
1344  dca3d_xy_error = sqrt(cov_out[0][0]);
1345  dca3d_z_error = sqrt(cov_out[2][2]);
1346 
1347 #ifdef _DEBUG_
1348  cout << __LINE__ << ": Vertex: ----------------" << endl;
1349  vertex_position.Print();
1350  vertex_cov.Print();
1351 
1352  cout << __LINE__ << ": State: ----------------" << endl;
1353  state6.Print();
1354  cov6.Print();
1355 
1356  cout << __LINE__ << ": Mean: ----------------" << endl;
1357  pos_in.Print();
1358  cout << "===>" << endl;
1359  pos_out.Print();
1360 
1361  cout << __LINE__ << ": Cov: ----------------" << endl;
1362  cov_in.Print();
1363  cout << "===>" << endl;
1364  cov_out.Print();
1365 
1366  cout << endl;
1367 #endif
1368  }
1369  catch (...)
1370  {
1371  if (Verbosity() > 0)
1372  LogWarning("DCA calculationfailed!");
1373  }
1374 
1375  out_track->set_dca3d_xy(dca3d_xy);
1376  out_track->set_dca3d_z(dca3d_z);
1377  out_track->set_dca3d_xy_error(dca3d_xy_error);
1378  out_track->set_dca3d_z_error(dca3d_z_error);
1379 
1380  //if(gf_state_vertex_ca) delete gf_state_vertex_ca;
1381 
1382  out_track->set_chisq(chi2);
1383  out_track->set_ndf(ndf);
1384  out_track->set_charge(phgf_track->get_charge());
1385 
1386  out_track->set_px(mom.Px());
1387  out_track->set_py(mom.Py());
1388  out_track->set_pz(mom.Pz());
1389 
1390  out_track->set_x(pos.X());
1391  out_track->set_y(pos.Y());
1392  out_track->set_z(pos.Z());
1393 
1394  for (int i = 0; i < 6; i++)
1395  {
1396  for (int j = i; j < 6; j++)
1397  {
1398  out_track->set_error(i, j, cov[i][j]);
1399  }
1400  }
1401 
1402  // for (SvtxTrack::ConstClusterIter iter = svtx_track->begin_clusters();
1403  // iter != svtx_track->end_clusters(); ++iter) {
1404  // unsigned int cluster_id = *iter;
1405  // SvtxCluster* cluster = _clustermap->get(cluster_id);
1406  // if (!cluster) {
1407  // LogError("No cluster Found!");
1408  // continue;
1409  // }
1410  // //cluster->identify; //DEBUG
1411  //
1412  // //unsigned int l = cluster->get_layer();
1413  //
1414  // TVector3 pos(cluster->get_x(), cluster->get_y(), cluster->get_z());
1415  //
1416  // double radius = pos.Pt();
1417  //
1418  // std::shared_ptr<genfit::MeasuredStateOnPlane> gf_state = nullptr;
1419  // try {
1420  // gf_state = std::shared_ptr < genfit::MeasuredStateOnPlane
1421  // > (phgf_track->extrapolateToCylinder(radius,
1422  // TVector3(0, 0, 0), TVector3(0, 0, 1), 0));
1423  // } catch (...) {
1424  // if (Verbosity() >= 2)
1425  // LogWarning("Exrapolation failed!");
1426  // }
1427  // if (!gf_state) {
1428  // if (Verbosity() > 1)
1429  // LogWarning("Exrapolation failed!");
1430  // continue;
1431  // }
1432  //
1433  // //SvtxTrackState* state = new SvtxTrackState_v1(radius);
1434  // std::shared_ptr<SvtxTrackState> state = std::shared_ptr<SvtxTrackState> (new SvtxTrackState_v1(radius));
1435  // state->set_x(gf_state->getPos().x());
1436  // state->set_y(gf_state->getPos().y());
1437  // state->set_z(gf_state->getPos().z());
1438  //
1439  // state->set_px(gf_state->getMom().x());
1440  // state->set_py(gf_state->getMom().y());
1441  // state->set_pz(gf_state->getMom().z());
1442  //
1443  // //gf_state->getCov().Print();
1444  //
1445  // for (int i = 0; i < 6; i++) {
1446  // for (int j = i; j < 6; j++) {
1447  // state->set_error(i, j, gf_state->get6DCov()[i][j]);
1448  // }
1449  // }
1450  //
1451  // out_track->insert_state(state.get());
1452  //
1453  //#ifdef _DEBUG_
1454  // cout
1455  // <<__LINE__
1456  // <<": " << radius <<" => "
1457  // <<sqrt(state->get_x()*state->get_x() + state->get_y()*state->get_y())
1458  // <<endl;
1459  //#endif
1460  // }
1461 
1462 #ifdef _DEBUG_
1463  cout << __LINE__ << endl;
1464 #endif
1465 
1466  const genfit::Track* gftrack = phgf_track->getGenFitTrack();
1467  const genfit::AbsTrackRep* rep = gftrack->getCardinalRep();
1468  for (unsigned int id = 0; id < gftrack->getNumPointsWithMeasurement(); ++id)
1469  {
1470  genfit::TrackPoint* trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id, gftrack->getCardinalRep());
1471 
1472  if (!trpoint)
1473  {
1474  if (Verbosity() > 1)
1475  LogWarning("!trpoint");
1476  continue;
1477  }
1478 
1479  genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1480  if (!kfi)
1481  {
1482  if (Verbosity() > 1)
1483  LogWarning("!kfi");
1484  continue;
1485  }
1486 
1487  const genfit::MeasuredStateOnPlane* gf_state = nullptr;
1488  try
1489  {
1490  // this works because KalmanFitterInfo returns a const reference to internal object and not a temporary object
1491  gf_state = &kfi->getFittedState(true);
1492  }
1493  catch (...)
1494  {
1495  if (Verbosity() >= 1)
1496  LogWarning("Exrapolation failed!");
1497  }
1498  if (!gf_state)
1499  {
1500  if (Verbosity() >= 1)
1501  LogWarning("Exrapolation failed!");
1502  continue;
1503  }
1504  genfit::MeasuredStateOnPlane temp;
1505  float pathlength = -phgf_track->extrapolateToPoint(temp, vertex_position, id);
1506 
1507  // create new svtx state and add to track
1508  auto state = create_track_state( pathlength, gf_state );
1509  out_track->insert_state( &state );
1510 
1511 #ifdef _DEBUG_
1512  cout
1513  << __LINE__
1514  << ": " << id
1515  << ": " << pathlength << " => "
1516  << sqrt(state->get_x() * state->get_x() + state->get_y() * state->get_y())
1517  << endl;
1518 #endif
1519  }
1520 
1521  // loop over clusters, check if layer is disabled, include extrapolated SvtxTrackState
1522  if( !_disabled_layers.empty() )
1523  {
1524 
1525  unsigned int id_min = 0;
1526  for (auto iter = svtx_track->begin_cluster_keys(); iter != svtx_track->end_cluster_keys(); ++iter)
1527  {
1528 
1529  auto cluster_key = *iter;
1530  auto cluster = _clustermap->findCluster(cluster_key);
1531  const int layer = TrkrDefs::getLayer(cluster_key);
1532 
1533  // skip enabled layers
1534  if( _disabled_layers.find( layer ) == _disabled_layers.end() )
1535  { continue; }
1536 
1537  // get position
1538  TVector3 pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
1539  float r_cluster = std::sqrt( square(pos[0]) + square(pos[1]) );
1540 
1541  // loop over states
1542  /* find first state whose radius is larger than that of cluster if any */
1543  unsigned int id = id_min;
1544  for( ; id < gftrack->getNumPointsWithMeasurement(); ++id)
1545  {
1546 
1547  auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id, rep);
1548  if (!trpoint) continue;
1549 
1550  auto kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1551  if (!kfi) continue;
1552 
1553  const genfit::MeasuredStateOnPlane* gf_state = nullptr;
1554  try
1555  {
1556 
1557  gf_state = &kfi->getFittedState(true);
1558 
1559  } catch (...) {
1560 
1561  if (Verbosity() > 1)
1562  { LogWarning("Failed to get kf fitted state"); }
1563 
1564  }
1565 
1566  if( !gf_state ) continue;
1567 
1568  float r_track = std::sqrt( square( gf_state->getPos().x() ) + square( gf_state->getPos().y() ) );
1569  if( r_track > r_cluster ) break;
1570 
1571  }
1572 
1573  // forward extrapolation
1574  genfit::MeasuredStateOnPlane gf_state;
1575  float pathlength = 0;
1576 
1577  // first point is previous, if valid
1578  if( id > 0 ) id_min = id-1;
1579 
1580  // extrapolate forward
1581  try
1582  {
1583  auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id_min, rep);
1584  if( !trpoint ) continue;
1585 
1586  auto kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1587  gf_state = *kfi->getForwardUpdate();
1588  pathlength = gf_state.extrapolateToPoint( pos );
1589  auto tmp = *kfi->getBackwardUpdate();
1590  pathlength -= tmp.extrapolateToPoint( vertex_position );
1591  } catch (...) {
1592  if(Verbosity() > 0)
1593  { std::cerr << PHWHERE << "Failed to forward extrapolate from id " << id_min << " to disabled layer " << layer << std::endl; }
1594  continue;
1595  }
1596 
1597  // also extrapolate backward from next state if any
1598  // and take the weighted average between both points
1599  if( id > 0 && id < gftrack->getNumPointsWithMeasurement() )
1600  try
1601  {
1602  auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id, rep);
1603  if( !trpoint ) continue;
1604 
1605  auto kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1606  genfit::KalmanFittedStateOnPlane gf_state_backward = *kfi->getBackwardUpdate();
1607  gf_state_backward.extrapolateToPlane( gf_state.getPlane() );
1608  gf_state = genfit::calcAverageState( gf_state, gf_state_backward );
1609  } catch (...) {
1610  if(Verbosity() > 0)
1611  { std::cerr << PHWHERE << "Failed to backward extrapolate from id " << id << " to disabled layer " << layer << std::endl; }
1612  continue;
1613  }
1614 
1615  // create new svtx state and add to track
1616  auto state = create_track_state( pathlength, &gf_state );
1617  out_track->insert_state( &state );
1618 
1619  }
1620 
1621  }
1622 
1623  return out_track;
1624 }
1625 
1626 /*
1627  * Fill SvtxVertexMap from GFRaveVertexes and Tracks
1628  */
1630  const std::vector<genfit::GFRaveVertex*>& rave_vertices,
1631  const std::vector<genfit::Track*>& gf_tracks)
1632 {
1633 
1634  if(Verbosity() > 0) cout << "Rave vertices size " << rave_vertices.size() << endl;
1635  if(rave_vertices.size() > 0)
1636  {
1637  for (unsigned int ivtx = 0; ivtx < rave_vertices.size(); ++ivtx)
1638  {
1639  genfit::GFRaveVertex* rave_vtx = rave_vertices[ivtx];
1640 
1641  if (!rave_vtx)
1642  {
1643  cerr << PHWHERE << endl;
1644  return false;
1645  }
1646 
1647  if(Verbosity() > 0) cout << " ivtx " << ivtx << " has Z = " << rave_vtx->getPos().Z() << endl;
1648 
1649  SvtxVertex_v1 svtx_vtx;
1650  svtx_vtx.set_chisq(rave_vtx->getChi2());
1651  svtx_vtx.set_ndof(rave_vtx->getNdf());
1652  svtx_vtx.set_position(0, rave_vtx->getPos().X());
1653  svtx_vtx.set_position(1, rave_vtx->getPos().Y());
1654  svtx_vtx.set_position(2, rave_vtx->getPos().Z());
1655 
1656  for (int i = 0; i < 3; i++)
1657  for (int j = 0; j < 3; j++)
1658  svtx_vtx.set_error(i, j, rave_vtx->getCov()[i][j]);
1659 
1660  for (unsigned int i = 0; i < rave_vtx->getNTracks(); i++)
1661  {
1662  //TODO Assume id's are sync'ed between _trackmap_refit and gf_tracks, need to change?
1663  const genfit::Track* rave_track =
1664  rave_vtx->getParameters(i)->getTrack();
1665  for (unsigned int j = 0; j < gf_tracks.size(); j++)
1666  {
1667  if (rave_track == gf_tracks[j])
1668  {
1669  svtx_vtx.insert_track(j);
1670  _rave_vertex_gf_track_map.insert(std::pair<unsigned int, unsigned int>(j, ivtx));
1671  if(Verbosity() > 0) cout << " rave vertex " << ivtx << " at Z " << svtx_vtx.get_position(2) << " rave track " << i << " genfit track ID " << j << endl;
1672  }
1673  }
1674  }
1675 
1676  if (_vertexmap_refit)
1677  {
1678  if(Verbosity() > 0) cout << "insert svtx_vtx into _vertexmap_refit " << endl;
1679  _vertexmap_refit->insert_clone( &svtx_vtx );
1680  if(Verbosity() > 10) _vertexmap_refit->identify();
1681  }
1682  else
1683  {
1684  LogError("!_vertexmap_refit");
1685  }
1686  } //loop over RAVE vertices
1687  }
1688 
1689  return true;
1690 }
1691 
1692 //bool PHGenFitTrkFitter::pos_cov_uvn_to_rz(const TVector3 u, const TVector3 v,
1693 // const TVector3 n, const TMatrixF pos_in, const TMatrixF cov_in,
1694 // TMatrixF& pos_out, TMatrixF& cov_out) const {
1695 //
1696 // if(pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3) {
1697 // if(Verbosity() > 0) LogWarning("pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3");
1698 // return false;
1699 // }
1700 //
1701 // if(cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3) {
1702 // if(Verbosity() > 0) LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1703 // return false;
1704 // }
1705 //
1706 // TVector3 up = TVector3(0., 0., 1.).Cross(n);
1707 // if(up.Mag() < 0.00001){
1708 // if(Verbosity() > 0) LogWarning("n is parallel to z");
1709 // return false;
1710 // }
1711 //
1712 // TMatrixF R(3, 3);
1713 // TMatrixF R_inv(3,3);
1714 // TMatrixF R_inv_T(3,3);
1715 //
1716 // try {
1717 // TMatrixF ROT1(3, 3);
1718 // TMatrixF ROT2(3, 3);
1719 // TMatrixF ROT3(3, 3);
1720 //
1721 // // rotate n along z to xz plane
1722 // float phi = -TMath::ATan2(n.Y(), n.X());
1723 // ROT1[0][0] = cos(phi);
1724 // ROT1[0][1] = -sin(phi);
1725 // ROT1[0][2] = 0;
1726 // ROT1[1][0] = sin(phi);
1727 // ROT1[1][1] = cos(phi);
1728 // ROT1[1][2] = 0;
1729 // ROT1[2][0] = 0;
1730 // ROT1[2][1] = 0;
1731 // ROT1[2][2] = 1;
1732 //
1733 // // rotate n along y to z
1734 // TVector3 n1(n);
1735 // n1.RotateZ(phi);
1736 // float theta = -TMath::ATan2(n1.X(), n1.Z());
1737 // ROT2[0][0] = cos(theta);
1738 // ROT2[0][1] = 0;
1739 // ROT2[0][2] = sin(theta);
1740 // ROT2[1][0] = 0;
1741 // ROT2[1][1] = 1;
1742 // ROT2[1][2] = 0;
1743 // ROT2[2][0] = -sin(theta);
1744 // ROT2[2][1] = 0;
1745 // ROT2[2][2] = cos(theta);
1746 //
1747 // // rotate u along z to x
1748 // TVector3 u2(u);
1749 // u2.RotateZ(phi);
1750 // u2.RotateY(theta);
1751 // float phip = -TMath::ATan2(u2.Y(), u2.X());
1752 // phip -= -TMath::ATan2(up.Y(), up.X());
1753 // ROT3[0][0] = cos(phip);
1754 // ROT3[0][1] = -sin(phip);
1755 // ROT3[0][2] = 0;
1756 // ROT3[1][0] = sin(phip);
1757 // ROT3[1][1] = cos(phip);
1758 // ROT3[1][2] = 0;
1759 // ROT3[2][0] = 0;
1760 // ROT3[2][1] = 0;
1761 // ROT3[2][2] = 1;
1762 //
1763 // // R: rotation from u,v,n to (z X n), v', z
1764 // R = ROT3 * ROT2 * ROT1;
1765 // R_inv = R.Invert();
1766 // R_inv_T.Transpose(R_inv);
1767 //
1768 // } catch (...) {
1769 // if (Verbosity() > 0)
1770 // LogWarning("Can't get rotation matrix");
1771 //
1772 // return false;
1773 // }
1774 //
1775 // pos_out.ResizeTo(3, 1);
1776 // cov_out.ResizeTo(3, 3);
1777 //
1778 // pos_out = R_inv * pos_in;
1779 // cov_out = R_inv * cov_in * R_inv_T;
1780 //
1781 // return true;
1782 //}
1783 
1784 bool PHGenFitTrkFitter::pos_cov_uvn_to_rz(const TVector3& u, const TVector3& v,
1785  const TVector3& n, const TMatrixF& pos_in, const TMatrixF& cov_in,
1786  TMatrixF& pos_out, TMatrixF& cov_out) const
1787 {
1788  if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1789  {
1790  if (Verbosity() > 0) LogWarning("pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3");
1791  return false;
1792  }
1793 
1794  if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1795  {
1796  if (Verbosity() > 0) LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1797  return false;
1798  }
1799 
1800  TVector3 Z_uvn(u.Z(), v.Z(), n.Z());
1801  TVector3 up_uvn = TVector3(0., 0., 1.).Cross(Z_uvn); // n_uvn X Z_uvn
1802 
1803  if (up_uvn.Mag() < 0.00001)
1804  {
1805  if (Verbosity() > 0) LogWarning("n is parallel to z");
1806  return false;
1807  }
1808 
1809  // R: rotation from u,v,n to n X Z, nX(nXZ), n
1810  TMatrixF R(3, 3);
1811  TMatrixF R_T(3, 3);
1812 
1813  try
1814  {
1815  // rotate u along z to up
1816  float phi = -TMath::ATan2(up_uvn.Y(), up_uvn.X());
1817  R[0][0] = cos(phi);
1818  R[0][1] = -sin(phi);
1819  R[0][2] = 0;
1820  R[1][0] = sin(phi);
1821  R[1][1] = cos(phi);
1822  R[1][2] = 0;
1823  R[2][0] = 0;
1824  R[2][1] = 0;
1825  R[2][2] = 1;
1826 
1827  R_T.Transpose(R);
1828  }
1829  catch (...)
1830  {
1831  if (Verbosity() > 0)
1832  LogWarning("Can't get rotation matrix");
1833 
1834  return false;
1835  }
1836 
1837  pos_out.ResizeTo(3, 1);
1838  cov_out.ResizeTo(3, 3);
1839 
1840  pos_out = R * pos_in;
1841  cov_out = R * cov_in * R_T;
1842 
1843  return true;
1844 }
1845 
1847  const TVector3& v, const TVector3& n, const TMatrixF& cov_in,
1848  TMatrixF& cov_out) const
1849 {
1855  TMatrixF R = get_rotation_matrix(u, v, n);
1856  //
1857  // LogDebug("PHGenFitTrkFitter::get_vertex_error_uvn::R = ");
1858  // R.Print();
1859  // cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
1860 
1861  if (!(abs(R.Determinant() - 1) < 0.01))
1862  {
1863  if (Verbosity() > 0)
1864  LogWarning("!(abs(R.Determinant()-1)<0.0001)");
1865  return false;
1866  }
1867 
1868  if (R.GetNcols() != 3 || R.GetNrows() != 3)
1869  {
1870  if (Verbosity() > 0)
1871  LogWarning("R.GetNcols() != 3 || R.GetNrows() != 3");
1872  return false;
1873  }
1874 
1875  if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1876  {
1877  if (Verbosity() > 0)
1878  LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1879  return false;
1880  }
1881 
1882  TMatrixF R_T(3, 3);
1883 
1884  R_T.Transpose(R);
1885 
1886  cov_out.ResizeTo(3, 3);
1887 
1888  cov_out = R * cov_in * R_T;
1889 
1890  return true;
1891 }
1892 
1894  const TVector3& n, const TMatrixF& pos_in, const TMatrixF& cov_in,
1895  TMatrixF& pos_out, TMatrixF& cov_out) const
1896 {
1897  if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1898  {
1899  if (Verbosity() > 0) LogWarning("pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3");
1900  return false;
1901  }
1902 
1903  if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1904  {
1905  if (Verbosity() > 0) LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1906  return false;
1907  }
1908 
1909  // produces a vector perpendicular to both the momentum vector and beam line - i.e. in the direction of the dca_xy
1910  // only the angle of r will be used, not the magnitude
1911  TVector3 r = n.Cross(TVector3(0., 0., 1.));
1912  if (r.Mag() < 0.00001)
1913  {
1914  if (Verbosity() > 0) LogWarning("n is parallel to z");
1915  return false;
1916  }
1917 
1918  // R: rotation from u,v,n to n X Z, nX(nXZ), n
1919  TMatrixF R(3, 3);
1920  TMatrixF R_T(3, 3);
1921 
1922  try
1923  {
1924  // rotate u along z to up
1925  float phi = -TMath::ATan2(r.Y(), r.X());
1926  R[0][0] = cos(phi);
1927  R[0][1] = -sin(phi);
1928  R[0][2] = 0;
1929  R[1][0] = sin(phi);
1930  R[1][1] = cos(phi);
1931  R[1][2] = 0;
1932  R[2][0] = 0;
1933  R[2][1] = 0;
1934  R[2][2] = 1;
1935 
1936  R_T.Transpose(R);
1937  }
1938  catch (...)
1939  {
1940  if (Verbosity() > 0)
1941  LogWarning("Can't get rotation matrix");
1942 
1943  return false;
1944  }
1945 
1946  pos_out.ResizeTo(3, 1);
1947  cov_out.ResizeTo(3, 3);
1948 
1949  pos_out = R * pos_in;
1950  cov_out = R * cov_in * R_T;
1951 
1952  return true;
1953 }
1954 
1960  const TVector3 y, const TVector3 z, const TVector3 xp, const TVector3 yp,
1961  const TVector3 zp) const
1962 {
1963  TMatrixF R(3, 3);
1964 
1965  TVector3 xu = x.Unit();
1966  TVector3 yu = y.Unit();
1967  TVector3 zu = z.Unit();
1968 
1969  const float max_diff = 0.01;
1970 
1971  if (!(
1972  abs(xu * yu) < max_diff and
1973  abs(xu * zu) < max_diff and
1974  abs(yu * zu) < max_diff))
1975  {
1976  if (Verbosity() > 0)
1977  LogWarning("input frame error!");
1978  return R;
1979  }
1980 
1981  TVector3 xpu = xp.Unit();
1982  TVector3 ypu = yp.Unit();
1983  TVector3 zpu = zp.Unit();
1984 
1985  if (!(
1986  abs(xpu * ypu) < max_diff and
1987  abs(xpu * zpu) < max_diff and
1988  abs(ypu * zpu) < max_diff))
1989  {
1990  if (Verbosity() > 0)
1991  LogWarning("output frame error!");
1992  return R;
1993  }
1994 
2000  TVector3 u(xpu.Dot(xu), xpu.Dot(yu), xpu.Dot(zu));
2001  TVector3 v(ypu.Dot(xu), ypu.Dot(yu), ypu.Dot(zu));
2002  TVector3 n(zpu.Dot(xu), zpu.Dot(yu), zpu.Dot(zu));
2003 
2004  try
2005  {
2006  std::shared_ptr<TRotation> rotation(new TRotation());
2007  //TRotation *rotation = new TRotation();
2008 
2010  rotation->RotateAxes(u, v, n);
2011 
2012  R[0][0] = rotation->XX();
2013  R[0][1] = rotation->XY();
2014  R[0][2] = rotation->XZ();
2015  R[1][0] = rotation->YX();
2016  R[1][1] = rotation->YY();
2017  R[1][2] = rotation->YZ();
2018  R[2][0] = rotation->ZX();
2019  R[2][1] = rotation->ZY();
2020  R[2][2] = rotation->ZZ();
2021  //
2022  // LogDebug("PHGenFitTrkFitter::get_rotation_matrix: TRotation:");
2023  // R.Print();
2024  // cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
2025 
2026  //delete rotation;
2027 
2028  // TMatrixF ROT1(3, 3);
2029  // TMatrixF ROT2(3, 3);
2030  // TMatrixF ROT3(3, 3);
2031  //
2032  // // rotate n along z to xz plane
2033  // float phi = -TMath::ATan2(n.Y(), n.X());
2034  // ROT1[0][0] = cos(phi);
2035  // ROT1[0][1] = -sin(phi);
2036  // ROT1[0][2] = 0;
2037  // ROT1[1][0] = sin(phi);
2038  // ROT1[1][1] = cos(phi);
2039  // ROT1[1][2] = 0;
2040  // ROT1[2][0] = 0;
2041  // ROT1[2][1] = 0;
2042  // ROT1[2][2] = 1;
2043  //
2044  // // rotate n along y to z
2045  // TVector3 n1(n);
2046  // n1.RotateZ(phi);
2047  // float theta = -TMath::ATan2(n1.X(), n1.Z());
2048  // ROT2[0][0] = cos(theta);
2049  // ROT2[0][1] = 0;
2050  // ROT2[0][2] = sin(theta);
2051  // ROT2[1][0] = 0;
2052  // ROT2[1][1] = 1;
2053  // ROT2[1][2] = 0;
2054  // ROT2[2][0] = -sin(theta);
2055  // ROT2[2][1] = 0;
2056  // ROT2[2][2] = cos(theta);
2057  //
2058  // // rotate u along z to x
2059  // TVector3 u2(u);
2060  // u2.RotateZ(phi);
2061  // u2.RotateY(theta);
2062  // float phip = -TMath::ATan2(u2.Y(), u2.X());
2063  // ROT3[0][0] = cos(phip);
2064  // ROT3[0][1] = -sin(phip);
2065  // ROT3[0][2] = 0;
2066  // ROT3[1][0] = sin(phip);
2067  // ROT3[1][1] = cos(phip);
2068  // ROT3[1][2] = 0;
2069  // ROT3[2][0] = 0;
2070  // ROT3[2][1] = 0;
2071  // ROT3[2][2] = 1;
2072  //
2073  // // R: rotation from u,v,n to (z X n), v', z
2074  // R = ROT3 * ROT2 * ROT1;
2075  //
2076  // R.Invert();
2077  // LogDebug("PHGenFitTrkFitter::get_rotation_matrix: Home Brew:");
2078  // R.Print();
2079  // cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
2080  }
2081  catch (...)
2082  {
2083  if (Verbosity() > 0)
2084  LogWarning("Can't get rotation matrix");
2085 
2086  return R;
2087  }
2088 
2089  return R;
2090 }