Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-06 08:22:04 

0001 /*!
0002  *  \file       TpcPrototypeGenFitTrkFitter.C
0003  *  \brief      Refit SvtxTracks with PHGenFit.
0004  *  \details    Refit SvtxTracks with PHGenFit.
0005  *  \author     Haiwang Yu <yuhw@nmsu.edu>
0006  */
0007 
0008 #include "TpcPrototypeGenFitTrkFitter.h"
0009 #include "TpcPrototypeTrack.h"
0010 #include "TpcPrototypeCluster.h"
0011 
0012 #include <trackbase/TrkrCluster.h>  // for TrkrCluster
0013 #include <trackbase/TrkrClusterContainer.h>
0014 #include <trackbase/TrkrDefs.h>
0015 #include <trackbase_historic/SvtxTrack_v1.h>
0016 
0017 #include <trackbase_historic/SvtxTrack.h>
0018 #include <trackbase_historic/SvtxTrackMap.h>
0019 #include <trackbase_historic/SvtxTrackMap_v1.h>
0020 #include <trackbase_historic/SvtxTrackState.h>  // for SvtxTrackState
0021 #include <trackbase_historic/SvtxTrackState_v1.h>
0022 #include <trackbase_historic/SvtxVertex.h>     // for SvtxVertex
0023 #include <trackbase_historic/SvtxVertexMap.h>  // for SvtxVertexMap
0024 #include <trackbase_historic/SvtxVertexMap_v1.h>
0025 #include <trackbase_historic/SvtxVertex_v1.h>
0026 
0027 #include <phgenfit/Fitter.h>
0028 #include <phgenfit/Measurement.h>  // for Measurement
0029 #include <phgenfit/PlanarMeasurement.h>
0030 #include <phgenfit/SpacepointMeasurement.h>
0031 #include <phgenfit/Track.h>
0032 
0033 #include <fun4all/Fun4AllReturnCodes.h>
0034 #include <fun4all/PHTFileServer.h>
0035 #include <fun4all/SubsysReco.h>  // for SubsysReco
0036 
0037 #include <phool/PHCompositeNode.h>
0038 #include <phool/PHIODataNode.h>
0039 #include <phool/PHNode.h>  // for PHNode
0040 #include <phool/PHNodeIterator.h>
0041 #include <phool/PHObject.h>  // for PHObject
0042 #include <phool/getClass.h>
0043 #include <phool/phool.h>
0044 
0045 #include <phfield/PHFieldUtility.h>
0046 #include <phgeom/PHGeomUtility.h>
0047 
0048 #include <GenFit/AbsMeasurement.h>  // for AbsMeasurement
0049 #include <GenFit/EventDisplay.h>    // for EventDisplay
0050 #include <GenFit/Exception.h>       // for Exception
0051 #include <GenFit/GFRaveConverters.h>
0052 #include <GenFit/GFRaveTrackParameters.h>  // for GFRaveTrackParame...
0053 #include <GenFit/GFRaveVertex.h>
0054 #include <GenFit/GFRaveVertexFactory.h>
0055 #include <GenFit/KalmanFitterInfo.h>
0056 #include <GenFit/MeasuredStateOnPlane.h>
0057 #include <GenFit/RKTrackRep.h>
0058 #include <GenFit/Track.h>
0059 #include <GenFit/TrackPoint.h>  // for TrackPoint
0060 
0061 //Rave
0062 #include <rave/ConstantMagneticField.h>
0063 #include <rave/VacuumPropagator.h>  // for VacuumPropagator
0064 #include <rave/VertexFactory.h>
0065 
0066 #include <TClonesArray.h>
0067 #include <TMatrixDSymfwd.h>  // for TMatrixDSym
0068 #include <TMatrixFfwd.h>     // for TMatrixF
0069 #include <TMatrixT.h>        // for TMatrixT, operator*
0070 #include <TMatrixTSym.h>     // for TMatrixTSym
0071 #include <TMatrixTUtils.h>   // for TMatrixTRow
0072 #include <TRotation.h>
0073 #include <TTree.h>
0074 #include <TVector3.h>
0075 #include <TVectorDfwd.h>  // for TVectorD
0076 #include <TVectorT.h>     // for TVectorT
0077 
0078 #include <cassert>
0079 #include <cmath>  // for sqrt, NAN
0080 #include <iostream>
0081 #include <map>
0082 #include <memory>
0083 #include <set>
0084 #include <utility>
0085 #include <vector>
0086 
0087 class PHField;
0088 class TGeoManager;
0089 namespace genfit
0090 {
0091 class AbsTrackRep;
0092 }
0093 
0094 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
0095 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
0096 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
0097 
0098 #define WILD_FLOAT -9999.
0099 
0100 #define _DEBUG_MODE_ 0
0101 
0102 //#define _DEBUG_
0103 
0104 using namespace std;
0105 
0106 class PHRaveVertexFactory
0107 {
0108  public:
0109   //! ctor
0110   PHRaveVertexFactory(const int Verbosity())
0111   {
0112     rave::ConstantMagneticField mfield(0., 0., 0.);  // RAVE use Tesla
0113     _factory = new rave::VertexFactory(mfield, rave::VacuumPropagator(),
0114                                        "default", Verbosity());
0115 
0116     IdGFTrackStateMap_.clear();
0117   }
0118 
0119   //! dotr
0120   ~PHRaveVertexFactory()
0121   {
0122     clearMap();
0123 
0124     delete _factory;
0125   }
0126 
0127   void findVertices(std::vector<genfit::GFRaveVertex*>* vertices,
0128                     const std::vector<genfit::Track*>& tracks, const bool use_beamspot = false)
0129   {
0130     clearMap();
0131 
0132     try
0133     {
0134       genfit::RaveToGFVertices(vertices,
0135                                _factory->create(
0136                                    genfit::GFTracksToTracks(tracks, NULL,
0137                                                             IdGFTrackStateMap_, 0),
0138                                    use_beamspot),
0139                                IdGFTrackStateMap_);
0140     }
0141     catch (genfit::Exception& e)
0142     {
0143       std::cerr << e.what();
0144     }
0145   }
0146 
0147   void findVertices(std::vector<genfit::GFRaveVertex*>* vertices,
0148                     const std::vector<genfit::Track*>& tracks,
0149                     std::vector<genfit::MeasuredStateOnPlane*>& GFStates,
0150                     const bool use_beamspot = false)
0151   {
0152     clearMap();
0153 
0154     try
0155     {
0156       genfit::RaveToGFVertices(vertices,
0157                                _factory->create(
0158                                    genfit::GFTracksToTracks(tracks, &GFStates,
0159                                                             IdGFTrackStateMap_, 0),
0160                                    use_beamspot),
0161                                IdGFTrackStateMap_);
0162     }
0163     catch (genfit::Exception& e)
0164     {
0165       std::cerr << e.what();
0166     }
0167   }
0168 
0169  private:
0170   void clearMap()
0171   {
0172     for (unsigned int i = 0; i < IdGFTrackStateMap_.size(); ++i)
0173       delete IdGFTrackStateMap_[i].state_;
0174 
0175     IdGFTrackStateMap_.clear();
0176   }
0177 
0178   std::map<int, genfit::trackAndState> IdGFTrackStateMap_;
0179 
0180   rave::VertexFactory* _factory;
0181 };
0182 
0183 /*
0184  * Constructor
0185  */
0186 TpcPrototypeGenFitTrkFitter::TpcPrototypeGenFitTrkFitter(const string& name)
0187   : SubsysReco(name)
0188   , _flags(NONE)
0189   , _output_mode(TpcPrototypeGenFitTrkFitter::MakeNewNode)
0190   , _over_write_svtxtrackmap(true)
0191   , _over_write_svtxvertexmap(true)
0192   , _fit_primary_tracks(false)
0193   , _use_truth_vertex(false)
0194   , _fitter(NULL)
0195   , _track_fitting_alg_name("DafRef")
0196   , _primary_pid_guess(2212)
0197   , _fit_min_pT(0.1)
0198   , _vertex_min_ndf(20)
0199   , _vertex_finder(NULL)
0200   , _vertexing_method("avf-smoothing:1")
0201   //  , _truth_container(NULL)
0202   , _clustermap(NULL)
0203   , _trackmap(NULL)
0204   , _vertexmap(NULL)
0205   , _trackmap_refit(NULL)
0206   , _primary_trackmap(NULL)
0207   , _vertexmap_refit(NULL)
0208   , _do_eval(false)
0209   , _eval_outname("TpcPrototypeGenFitTrkFitter_eval.root")
0210   , _eval_tree(NULL)
0211   , _tca_ntrack(-1)
0212   , _tca_particlemap(NULL)
0213   , _tca_vtxmap(NULL)
0214   , _tca_trackmap(NULL)
0215   , _tca_tpctrackmap(nullptr)
0216   , _tca_vertexmap(NULL)
0217   , _tca_trackmap_refit(NULL)
0218   , _tca_primtrackmap(NULL)
0219   , _tca_vertexmap_refit(NULL)
0220   , _do_evt_display(false)
0221 {
0222   Verbosity(0);
0223 
0224   _event = 0;
0225 
0226   _cluster_eval_tree = NULL;
0227   _cluster_eval_tree_x = WILD_FLOAT;
0228   _cluster_eval_tree_y = WILD_FLOAT;
0229   _cluster_eval_tree_z = WILD_FLOAT;
0230   _cluster_eval_tree_gx = WILD_FLOAT;
0231   _cluster_eval_tree_gy = WILD_FLOAT;
0232   _cluster_eval_tree_gz = WILD_FLOAT;
0233 }
0234 
0235 /*
0236  * Init
0237  */
0238 int TpcPrototypeGenFitTrkFitter::Init(PHCompositeNode* topNode)
0239 {
0240   //    CreateNodes(topNode);
0241 
0242   return Fun4AllReturnCodes::EVENT_OK;
0243 }
0244 
0245 /*
0246  * Init run
0247  */
0248 int TpcPrototypeGenFitTrkFitter::InitRun(PHCompositeNode* topNode)
0249 {
0250   CreateNodes(topNode);
0251 
0252   TGeoManager* tgeo_manager = PHGeomUtility::GetTGeoManager(topNode);
0253   PHField* field = PHFieldUtility::GetFieldMapNode(nullptr, topNode);
0254 
0255   //_fitter = new PHGenFit::Fitter("sPHENIX_Geo.root","sPHENIX.2d.root", 1.4 / 1.5);
0256   _fitter = PHGenFit::Fitter::getInstance(tgeo_manager,
0257                                           field, _track_fitting_alg_name,
0258                                           "RKTrackRep", _do_evt_display);
0259 
0260   if (!_fitter)
0261   {
0262     cerr << PHWHERE << endl;
0263     return Fun4AllReturnCodes::ABORTRUN;
0264   }
0265 
0266   _fitter->set_verbosity(Verbosity());
0267 
0268   //LogDebug(genfit::FieldManager::getInstance()->getFieldVal(TVector3(0, 0, 0)).Z());
0269 
0270   _vertex_finder = new genfit::GFRaveVertexFactory(Verbosity());
0271   //_vertex_finder->setMethod("kalman-smoothing:1"); //! kalman-smoothing:1 is the defaul method
0272   _vertex_finder->setMethod(_vertexing_method.data());
0273   //_vertex_finder->setBeamspot();
0274 
0275   //_vertex_finder = new PHRaveVertexFactory(Verbosity());
0276 
0277   if (!_vertex_finder)
0278   {
0279     cerr << PHWHERE << endl;
0280     return Fun4AllReturnCodes::ABORTRUN;
0281   }
0282 
0283   if (_do_eval)
0284   {
0285     if (Verbosity() >= 1)
0286       cout << PHWHERE << " Openning file: " << _eval_outname << endl;
0287     PHTFileServer::get().open(_eval_outname, "RECREATE");
0288     init_eval_tree();
0289   }
0290 
0291   return Fun4AllReturnCodes::EVENT_OK;
0292 }
0293 /*
0294  * process_event():
0295  *  Call user instructions for every event.
0296  *  This function contains the analysis structure.
0297  *
0298  */
0299 int TpcPrototypeGenFitTrkFitter::process_event(PHCompositeNode* topNode)
0300 {
0301   _event++;
0302 
0303   if (Verbosity() > 1)
0304     std::cout << PHWHERE << "Events processed: " << _event << std::endl;
0305   //    if (_event % 1000 == 0)
0306   //        cout << PHWHERE << "Events processed: " << _event << endl;
0307 
0308   //cout << "Start TpcPrototypeGenFitTrkFitter::process_event" << endl;
0309 
0310   GetNodes(topNode);
0311 
0312   //! stands for Refit_GenFit_Tracks
0313   vector<genfit::Track*> rf_gf_tracks;
0314   rf_gf_tracks.clear();
0315 
0316   vector<std::shared_ptr<PHGenFit::Track> > rf_phgf_tracks;
0317   //  rf_phgf_tracks.clear();
0318 
0319   map<unsigned int, unsigned int> svtxtrack_genfittrack_map;
0320 
0321   if (_trackmap_refit)
0322     _trackmap_refit->empty();
0323 
0324   // _trackmap is SvtxTrackMap from the node tree
0325   for (SvtxTrackMap::Iter iter = _trackmap->begin(); iter != _trackmap->end();
0326        ++iter)
0327   {
0328     SvtxTrack* svtx_track = iter->second;
0329     if (Verbosity() > 50)
0330     {
0331       cout << "   process SVTXTrack " << iter->first << endl;
0332       svtx_track->identify();
0333     }
0334     if (!svtx_track)
0335       continue;
0336     if (!(svtx_track->get_pt() > _fit_min_pT))
0337       continue;
0338 
0339     //! stands for Refit_PHGenFit_Track
0340     std::shared_ptr<PHGenFit::Track> rf_phgf_track = ReFitTrack(topNode, svtx_track);
0341 
0342     if (rf_phgf_track)
0343     {
0344       svtxtrack_genfittrack_map[svtx_track->get_id()] =
0345           rf_phgf_tracks.size();
0346       rf_phgf_tracks.push_back(rf_phgf_track);
0347       if (rf_phgf_track->get_ndf() > _vertex_min_ndf)
0348         rf_gf_tracks.push_back(rf_phgf_track->getGenFitTrack());
0349     }
0350   }
0351 
0352   /*
0353      * add tracks to event display
0354      * needs to make copied for smart ptrs will be destroied even
0355      * there are still references in TGeo::EventView
0356      */
0357   if (_do_evt_display)
0358   {
0359     //search for unused clusters
0360 
0361     assert(_clustermap);
0362     //unused clusters
0363     set<TrkrDefs::cluskey> cluster_ids;
0364     //all clusters
0365     auto cluster_range = _clustermap->getClusters(TrkrDefs::tpcId);
0366     for (auto iter = cluster_range.first; iter != cluster_range.second; ++iter)
0367     {
0368       cluster_ids.insert(iter->first);
0369     }
0370     // minus used clusters
0371     for (SvtxTrackMap::Iter iter = _trackmap->begin(); iter != _trackmap->end();
0372          ++iter)
0373     {
0374       SvtxTrack* svtx_track = iter->second;
0375       for (auto iter = svtx_track->begin_cluster_keys();
0376            iter != svtx_track->end_cluster_keys(); ++iter)
0377       {
0378         cluster_ids.erase(*iter);
0379       }
0380     }
0381 
0382     //    // add tracks
0383     vector<genfit::Track*> copy;
0384     for (genfit::Track* t : rf_gf_tracks)
0385     {
0386       copy.push_back(new genfit::Track(*t));
0387     }
0388     //
0389 
0390     //add clusters
0391     for (const auto cluster_id : cluster_ids)
0392     {
0393       const TrkrCluster* cluster =
0394           _clustermap->findCluster(cluster_id);
0395       assert(cluster);
0396 
0397       std::shared_ptr<PHGenFit::Track> cluster_holder = DisplayCluster(cluster);
0398       if (cluster_holder.get())
0399         copy.push_back(new genfit::Track(*cluster_holder->getGenFitTrack()));
0400     }
0401 
0402     _fitter->getEventDisplay()->addEvent(copy);
0403   }
0404 
0405   for (SvtxTrackMap::Iter iter = _trackmap->begin(); iter != _trackmap->end();)
0406   {
0407     std::shared_ptr<PHGenFit::Track> rf_phgf_track = NULL;
0408 
0409     if (svtxtrack_genfittrack_map.find(iter->second->get_id()) != svtxtrack_genfittrack_map.end())
0410     {
0411       unsigned int itrack =
0412           svtxtrack_genfittrack_map[iter->second->get_id()];
0413       rf_phgf_track = rf_phgf_tracks[itrack];
0414     }
0415 
0416     if (rf_phgf_track)
0417     {
0418       //FIXME figure out which vertex to use.
0419       SvtxVertex* vertex = NULL;
0420       if (_over_write_svtxvertexmap)
0421       {
0422         if (_vertexmap->size() > 0)
0423           vertex = _vertexmap->get(0);
0424         //cout << PHWHERE << "        will use vertex " << vertex->get_x() << "  " << vertex->get_y() << "  " << vertex->get_z() << endl;
0425       }
0426       else
0427       {
0428         if (_vertexmap_refit->size() > 0)
0429           vertex = _vertexmap_refit->get(0);
0430       }
0431 
0432       std::shared_ptr<SvtxTrack> rf_track = MakeSvtxTrack(iter->second, rf_phgf_track,
0433                                                           vertex);
0434       if (!rf_track)
0435       {
0436         //if (_output_mode == OverwriteOriginalNode)
0437         if (_over_write_svtxtrackmap)
0438         {
0439           auto key = iter->first;
0440           ++iter;
0441           _trackmap->erase(key);
0442           continue;
0443         }
0444       }
0445 
0446       //            delete vertex;//DEBUG
0447 
0448       //            rf_phgf_tracks.push_back(rf_phgf_track);
0449       //            rf_gf_tracks.push_back(rf_phgf_track->getGenFitTrack());
0450 
0451       if (!(_over_write_svtxtrackmap) || _output_mode == DebugMode)
0452         if (_trackmap_refit)
0453         {
0454           _trackmap_refit->insert(rf_track.get());
0455           //                    delete rf_track;
0456         }
0457 
0458       if (_over_write_svtxtrackmap || _output_mode == DebugMode)
0459       {
0460         *(dynamic_cast<SvtxTrack_v1*>(iter->second)) =
0461             *(dynamic_cast<SvtxTrack_v1*>(rf_track.get()));
0462         //              delete rf_track;
0463       }
0464     }
0465     else
0466     {
0467       if (_over_write_svtxtrackmap)
0468       {
0469         auto key = iter->first;
0470         ++iter;
0471         _trackmap->erase(key);
0472         continue;
0473       }
0474     }
0475 
0476     ++iter;
0477   }
0478   // Need to keep tracks if _do_evt_display
0479   if (!_do_evt_display)
0480   {
0481     rf_phgf_tracks.clear();
0482   }
0483 
0484   if (_do_eval)
0485   {
0486     fill_eval_tree(topNode);
0487   }
0488 #ifdef _DEBUG_
0489   cout << __LINE__ << endl;
0490 #endif
0491   return Fun4AllReturnCodes::EVENT_OK;
0492 }
0493 
0494 /*
0495  * End
0496  */
0497 int TpcPrototypeGenFitTrkFitter::End(PHCompositeNode* topNode)
0498 {
0499   if (_do_eval)
0500   {
0501     if (Verbosity() >= 1)
0502       cout << PHWHERE << " Writing to file: " << _eval_outname << endl;
0503     PHTFileServer::get().cd(_eval_outname);
0504     _eval_tree->Write();
0505     _cluster_eval_tree->Write();
0506   }
0507 
0508   if (_do_evt_display)
0509   {
0510     //    if(opts[i] == 'A') drawAutoScale_ = true;
0511     //    if(opts[i] == 'B') drawBackward_ = true;
0512     //    if(opts[i] == 'D') drawDetectors_ = true;
0513     //    if(opts[i] == 'E') drawErrors_ = true;
0514     //    if(opts[i] == 'F') drawForward_ = true;
0515     //    if(opts[i] == 'H') drawHits_ = true;
0516     //    if(opts[i] == 'M') drawTrackMarkers_ = true;
0517     //    if(opts[i] == 'P') drawPlanes_ = true;
0518     //    if(opts[i] == 'S') drawScaleMan_ = true;
0519     //    if(opts[i] == 'T') drawTrack_ = true;
0520     //    if(opts[i] == 'X') drawSilent_ = true;
0521     //    if(opts[i] == 'G') drawGeometry_ = true;
0522     _fitter->getEventDisplay()->setOptions("ADEHPTG");
0523 
0524     _fitter->displayEvent();
0525   }
0526   return Fun4AllReturnCodes::EVENT_OK;
0527 }
0528 
0529 /*
0530  * dtor
0531  */
0532 TpcPrototypeGenFitTrkFitter::~TpcPrototypeGenFitTrkFitter()
0533 {
0534   delete _fitter;
0535   delete _vertex_finder;
0536 }
0537 
0538 /*
0539  * fill_eval_tree():
0540  */
0541 void TpcPrototypeGenFitTrkFitter::fill_eval_tree(PHCompositeNode* topNode)
0542 {
0543   //! Make sure to reset all the TTree variables before trying to set them.
0544   reset_eval_variables();
0545 
0546   int i = 0;
0547   for (SvtxTrackMap::ConstIter itr = _trackmap->begin();
0548        itr != _trackmap->end(); ++itr)
0549   {
0550     //    new ((*_tca_trackmap)[i])(SvtxTrack_v1)(
0551     //        *dynamic_cast<SvtxTrack_v1*>(itr->second));
0552 
0553     new ((*_tca_tpctrackmap)[i])(TpcPrototypeTrack)(*(MakeTpcPrototypeTrack(itr->second)));
0554 
0555     i++;
0556   }
0557   _tca_ntrack = i;
0558 
0559   i = 0;
0560   if (_vertexmap)
0561     for (SvtxVertexMap::ConstIter itr = _vertexmap->begin();
0562          itr != _vertexmap->end(); ++itr)
0563       new ((*_tca_vertexmap)[i++])(SvtxVertex_v1)(
0564           *dynamic_cast<SvtxVertex_v1*>(itr->second));
0565 
0566   if (_trackmap_refit)
0567   {
0568     i = 0;
0569     for (SvtxTrackMap::ConstIter itr = _trackmap_refit->begin();
0570          itr != _trackmap_refit->end(); ++itr)
0571       new ((*_tca_trackmap_refit)[i++])(SvtxTrack_v1)(
0572           *dynamic_cast<SvtxTrack_v1*>(itr->second));
0573   }
0574 
0575   if (_fit_primary_tracks)
0576   {
0577     i = 0;
0578     for (SvtxTrackMap::ConstIter itr = _primary_trackmap->begin();
0579          itr != _primary_trackmap->end(); ++itr)
0580       new ((*_tca_primtrackmap)[i++])(SvtxTrack_v1)(
0581           *dynamic_cast<SvtxTrack_v1*>(itr->second));
0582   }
0583 
0584   if (_vertexmap_refit)
0585   {
0586     i = 0;
0587     for (SvtxVertexMap::ConstIter itr = _vertexmap_refit->begin();
0588          itr != _vertexmap_refit->end(); ++itr)
0589       new ((*_tca_vertexmap_refit)[i++])(SvtxVertex_v1)(
0590           *dynamic_cast<SvtxVertex_v1*>(itr->second));
0591   }
0592 
0593   _eval_tree->Fill();
0594 
0595   return;
0596 }
0597 
0598 /*
0599  * init_eval_tree
0600  */
0601 void TpcPrototypeGenFitTrkFitter::init_eval_tree()
0602 {
0603   if (!_tca_particlemap)
0604     _tca_particlemap = new TClonesArray("PHG4Particlev2");
0605   if (!_tca_vtxmap)
0606     _tca_vtxmap = new TClonesArray("PHG4VtxPointv1");
0607 
0608   if (!_tca_trackmap)
0609     _tca_trackmap = new TClonesArray("SvtxTrack_v1");
0610   if (!_tca_tpctrackmap)
0611     _tca_tpctrackmap = new TClonesArray("TpcPrototypeTrack");
0612   if (!_tca_vertexmap)
0613     _tca_vertexmap = new TClonesArray("SvtxVertex_v1");
0614   if (!_tca_trackmap_refit)
0615     _tca_trackmap_refit = new TClonesArray("SvtxTrack_v1");
0616   if (_fit_primary_tracks)
0617     if (!_tca_primtrackmap)
0618       _tca_primtrackmap = new TClonesArray("SvtxTrack_v1");
0619   if (!_tca_vertexmap_refit)
0620     _tca_vertexmap_refit = new TClonesArray("SvtxVertex_v1");
0621 
0622   //! create TTree
0623   _eval_tree = new TTree("T", "TpcPrototypeGenFitTrkFitter Evaluation");
0624   _eval_tree->Branch("nTrack", &_tca_ntrack, "nTrack/I");
0625 
0626   //  _eval_tree->Branch("PrimaryParticle", _tca_particlemap);
0627   //  _eval_tree->Branch("TruthVtx", _tca_vtxmap);
0628 
0629   _eval_tree->Branch("SvtxTrack", _tca_trackmap);
0630   _eval_tree->Branch("TPCTrack", _tca_tpctrackmap);
0631   //  _eval_tree->Branch("SvtxVertex", _tca_vertexmap);
0632   //  _eval_tree->Branch("SvtxTrackRefit", _tca_trackmap_refit);
0633   if (_fit_primary_tracks)
0634     _eval_tree->Branch("PrimSvtxTrack", _tca_primtrackmap);
0635   //  _eval_tree->Branch("SvtxVertexRefit", _tca_vertexmap_refit);
0636 
0637   _cluster_eval_tree = new TTree("cluster_eval", "cluster eval tree");
0638   _cluster_eval_tree->Branch("x", &_cluster_eval_tree_x, "x/F");
0639   _cluster_eval_tree->Branch("y", &_cluster_eval_tree_y, "y/F");
0640   _cluster_eval_tree->Branch("z", &_cluster_eval_tree_z, "z/F");
0641   _cluster_eval_tree->Branch("gx", &_cluster_eval_tree_gx, "gx/F");
0642   _cluster_eval_tree->Branch("gy", &_cluster_eval_tree_gy, "gy/F");
0643   _cluster_eval_tree->Branch("gz", &_cluster_eval_tree_gz, "gz/F");
0644 }
0645 
0646 /*
0647  * reset_eval_variables():
0648  *  Reset all the tree variables to their default values.
0649  *  Needs to be called at the start of every event
0650  */
0651 void TpcPrototypeGenFitTrkFitter::reset_eval_variables()
0652 {
0653   _tca_particlemap->Clear();
0654   _tca_vtxmap->Clear();
0655 
0656   _tca_ntrack = -1;
0657   _tca_trackmap->Clear();
0658   _tca_tpctrackmap->Clear();
0659   _tca_vertexmap->Clear();
0660   _tca_trackmap_refit->Clear();
0661   if (_fit_primary_tracks)
0662     _tca_primtrackmap->Clear();
0663   _tca_vertexmap_refit->Clear();
0664 
0665   _cluster_eval_tree_x = WILD_FLOAT;
0666   _cluster_eval_tree_y = WILD_FLOAT;
0667   _cluster_eval_tree_z = WILD_FLOAT;
0668   _cluster_eval_tree_gx = WILD_FLOAT;
0669   _cluster_eval_tree_gy = WILD_FLOAT;
0670   _cluster_eval_tree_gz = WILD_FLOAT;
0671 }
0672 
0673 int TpcPrototypeGenFitTrkFitter::CreateNodes(PHCompositeNode* topNode)
0674 {
0675   // create nodes...
0676   PHNodeIterator iter(topNode);
0677 
0678   PHCompositeNode* dstNode = static_cast<PHCompositeNode*>(iter.findFirst(
0679       "PHCompositeNode", "DST"));
0680   if (!dstNode)
0681   {
0682     cerr << PHWHERE << "DST Node missing, doing nothing." << endl;
0683     return Fun4AllReturnCodes::ABORTEVENT;
0684   }
0685   PHNodeIterator iter_dst(dstNode);
0686 
0687   // Create the SVTX node
0688   PHCompositeNode* tb_node = dynamic_cast<PHCompositeNode*>(iter_dst.findFirst(
0689       "PHCompositeNode", "SVTX"));
0690   if (!tb_node)
0691   {
0692     tb_node = new PHCompositeNode("SVTX");
0693     dstNode->addNode(tb_node);
0694     if (Verbosity() > 0)
0695       cout << "SVTX node added" << endl;
0696   }
0697 
0698   if (!(_over_write_svtxtrackmap) || _output_mode == DebugMode)
0699   {
0700     _trackmap_refit = new SvtxTrackMap_v1;
0701     PHIODataNode<PHObject>* tracks_node = new PHIODataNode<PHObject>(
0702         _trackmap_refit, "SvtxTrackMapRefit", "PHObject");
0703     tb_node->addNode(tracks_node);
0704     if (Verbosity() > 0)
0705       cout << "Svtx/SvtxTrackMapRefit node added" << endl;
0706   }
0707 
0708   if (_fit_primary_tracks)
0709   {
0710     _primary_trackmap = new SvtxTrackMap_v1;
0711     PHIODataNode<PHObject>* primary_tracks_node =
0712         new PHIODataNode<PHObject>(_primary_trackmap, "PrimaryTrackMap",
0713                                    "PHObject");
0714     tb_node->addNode(primary_tracks_node);
0715     if (Verbosity() > 0)
0716       cout << "Svtx/PrimaryTrackMap node added" << endl;
0717   }
0718 
0719   if (!(_over_write_svtxvertexmap))
0720   {
0721     _vertexmap_refit = new SvtxVertexMap_v1;
0722     PHIODataNode<PHObject>* vertexes_node = new PHIODataNode<PHObject>(
0723         _vertexmap_refit, "SvtxVertexMapRefit", "PHObject");
0724     tb_node->addNode(vertexes_node);
0725     if (Verbosity() > 0)
0726       cout << "Svtx/SvtxVertexMapRefit node added" << endl;
0727   }
0728   else if (!findNode::getClass<SvtxVertexMap>(topNode, "SvtxVertexMap"))
0729   {
0730     _vertexmap = new SvtxVertexMap_v1;
0731     PHIODataNode<PHObject>* vertexes_node = new PHIODataNode<PHObject>(
0732         _vertexmap, "SvtxVertexMap", "PHObject");
0733     tb_node->addNode(vertexes_node);
0734     if (Verbosity() > 0)
0735       cout << "Svtx/SvtxVertexMap node added" << endl;
0736   }
0737 
0738   return Fun4AllReturnCodes::EVENT_OK;
0739 }
0740 
0741 /*
0742  * GetNodes():
0743  *  Get all the all the required nodes off the node tree
0744  */
0745 int TpcPrototypeGenFitTrkFitter::GetNodes(PHCompositeNode* topNode)
0746 {
0747   //DST objects
0748   //Truth container
0749   //  _truth_container = findNode::getClass<PHG4TruthInfoContainer>(topNode,
0750   //                                                                "G4TruthInfo");
0751   //  if (!_truth_container && _event < 2)
0752   //  {
0753   //    cout << PHWHERE << " PHG4TruthInfoContainer node not found on node tree"
0754   //         << endl;
0755   //    return Fun4AllReturnCodes::ABORTEVENT;
0756   //  }
0757 
0758   // Input Svtx Clusters
0759   //_clustermap = findNode::getClass<SvtxClusterMap>(topNode, "SvtxClusterMap");
0760   _clustermap = findNode::getClass<TrkrClusterContainer>(topNode, "TRKR_CLUSTER");
0761   if (!_clustermap && _event < 2)
0762   {
0763     cout << PHWHERE << " TRKR_CLUSTER node not found on node tree"
0764          << endl;
0765     return Fun4AllReturnCodes::ABORTEVENT;
0766   }
0767 
0768   // Input Svtx Tracks
0769   _trackmap = findNode::getClass<SvtxTrackMap>(topNode, "SvtxTrackMap");
0770   if (!_trackmap && _event < 2)
0771   {
0772     cout << PHWHERE << " SvtxTrackMap node not found on node tree"
0773          << endl;
0774     return Fun4AllReturnCodes::ABORTEVENT;
0775   }
0776 
0777   // Input Svtx Vertices
0778   _vertexmap = findNode::getClass<SvtxVertexMap>(topNode, "SvtxVertexMap");
0779   if (!_vertexmap && _event < 2)
0780   {
0781     cout << PHWHERE << " SvtxVertexrMap node not found on node tree"
0782          << endl;
0783     return Fun4AllReturnCodes::ABORTEVENT;
0784   }
0785 
0786   // Output Svtx Tracks
0787   if (!(_over_write_svtxtrackmap) || _output_mode == DebugMode)
0788   {
0789     _trackmap_refit = findNode::getClass<SvtxTrackMap>(topNode,
0790                                                        "SvtxTrackMapRefit");
0791     if (!_trackmap_refit && _event < 2)
0792     {
0793       cout << PHWHERE << " SvtxTrackMapRefit node not found on node tree"
0794            << endl;
0795       return Fun4AllReturnCodes::ABORTEVENT;
0796     }
0797   }
0798 
0799   // Output Primary Svtx Tracks
0800   if (_fit_primary_tracks)
0801   {
0802     _primary_trackmap = findNode::getClass<SvtxTrackMap>(topNode,
0803                                                          "PrimaryTrackMap");
0804     if (!_primary_trackmap && _event < 2)
0805     {
0806       cout << PHWHERE << " PrimaryTrackMap node not found on node tree"
0807            << endl;
0808       return Fun4AllReturnCodes::ABORTEVENT;
0809     }
0810   }
0811 
0812   // Output Svtx Vertices
0813   if (!(_over_write_svtxvertexmap))
0814   {
0815     _vertexmap_refit = findNode::getClass<SvtxVertexMap>(topNode,
0816                                                          "SvtxVertexMapRefit");
0817     if (!_vertexmap_refit && _event < 2)
0818     {
0819       cout << PHWHERE << " SvtxVertexMapRefit node not found on node tree"
0820            << endl;
0821       return Fun4AllReturnCodes::ABORTEVENT;
0822     }
0823   }
0824 
0825   return Fun4AllReturnCodes::EVENT_OK;
0826 }
0827 
0828 std::shared_ptr<PHGenFit::Track> TpcPrototypeGenFitTrkFitter::DisplayCluster(const TrkrCluster* cluster)
0829 {
0830   assert(cluster);
0831 
0832   if (Verbosity() >= 1)
0833   {
0834     cout << __PRETTY_FUNCTION__ << ": process cluster: ";
0835     cluster->identify();
0836   }
0837 
0838   // prepare seed
0839   TVector3 seed_mom(100, 0, 0);
0840   TVector3 seed_pos(0, 0, 0);
0841   TMatrixDSym seed_cov(6);
0842   for (int i = 0; i < 6; i++)
0843   {
0844     for (int j = 0; j < 6; j++)
0845     {
0846       seed_cov[i][j] = 100.;
0847     }
0848   }
0849 
0850   // Create measurements
0851   std::vector<PHGenFit::Measurement*> measurements;
0852 
0853   TrkrDefs::cluskey cluster_key = cluster->getClusKey();
0854 
0855   TVector3 pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
0856 
0857   seed_mom.SetPhi(pos.Phi());
0858   seed_mom.SetTheta(pos.Theta());
0859 
0860   seed_pos = pos;
0861 
0862   //TODO use u, v explicitly?
0863   TVector3 n(cluster->getPosition(0), cluster->getPosition(1), 0);
0864 
0865   for (int radial_shift = -1; radial_shift <= 1; ++radial_shift)
0866   {
0867     const double new_radial = pos.Perp() + radial_shift * 0.1;
0868     TVector3 new_pos(pos);
0869     new_pos.SetPerp(new_radial);
0870 
0871     //------------------------------
0872     // new
0873 
0874     // Replace n for the silicon subsystems
0875 
0876     // get the trkrid
0877     int layer = TrkrDefs::getLayer(cluster_key);
0878 
0879     // end new
0880     //-----------------
0881 
0882     PHGenFit::Measurement* meas = new PHGenFit::PlanarMeasurement(new_pos, n,
0883                                                                   cluster->getRPhiError(), cluster->getZError());
0884 
0885     if (Verbosity() > 50)
0886     {
0887       cout << "Add meas layer " << layer << " cluskey " << cluster_key
0888            << endl
0889            << " pos.X " << pos.X() << " pos.Y " << pos.Y() << " pos.Z " << pos.Z()
0890            << "  n.X " << n.X() << " n.Y " << n.Y()
0891            << " RPhiErr " << cluster->getRPhiError()
0892            << " ZErr " << cluster->getZError()
0893            << endl;
0894     }
0895     measurements.push_back(meas);
0896   }
0897 
0898   /*!
0899    * mu+: -13
0900    * mu-: 13
0901    * pi+: 211
0902    * pi-: -211
0903    * e-:  11
0904    * e+:  -11
0905    */
0906   //TODO Add multiple TrackRep choices.
0907   //int pid = 211;
0908   genfit::AbsTrackRep* rep = new genfit::RKTrackRep(-13);
0909   std::shared_ptr<PHGenFit::Track> track(new PHGenFit::Track(rep, seed_pos, seed_mom,
0910                                                              seed_cov));
0911 
0912   //TODO unsorted measurements, should use sorted ones?
0913   track->addMeasurements(measurements);
0914 
0915   //  if (measurements.size()==1) return track;
0916 
0917   /*!
0918    *  Fit the track
0919    *  ret code 0 means 0 error or good status
0920    */
0921   if (_fitter->processTrack(track.get(), false) != 0)
0922   {
0923     if (Verbosity() >= 1)
0924     {
0925       LogWarning("Track fitting failed");
0926       cout << __PRETTY_FUNCTION__ << ": failed cluster build: track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
0927            << " mom.X " << track->get_mom().X()
0928            << " mom.Y " << track->get_mom().Y()
0929            << " mom.Z " << track->get_mom().Z()
0930            << endl;
0931     }
0932     //delete track;
0933     return nullptr;
0934   }
0935 
0936   if (Verbosity() > 50)
0937     cout << " track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
0938          << " mom.X " << track->get_mom().X()
0939          << " mom.Y " << track->get_mom().Y()
0940          << " mom.Z " << track->get_mom().Z()
0941          << endl;
0942 
0943   return track;
0944 }
0945 
0946 //struct CompMeasurementByR {
0947 //  bool operator() (PHGenFit::Measurement *m1,PHGenFit::Measurement *m2) {
0948 //    float x1 = m1->getMeasurement()
0949 //
0950 //    return (i<j);}
0951 //} myobject;
0952 
0953 /*
0954  * fit track with SvtxTrack as input seed.
0955  * \param intrack Input SvtxTrack
0956  * \param invertex Input Vertex, if fit track as a primary vertex
0957  */
0958 //PHGenFit::Track* TpcPrototypeGenFitTrkFitter::ReFitTrack(PHCompositeNode *topNode, const SvtxTrack* intrack,
0959 std::shared_ptr<PHGenFit::Track> TpcPrototypeGenFitTrkFitter::ReFitTrack(PHCompositeNode* topNode, const SvtxTrack* intrack,
0960                                                                          const SvtxVertex* invertex)
0961 {
0962   //std::shared_ptr<PHGenFit::Track> empty_track(NULL);
0963   if (!intrack)
0964   {
0965     cerr << PHWHERE << " Input SvtxTrack is NULL!" << endl;
0966     return NULL;
0967   }
0968 
0969   // prepare seed
0970   TVector3 seed_mom(100, 0, 0);
0971   TVector3 seed_pos(0, 0, 0);
0972   TMatrixDSym seed_cov(6);
0973   for (int i = 0; i < 6; i++)
0974   {
0975     for (int j = 0; j < 6; j++)
0976     {
0977       seed_cov[i][j] = 100.;
0978     }
0979   }
0980 
0981   // Create measurements
0982   std::vector<PHGenFit::Measurement*> measurements;
0983 
0984   //! 1000 is a arbitrary number for now
0985   const double vertex_chi2_over_dnf_cut = 1000;
0986   const double vertex_cov_element_cut = 10000;  //arbitrary cut cm*cm
0987 
0988   if (invertex and invertex->size_tracks() > 1 and invertex->get_chisq() / invertex->get_ndof() < vertex_chi2_over_dnf_cut)
0989   {
0990     TVector3 pos(invertex->get_x(), invertex->get_y(), invertex->get_z());
0991     TMatrixDSym cov(3);
0992     cov.Zero();
0993     bool is_vertex_cov_sane = true;
0994     for (unsigned int i = 0; i < 3; i++)
0995       for (unsigned int j = 0; j < 3; j++)
0996       {
0997         cov(i, j) = invertex->get_error(i, j);
0998 
0999         if (i == j)
1000         {
1001           if (!(invertex->get_error(i, j) > 0 and invertex->get_error(i, j) < vertex_cov_element_cut))
1002             is_vertex_cov_sane = false;
1003         }
1004       }
1005 
1006     if (is_vertex_cov_sane)
1007     {
1008       PHGenFit::Measurement* meas = new PHGenFit::SpacepointMeasurement(
1009           pos, cov);
1010       measurements.push_back(meas);
1011       if (Verbosity() >= 2)
1012       {
1013         meas->getMeasurement()->Print();
1014       }
1015     }
1016   }
1017 
1018   // sort clusters with radius before fitting
1019   if (Verbosity() > 20) intrack->identify();
1020   std::map<float, TrkrDefs::cluskey> m_r_cluster_id;
1021   for (auto iter = intrack->begin_cluster_keys();
1022        iter != intrack->end_cluster_keys(); ++iter)
1023   {
1024     TrkrDefs::cluskey cluster_key = *iter;
1025     TrkrCluster* cluster = _clustermap->findCluster(cluster_key);
1026     float x = cluster->getPosition(0);
1027     float y = cluster->getPosition(1);
1028     float r = sqrt(x * x + y * y);
1029     m_r_cluster_id.insert(std::pair<float, TrkrDefs::cluskey>(r, cluster_key));
1030     int layer_out = TrkrDefs::getLayer(cluster_key);
1031     if (Verbosity() > 20) cout << "    Layer " << layer_out << " cluster " << cluster_key << " radius " << r << endl;
1032   }
1033 
1034   for (auto iter = m_r_cluster_id.begin();
1035        iter != m_r_cluster_id.end();
1036        ++iter)
1037   {
1038     TrkrDefs::cluskey cluster_key = iter->second;
1039     TrkrCluster* cluster = _clustermap->findCluster(cluster_key);
1040     if (!cluster)
1041     {
1042       LogError("No cluster Found!");
1043       continue;
1044     }
1045 
1046     TVector3 pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
1047 
1048     seed_mom.SetPhi(pos.Phi());
1049     seed_mom.SetTheta(pos.Theta());
1050 
1051     //TODO use u, v explicitly?
1052     TVector3 n(cluster->getPosition(0), cluster->getPosition(1), 0);
1053 
1054     //------------------------------
1055     // new
1056 
1057     // Replace n for the silicon subsystems
1058 
1059     // get the trkrid
1060     unsigned int trkrid = TrkrDefs::getTrkrId(cluster_key);
1061     int layer = TrkrDefs::getLayer(cluster_key);
1062 
1063     if (trkrid == TrkrDefs::mvtxId)
1064     {
1065       assert(0);
1066     }
1067     else if (trkrid == TrkrDefs::inttId)
1068     {
1069       assert(0);
1070     }
1071     // end new
1072     //-----------------
1073 
1074     PHGenFit::Measurement* meas = new PHGenFit::PlanarMeasurement(pos, n,
1075                                                                   cluster->getRPhiError(), cluster->getZError());
1076 
1077     if (Verbosity() > 50)
1078     {
1079       cout << "Add meas layer " << layer << " cluskey " << cluster_key
1080            << endl
1081            << " pos.X " << pos.X() << " pos.Y " << pos.Y() << " pos.Z " << pos.Z()
1082            << "  n.X " << n.X() << " n.Y " << n.Y()
1083            << " RPhiErr " << cluster->getRPhiError()
1084            << " ZErr " << cluster->getZError()
1085            << endl;
1086     }
1087     measurements.push_back(meas);
1088   }
1089 
1090   /*!
1091      * mu+: -13
1092      * mu-: 13
1093      * pi+: 211
1094      * pi-: -211
1095      * e-:  11
1096      * e+:  -11
1097      */
1098   //TODO Add multiple TrackRep choices.
1099   //int pid = 211;
1100   genfit::AbsTrackRep* rep = new genfit::RKTrackRep(_primary_pid_guess);
1101   std::shared_ptr<PHGenFit::Track> track(new PHGenFit::Track(rep, seed_pos, seed_mom,
1102                                                              seed_cov));
1103   track->addMeasurements(measurements);
1104 
1105   //  if (measurements.size()==1) return track;
1106 
1107   /*!
1108      *  Fit the track
1109      *  ret code 0 means 0 error or good status
1110      */
1111   if (_fitter->processTrack(track.get(), false) != 0)
1112   {
1113     if (Verbosity() >= 1)
1114     {
1115       LogWarning("Track fitting failed");
1116       cout << __PRETTY_FUNCTION__ << " track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
1117            << " mom.X " << track->get_mom().X()
1118            << " mom.Y " << track->get_mom().Y()
1119            << " mom.Z " << track->get_mom().Z()
1120            << endl;
1121     }
1122     //delete track;
1123     return nullptr;
1124   }
1125 
1126   if (Verbosity() > 50)
1127     cout << " track->getChisq() " << track->get_chi2() << " get_ndf " << track->get_ndf()
1128          << " mom.X " << track->get_mom().X()
1129          << " mom.Y " << track->get_mom().Y()
1130          << " mom.Z " << track->get_mom().Z()
1131          << endl;
1132 
1133   return track;
1134 }
1135 
1136 shared_ptr<TpcPrototypeTrack> TpcPrototypeGenFitTrkFitter::MakeTpcPrototypeTrack(const SvtxTrack* svtxtrack)
1137 {
1138   assert(svtxtrack);
1139   if (Verbosity() >= 1)
1140   {
1141     cout << __PRETTY_FUNCTION__ << " refit ";
1142     svtxtrack->identify();
1143   }
1144 
1145   shared_ptr<TpcPrototypeTrack> track(new TpcPrototypeTrack);
1146   track->event = _event;
1147   track->trackID = svtxtrack->get_id();
1148   track->chisq = svtxtrack->get_chisq();
1149   track->ndf = svtxtrack->get_ndf();
1150   track->px = svtxtrack->get_px();
1151   track->py = svtxtrack->get_py();
1152   track->pz = svtxtrack->get_pz();
1153   track->x = svtxtrack->get_x();
1154   track->y = svtxtrack->get_y();
1155   track->z = svtxtrack->get_z();
1156 
1157   track->nCluster = 0;
1158 
1159   // sort intput cluters to one per layer
1160   assert(_clustermap);
1161   vector<TrkrCluster*> clusterLayer(track->nLayer, nullptr);
1162   for (auto iter = svtxtrack->begin_cluster_keys();
1163        iter != svtxtrack->end_cluster_keys(); ++iter)
1164   {
1165     TrkrDefs::cluskey cluster_key = *iter;
1166     TrkrCluster* cluster = _clustermap->findCluster(cluster_key);
1167     int layer = TrkrDefs::getLayer(cluster_key);
1168 
1169     if (Verbosity())
1170     {
1171       cout << __PRETTY_FUNCTION__ << " - layer sorting cluster ";
1172       cluster->identify();
1173     }
1174 
1175     assert(layer >= 0);
1176     assert(layer < track->nLayer);
1177     assert(cluster);
1178 
1179     if (clusterLayer[layer])
1180     {
1181       cout << __PRETTY_FUNCTION__ << " -WARNING- more than one cluster at layer " << layer << ": " << endl;
1182       clusterLayer[layer]->identify();
1183       cluster->identify();
1184     }
1185     else
1186     {
1187       clusterLayer[layer] = cluster;
1188       track->nCluster += 1;
1189     }
1190   }
1191 
1192   if (track->nCluster < 4)
1193     return track;
1194 
1195   // refit by "excluding" each cluster
1196   // prepare seed
1197   TVector3 seed_mom(svtxtrack->get_px(), svtxtrack->get_py(), svtxtrack->get_pz());
1198   seed_mom.SetMag(120);
1199 
1200   TVector3 seed_pos(svtxtrack->get_x(), svtxtrack->get_y(), svtxtrack->get_z());
1201   TMatrixDSym seed_cov(6);
1202   for (int i = 0; i < 6; i++)
1203   {
1204     for (int j = 0; j < 6; j++)
1205     {
1206       seed_cov[i][j] = 100.;
1207     }
1208   }
1209   for (int layerStudy = 0; layerStudy < track->nLayer; ++layerStudy)
1210   {
1211     //scale uncertainty for the cluster in study with this factor
1212     const static double errorScaleFactor = 100;
1213 
1214     if (clusterLayer[layerStudy] == nullptr) continue;
1215 
1216     // Create measurements
1217     std::vector<PHGenFit::Measurement*> measurements;
1218 
1219     int indexStudy = -1;
1220     int currentIndex = 0;
1221     for (const auto cluster : clusterLayer)
1222     {
1223       if (!cluster) continue;
1224 
1225       assert(cluster);
1226       TrkrDefs::cluskey cluster_key = cluster->getClusKey();
1227       int layer = TrkrDefs::getLayer(cluster_key);
1228 
1229       const double scale_this_layer = (layer == layerStudy) ? errorScaleFactor : 1;
1230 
1231       TVector3 pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
1232 
1233       //      seed_mom.SetPhi(pos.Phi());
1234       //      seed_mom.SetTheta(pos.Theta());
1235 
1236       //TODO use u, v explicitly?
1237       TVector3 n(cluster->getPosition(0), cluster->getPosition(1), 0);
1238 
1239       PHGenFit::Measurement* meas = new PHGenFit::PlanarMeasurement(pos, n,
1240                                                                     cluster->getRPhiError() * scale_this_layer,
1241                                                                     cluster->getZError() * scale_this_layer);
1242 
1243       measurements.push_back(meas);
1244 
1245       if (layer == layerStudy) indexStudy = currentIndex;
1246       ++currentIndex;
1247     }  //    for (const auto cluster : clusterLayer)
1248     assert(indexStudy >= 0);
1249     assert(indexStudy < track->nLayer);
1250 
1251     // do the fit
1252     genfit::AbsTrackRep* rep = new genfit::RKTrackRep(_primary_pid_guess);
1253     std::shared_ptr<PHGenFit::Track> phgf_track(new PHGenFit::Track(rep, seed_pos, seed_mom,
1254                                                                     seed_cov));
1255     phgf_track->addMeasurements(measurements);
1256     if (_fitter->processTrack(phgf_track.get(), false) != 0)
1257     {
1258       if (Verbosity() >= 1)
1259       {
1260         LogWarning("Track fitting failed");
1261         cout << __PRETTY_FUNCTION__ << " track->getChisq() " << phgf_track->get_chi2() << " get_ndf " << phgf_track->get_ndf()
1262              << " mom.X " << phgf_track->get_mom().X()
1263              << " mom.Y " << phgf_track->get_mom().Y()
1264              << " mom.Z " << phgf_track->get_mom().Z()
1265              << endl;
1266       }
1267       //delete track;
1268       continue;
1269     }
1270 
1271     //propagate to state
1272     {
1273       std::shared_ptr<const genfit::MeasuredStateOnPlane> gf_state = NULL;
1274       try
1275       {
1276         const genfit::Track* gftrack = phgf_track->getGenFitTrack();
1277         assert(gftrack);
1278         const genfit::AbsTrackRep* rep = gftrack->getCardinalRep();
1279         assert(rep);
1280         genfit::TrackPoint* trpoint = gftrack->getPointWithMeasurementAndFitterInfo(indexStudy, gftrack->getCardinalRep());
1281         assert(trpoint);
1282         genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1283         assert(kfi);
1284         //gf_state = std::shared_ptr <genfit::MeasuredStateOnPlane> (const_cast<genfit::MeasuredStateOnPlane*> (&(kfi->getFittedState(true))));
1285         const genfit::MeasuredStateOnPlane* temp_state = &(kfi->getFittedState(true));
1286         gf_state = std::shared_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*temp_state));
1287       }
1288       catch (...)
1289       {
1290         if (Verbosity() > 1)
1291           LogWarning("Exrapolation failed!");
1292         continue;
1293       }
1294       if (!gf_state)
1295       {
1296         if (Verbosity() > 1)
1297           LogWarning("Exrapolation failed!");
1298         continue;
1299       }
1300       TVector3 extra_pos(gf_state->getPos());
1301 
1302       const TrkrCluster* cluster(clusterLayer[layerStudy]);
1303       assert(cluster);
1304       TVector3 cluster_pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
1305 
1306       TVector3 n_dir(cluster->getPosition(0), cluster->getPosition(1), 0);
1307       n_dir.SetMag(1);
1308       TVector3 z_dir(0, 0, 1);
1309       TVector3 azimuth_dir(z_dir.Cross(n_dir));
1310       TVector3 pos_diff = cluster_pos - extra_pos;
1311       const double n_residual = pos_diff.Dot(n_dir);
1312       const double z_residual = pos_diff.Dot(z_dir);
1313       const double azimuth_residual = pos_diff.Dot(azimuth_dir);
1314 
1315       if (Verbosity() >= 1)
1316       {
1317         cout << __PRETTY_FUNCTION__;
1318         cout << " - layer " << layerStudy << " at index " << indexStudy;
1319         cout << " cluster @ " << cluster_pos.x() << ", " << cluster_pos.y() << ", " << cluster_pos.z();
1320         cout << " extrapolate to " << extra_pos.x() << ", " << extra_pos.y() << ", " << extra_pos.z();
1321         cout << ", n_residual = " << n_residual;
1322         cout << ", z_residual = " << z_residual;
1323         cout << ", azimuth_residual = " << azimuth_residual;
1324         cout << endl;
1325 
1326         cout << "Cluster data which provide much more detailed information on the raw signals: "<<endl;
1327 
1328         // in case TpcPrototypeCluster specific functionality is needed, first to a conversion and check
1329         const TpcPrototypeCluster * prototype_cluster = dynamic_cast<const TpcPrototypeCluster *>(cluster);
1330         assert(prototype_cluster);
1331 
1332         prototype_cluster->identify();
1333       }
1334       assert(abs(n_residual) < 1e-4);  //same layer check
1335 
1336       (track->clusterKey)[layerStudy] = cluster->getClusKey();
1337       (track->clusterlayer)[layerStudy] = layerStudy;
1338       (track->clusterid)[layerStudy] = TrkrDefs::getClusIndex(cluster->getClusKey());
1339 
1340       (track->clusterX)[layerStudy] = cluster->getPosition(0);
1341       (track->clusterY)[layerStudy] = cluster->getPosition(1);
1342       (track->clusterZ)[layerStudy] = cluster->getPosition(2);
1343       (track->clusterE)[layerStudy] = cluster->getAdc();
1344       (track->clusterSizePhi)[layerStudy] = cluster->getPhiSize();
1345       (track->clusterResidualPhi)[layerStudy] = azimuth_residual;
1346       (track->clusterProjectionPhi)[layerStudy] = extra_pos.Phi();
1347       (track->clusterResidualZ)[layerStudy] = z_residual;
1348     }  //propagate to state
1349 
1350   }  // refit by "excluding" each cluster  for (int layer = 0; layer < track->nLayer; ++layer)
1351 
1352   return track;
1353 }
1354 
1355 /*
1356  * Make SvtxTrack from PHGenFit::Track and SvtxTrack
1357  */
1358 //SvtxTrack* TpcPrototypeGenFitTrkFitter::MakeSvtxTrack(const SvtxTrack* svtx_track,
1359 std::shared_ptr<SvtxTrack> TpcPrototypeGenFitTrkFitter::MakeSvtxTrack(const SvtxTrack* svtx_track,
1360                                                                       const std::shared_ptr<PHGenFit::Track>& phgf_track, const SvtxVertex* vertex)
1361 {
1362   double chi2 = phgf_track->get_chi2();
1363   double ndf = phgf_track->get_ndf();
1364 
1365   TVector3 vertex_position(0, 0, 0);
1366   TMatrixF vertex_cov(3, 3);
1367   double dvr2 = 0;
1368   double dvz2 = 0;
1369 
1370   //  if (_use_truth_vertex)
1371   //  {
1372   //    PHG4VtxPoint* first_point = _truth_container->GetPrimaryVtx(_truth_container->GetPrimaryVertexIndex());
1373   //    vertex_position.SetXYZ(first_point->get_x(), first_point->get_y(), first_point->get_z());
1374   //    if (Verbosity() > 1)
1375   //    {
1376   //      cout << PHWHERE << "Using: truth vertex: {" << vertex_position.X() << ", " << vertex_position.Y() << ", " << vertex_position.Z() << "} " << endl;
1377   //    }
1378   //  }
1379   //  else if (vertex)
1380   //  {
1381   //    vertex_position.SetXYZ(vertex->get_x(), vertex->get_y(),
1382   //                           vertex->get_z());
1383   //    dvr2 = vertex->get_error(0, 0) + vertex->get_error(1, 1);
1384   //    dvz2 = vertex->get_error(2, 2);
1385   //
1386   //    for (int i = 0; i < 3; i++)
1387   //      for (int j = 0; j < 3; j++)
1388   //        vertex_cov[i][j] = vertex->get_error(i, j);
1389   //  }
1390 
1391   //genfit::MeasuredStateOnPlane* gf_state_beam_line_ca = NULL;
1392   std::shared_ptr<genfit::MeasuredStateOnPlane> gf_state_beam_line_ca = NULL;
1393   try
1394   {
1395     gf_state_beam_line_ca = std::shared_ptr<genfit::MeasuredStateOnPlane>(phgf_track->extrapolateToLine(vertex_position,
1396                                                                                                         TVector3(0., 0., 1.)));
1397   }
1398   catch (...)
1399   {
1400     if (Verbosity() >= 2)
1401       LogWarning("extrapolateToLine failed!");
1402   }
1403   if (!gf_state_beam_line_ca) return NULL;
1404 
1405   /*!
1406      *  1/p, u'/z', v'/z', u, v
1407      *  u is defined as momentum X beam line at POCA of the beam line
1408      *  v is alone the beam line
1409      *  so u is the dca2d direction
1410      */
1411 
1412   double u = gf_state_beam_line_ca->getState()[3];
1413   double v = gf_state_beam_line_ca->getState()[4];
1414 
1415   double du2 = gf_state_beam_line_ca->getCov()[3][3];
1416   double dv2 = gf_state_beam_line_ca->getCov()[4][4];
1417   //cout << PHWHERE << "        u " << u << " v " << v << " du2 " << du2 << " dv2 " << dv2 << " dvr2 " << dvr2 << endl;
1418   //delete gf_state_beam_line_ca;
1419 
1420   //const SvtxTrack_v1* temp_track = static_cast<const SvtxTrack_v1*> (svtx_track);
1421   //    SvtxTrack_v1* out_track = new SvtxTrack_v1(
1422   //            *static_cast<const SvtxTrack_v1*>(svtx_track));
1423   std::shared_ptr<SvtxTrack_v1> out_track = std::shared_ptr<SvtxTrack_v1>(new SvtxTrack_v1(*static_cast<const SvtxTrack_v1*>(svtx_track)));
1424 
1425   out_track->set_dca2d(u);
1426   out_track->set_dca2d_error(sqrt(du2 + dvr2));
1427 
1428   std::shared_ptr<genfit::MeasuredStateOnPlane> gf_state_vertex_ca = NULL;
1429   try
1430   {
1431     gf_state_vertex_ca = std::shared_ptr<genfit::MeasuredStateOnPlane>(phgf_track->extrapolateToPoint(vertex_position));
1432   }
1433   catch (...)
1434   {
1435     if (Verbosity() >= 2)
1436       LogWarning("extrapolateToPoint failed!");
1437   }
1438   if (!gf_state_vertex_ca)
1439   {
1440     //delete out_track;
1441     return NULL;
1442   }
1443 
1444   TVector3 mom = gf_state_vertex_ca->getMom();
1445   TVector3 pos = gf_state_vertex_ca->getPos();
1446   TMatrixDSym cov = gf_state_vertex_ca->get6DCov();
1447 
1448   //    genfit::MeasuredStateOnPlane* gf_state_vertex_ca =
1449   //            phgf_track->extrapolateToLine(vertex_position,
1450   //                    TVector3(0., 0., 1.));
1451 
1452   u = gf_state_vertex_ca->getState()[3];
1453   v = gf_state_vertex_ca->getState()[4];
1454 
1455   du2 = gf_state_vertex_ca->getCov()[3][3];
1456   dv2 = gf_state_vertex_ca->getCov()[4][4];
1457 
1458   double dca3d = sqrt(u * u + v * v);
1459   double dca3d_error = sqrt(du2 + dv2 + dvr2 + dvz2);
1460 
1461   out_track->set_dca(dca3d);
1462   out_track->set_dca_error(dca3d_error);
1463 
1464   //
1465   // in: X, Y, Z; out; r: n X Z, Z X r, Z
1466 
1467   float dca3d_xy = NAN;
1468   float dca3d_z = NAN;
1469   float dca3d_xy_error = NAN;
1470   float dca3d_z_error = NAN;
1471 
1472   try
1473   {
1474     TMatrixF pos_in(3, 1);
1475     TMatrixF cov_in(3, 3);
1476     TMatrixF pos_out(3, 1);
1477     TMatrixF cov_out(3, 3);
1478 
1479     TVectorD state6(6);      // pos(3), mom(3)
1480     TMatrixDSym cov6(6, 6);  //
1481 
1482     gf_state_vertex_ca->get6DStateCov(state6, cov6);
1483 
1484     TVector3 vn(state6[3], state6[4], state6[5]);
1485 
1486     // mean of two multivariate gaussians Pos - Vertex
1487     pos_in[0][0] = state6[0] - vertex_position.X();
1488     pos_in[1][0] = state6[1] - vertex_position.Y();
1489     pos_in[2][0] = state6[2] - vertex_position.Z();
1490 
1491     for (int i = 0; i < 3; ++i)
1492     {
1493       for (int j = 0; j < 3; ++j)
1494       {
1495         cov_in[i][j] = cov6[i][j] + vertex_cov[i][j];
1496       }
1497     }
1498 
1499     // vn is momentum vector, pos_in is position vector (of what?)
1500     pos_cov_XYZ_to_RZ(vn, pos_in, cov_in, pos_out, cov_out);
1501 
1502     if (Verbosity() > 50)
1503     {
1504       cout << " vn.X " << vn.X() << " vn.Y " << vn.Y() << " vn.Z " << vn.Z() << endl;
1505       cout << " pos_in.X " << pos_in[0][0] << " pos_in.Y " << pos_in[1][0] << " pos_in.Z " << pos_in[2][0] << endl;
1506       cout << " pos_out.X " << pos_out[0][0] << " pos_out.Y " << pos_out[1][0] << " pos_out.Z " << pos_out[2][0] << endl;
1507     }
1508 
1509     dca3d_xy = pos_out[0][0];
1510     dca3d_z = pos_out[2][0];
1511     dca3d_xy_error = sqrt(cov_out[0][0]);
1512     dca3d_z_error = sqrt(cov_out[2][2]);
1513   }
1514   catch (...)
1515   {
1516     if (Verbosity() > 0)
1517       LogWarning("DCA calculationfailed!");
1518   }
1519 
1520   out_track->set_dca3d_xy(dca3d_xy);
1521   out_track->set_dca3d_z(dca3d_z);
1522   out_track->set_dca3d_xy_error(dca3d_xy_error);
1523   out_track->set_dca3d_z_error(dca3d_z_error);
1524 
1525   //if(gf_state_vertex_ca) delete gf_state_vertex_ca;
1526 
1527   out_track->set_chisq(chi2);
1528   out_track->set_ndf(ndf);
1529   out_track->set_charge(phgf_track->get_charge());
1530 
1531   out_track->set_px(mom.Px());
1532   out_track->set_py(mom.Py());
1533   out_track->set_pz(mom.Pz());
1534 
1535   out_track->set_x(pos.X());
1536   out_track->set_y(pos.Y());
1537   out_track->set_z(pos.Z());
1538 
1539   for (int i = 0; i < 6; i++)
1540   {
1541     for (int j = i; j < 6; j++)
1542     {
1543       out_track->set_error(i, j, cov[i][j]);
1544     }
1545   }
1546 
1547   const genfit::Track* gftrack = phgf_track->getGenFitTrack();
1548   const genfit::AbsTrackRep* rep = gftrack->getCardinalRep();
1549   for (unsigned int id = 0; id < gftrack->getNumPointsWithMeasurement(); ++id)
1550   {
1551     genfit::TrackPoint* trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id, gftrack->getCardinalRep());
1552 
1553     if (!trpoint)
1554     {
1555       if (Verbosity() > 1)
1556         LogWarning("!trpoint");
1557       continue;
1558     }
1559 
1560     genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(trpoint->getFitterInfo(rep));
1561     if (!kfi)
1562     {
1563       if (Verbosity() > 1)
1564         LogWarning("!kfi");
1565       continue;
1566     }
1567 
1568     std::shared_ptr<const genfit::MeasuredStateOnPlane> gf_state = NULL;
1569     try
1570     {
1571       //gf_state = std::shared_ptr <genfit::MeasuredStateOnPlane> (const_cast<genfit::MeasuredStateOnPlane*> (&(kfi->getFittedState(true))));
1572       const genfit::MeasuredStateOnPlane* temp_state = &(kfi->getFittedState(true));
1573       gf_state = std::shared_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*temp_state));
1574     }
1575     catch (...)
1576     {
1577       if (Verbosity() > 1)
1578         LogWarning("Exrapolation failed!");
1579     }
1580     if (!gf_state)
1581     {
1582       if (Verbosity() > 1)
1583         LogWarning("Exrapolation failed!");
1584       continue;
1585     }
1586     genfit::MeasuredStateOnPlane temp;
1587     float pathlength = -phgf_track->extrapolateToPoint(temp, vertex_position, id);
1588 
1589     std::shared_ptr<SvtxTrackState> state = std::shared_ptr<SvtxTrackState>(new SvtxTrackState_v1(pathlength));
1590     state->set_x(gf_state->getPos().x());
1591     state->set_y(gf_state->getPos().y());
1592     state->set_z(gf_state->getPos().z());
1593 
1594     state->set_px(gf_state->getMom().x());
1595     state->set_py(gf_state->getMom().y());
1596     state->set_pz(gf_state->getMom().z());
1597 
1598     //gf_state->getCov().Print();
1599 
1600     for (int i = 0; i < 6; i++)
1601     {
1602       for (int j = i; j < 6; j++)
1603       {
1604         state->set_error(i, j, gf_state->get6DCov()[i][j]);
1605       }
1606     }
1607 
1608     out_track->insert_state(state.get());
1609   }
1610 
1611   return out_track;
1612 }
1613 
1614 /*
1615  * Fill SvtxVertexMap from GFRaveVertexes and Tracks
1616  */
1617 bool TpcPrototypeGenFitTrkFitter::FillSvtxVertexMap(
1618     const std::vector<genfit::GFRaveVertex*>& rave_vertices,
1619     const std::vector<genfit::Track*>& gf_tracks)
1620 {
1621   if (_over_write_svtxvertexmap)
1622   {
1623     _vertexmap->clear();
1624   }
1625 
1626   for (unsigned int ivtx = 0; ivtx < rave_vertices.size(); ++ivtx)
1627   {
1628     genfit::GFRaveVertex* rave_vtx = rave_vertices[ivtx];
1629 
1630     if (!rave_vtx)
1631     {
1632       cerr << PHWHERE << endl;
1633       return false;
1634     }
1635 
1636     std::shared_ptr<SvtxVertex> svtx_vtx(new SvtxVertex_v1());
1637 
1638     svtx_vtx->set_chisq(rave_vtx->getChi2());
1639     svtx_vtx->set_ndof(rave_vtx->getNdf());
1640     svtx_vtx->set_position(0, rave_vtx->getPos().X());
1641     svtx_vtx->set_position(1, rave_vtx->getPos().Y());
1642     svtx_vtx->set_position(2, rave_vtx->getPos().Z());
1643 
1644     for (int i = 0; i < 3; i++)
1645       for (int j = 0; j < 3; j++)
1646         svtx_vtx->set_error(i, j, rave_vtx->getCov()[i][j]);
1647 
1648     for (unsigned int i = 0; i < rave_vtx->getNTracks(); i++)
1649     {
1650       //TODO Assume id's are sync'ed between _trackmap_refit and gf_tracks, need to change?
1651       const genfit::Track* rave_track =
1652           rave_vtx->getParameters(i)->getTrack();
1653       for (unsigned int j = 0; j < gf_tracks.size(); j++)
1654       {
1655         if (rave_track == gf_tracks[j])
1656           svtx_vtx->insert_track(j);
1657       }
1658     }
1659 
1660     if (_over_write_svtxvertexmap)
1661     {
1662       if (_vertexmap)
1663       {
1664         _vertexmap->insert_clone(svtx_vtx.get());
1665       }
1666       else
1667       {
1668         LogError("!_vertexmap");
1669       }
1670     }
1671     else
1672     {
1673       if (_vertexmap_refit)
1674       {
1675         _vertexmap_refit->insert_clone(svtx_vtx.get());
1676       }
1677       else
1678       {
1679         LogError("!_vertexmap_refit");
1680       }
1681     }
1682 
1683     //      if (Verbosity() >= 2) {
1684     //          cout << PHWHERE << endl;
1685     //          svtx_vtx->Print();
1686     //          _vertexmap_refit->Print();
1687     //      }
1688 
1689     //delete svtx_vtx;
1690   }  //loop over RAVE vertices
1691 
1692   return true;
1693 }
1694 
1695 bool TpcPrototypeGenFitTrkFitter::pos_cov_uvn_to_rz(const TVector3& u, const TVector3& v,
1696                                                     const TVector3& n, const TMatrixF& pos_in, const TMatrixF& cov_in,
1697                                                     TMatrixF& pos_out, TMatrixF& cov_out) const
1698 {
1699   if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1700   {
1701     if (Verbosity() > 0) LogWarning("pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3");
1702     return false;
1703   }
1704 
1705   if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1706   {
1707     if (Verbosity() > 0) LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1708     return false;
1709   }
1710 
1711   TVector3 Z_uvn(u.Z(), v.Z(), n.Z());
1712   TVector3 up_uvn = TVector3(0., 0., 1.).Cross(Z_uvn);  // n_uvn X Z_uvn
1713 
1714   if (up_uvn.Mag() < 0.00001)
1715   {
1716     if (Verbosity() > 0) LogWarning("n is parallel to z");
1717     return false;
1718   }
1719 
1720   // R: rotation from u,v,n to n X Z, nX(nXZ), n
1721   TMatrixF R(3, 3);
1722   TMatrixF R_T(3, 3);
1723 
1724   try
1725   {
1726     // rotate u along z to up
1727     float phi = -atan2(up_uvn.Y(), up_uvn.X());
1728     R[0][0] = cos(phi);
1729     R[0][1] = -sin(phi);
1730     R[0][2] = 0;
1731     R[1][0] = sin(phi);
1732     R[1][1] = cos(phi);
1733     R[1][2] = 0;
1734     R[2][0] = 0;
1735     R[2][1] = 0;
1736     R[2][2] = 1;
1737 
1738     R_T.Transpose(R);
1739   }
1740   catch (...)
1741   {
1742     if (Verbosity() > 0)
1743       LogWarning("Can't get rotation matrix");
1744 
1745     return false;
1746   }
1747 
1748   pos_out.ResizeTo(3, 1);
1749   cov_out.ResizeTo(3, 3);
1750 
1751   pos_out = R * pos_in;
1752   cov_out = R * cov_in * R_T;
1753 
1754   return true;
1755 }
1756 
1757 bool TpcPrototypeGenFitTrkFitter::get_vertex_error_uvn(const TVector3& u,
1758                                                        const TVector3& v, const TVector3& n, const TMatrixF& cov_in,
1759                                                        TMatrixF& cov_out) const
1760 {
1761   /*!
1762      * Get matrix that rotates frame (u,v,n) to (x,y,z)
1763      * or the matrix that rotates vector defined in (x,y,z) to defined (u,v,n)
1764      */
1765 
1766   TMatrixF R = get_rotation_matrix(u, v, n);
1767   //
1768   //    LogDebug("TpcPrototypeGenFitTrkFitter::get_vertex_error_uvn::R = ");
1769   //    R.Print();
1770   //    cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
1771 
1772   if (!(abs(R.Determinant() - 1) < 0.01))
1773   {
1774     if (Verbosity() > 0)
1775       LogWarning("!(abs(R.Determinant()-1)<0.0001)");
1776     return false;
1777   }
1778 
1779   if (R.GetNcols() != 3 || R.GetNrows() != 3)
1780   {
1781     if (Verbosity() > 0)
1782       LogWarning("R.GetNcols() != 3 || R.GetNrows() != 3");
1783     return false;
1784   }
1785 
1786   if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1787   {
1788     if (Verbosity() > 0)
1789       LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1790     return false;
1791   }
1792 
1793   TMatrixF R_T(3, 3);
1794 
1795   R_T.Transpose(R);
1796 
1797   cov_out.ResizeTo(3, 3);
1798 
1799   cov_out = R * cov_in * R_T;
1800 
1801   return true;
1802 }
1803 
1804 bool TpcPrototypeGenFitTrkFitter::pos_cov_XYZ_to_RZ(
1805     const TVector3& n, const TMatrixF& pos_in, const TMatrixF& cov_in,
1806     TMatrixF& pos_out, TMatrixF& cov_out) const
1807 {
1808   if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1809   {
1810     if (Verbosity() > 0) LogWarning("pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3");
1811     return false;
1812   }
1813 
1814   if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1815   {
1816     if (Verbosity() > 0) LogWarning("cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1817     return false;
1818   }
1819 
1820   // produces a vector perpendicular to both the momentum vector and beam line - i.e. in the direction of the dca_xy
1821   // only the angle of r will be used, not the magnitude
1822   TVector3 r = n.Cross(TVector3(0., 0., 1.));
1823   if (r.Mag() < 0.00001)
1824   {
1825     if (Verbosity() > 0) LogWarning("n is parallel to z");
1826     return false;
1827   }
1828 
1829   // R: rotation from u,v,n to n X Z, nX(nXZ), n
1830   TMatrixF R(3, 3);
1831   TMatrixF R_T(3, 3);
1832 
1833   try
1834   {
1835     // rotate u along z to up
1836     float phi = -atan2(r.Y(), r.X());
1837     R[0][0] = cos(phi);
1838     R[0][1] = -sin(phi);
1839     R[0][2] = 0;
1840     R[1][0] = sin(phi);
1841     R[1][1] = cos(phi);
1842     R[1][2] = 0;
1843     R[2][0] = 0;
1844     R[2][1] = 0;
1845     R[2][2] = 1;
1846 
1847     R_T.Transpose(R);
1848   }
1849   catch (...)
1850   {
1851     if (Verbosity() > 0)
1852       LogWarning("Can't get rotation matrix");
1853 
1854     return false;
1855   }
1856 
1857   pos_out.ResizeTo(3, 1);
1858   cov_out.ResizeTo(3, 3);
1859 
1860   pos_out = R * pos_in;
1861   cov_out = R * cov_in * R_T;
1862 
1863   return true;
1864 }
1865 
1866 /*!
1867  * Get 3D Rotation Matrix that rotates frame (x,y,z) to (x',y',z')
1868  * Default rotate local to global, or rotate vector in global to local representation
1869  */
1870 TMatrixF TpcPrototypeGenFitTrkFitter::get_rotation_matrix(const TVector3 x,
1871                                                           const TVector3 y, const TVector3 z, const TVector3 xp, const TVector3 yp,
1872                                                           const TVector3 zp) const
1873 {
1874   TMatrixF R(3, 3);
1875 
1876   TVector3 xu = x.Unit();
1877   TVector3 yu = y.Unit();
1878   TVector3 zu = z.Unit();
1879 
1880   const float max_diff = 0.01;
1881 
1882   if (!(
1883           abs(xu * yu) < max_diff and
1884           abs(xu * zu) < max_diff and
1885           abs(yu * zu) < max_diff))
1886   {
1887     if (Verbosity() > 0)
1888       LogWarning("input frame error!");
1889     return R;
1890   }
1891 
1892   TVector3 xpu = xp.Unit();
1893   TVector3 ypu = yp.Unit();
1894   TVector3 zpu = zp.Unit();
1895 
1896   if (!(
1897           abs(xpu * ypu) < max_diff and
1898           abs(xpu * zpu) < max_diff and
1899           abs(ypu * zpu) < max_diff))
1900   {
1901     if (Verbosity() > 0)
1902       LogWarning("output frame error!");
1903     return R;
1904   }
1905 
1906   /*!
1907      * Decompose x',y',z' in x,y,z and call them u,v,n
1908      * Then the question will be rotate the standard X,Y,Z to u,v,n
1909      */
1910 
1911   TVector3 u(xpu.Dot(xu), xpu.Dot(yu), xpu.Dot(zu));
1912   TVector3 v(ypu.Dot(xu), ypu.Dot(yu), ypu.Dot(zu));
1913   TVector3 n(zpu.Dot(xu), zpu.Dot(yu), zpu.Dot(zu));
1914 
1915   try
1916   {
1917     std::shared_ptr<TRotation> rotation(new TRotation());
1918     //TRotation *rotation = new TRotation();
1919 
1920     //! Rotation that rotate standard (X, Y, Z) to (u, v, n)
1921     rotation->RotateAxes(u, v, n);
1922 
1923     R[0][0] = rotation->XX();
1924     R[0][1] = rotation->XY();
1925     R[0][2] = rotation->XZ();
1926     R[1][0] = rotation->YX();
1927     R[1][1] = rotation->YY();
1928     R[1][2] = rotation->YZ();
1929     R[2][0] = rotation->ZX();
1930     R[2][1] = rotation->ZY();
1931     R[2][2] = rotation->ZZ();
1932     //
1933     //      LogDebug("TpcPrototypeGenFitTrkFitter::get_rotation_matrix: TRotation:");
1934     //      R.Print();
1935     //      cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
1936 
1937     //delete rotation;
1938 
1939     //      TMatrixF ROT1(3, 3);
1940     //      TMatrixF ROT2(3, 3);
1941     //      TMatrixF ROT3(3, 3);
1942     //
1943     //      // rotate n along z to xz plane
1944     //      float phi = -atan2(n.Y(), n.X());
1945     //      ROT1[0][0] = cos(phi);
1946     //      ROT1[0][1] = -sin(phi);
1947     //      ROT1[0][2] = 0;
1948     //      ROT1[1][0] = sin(phi);
1949     //      ROT1[1][1] = cos(phi);
1950     //      ROT1[1][2] = 0;
1951     //      ROT1[2][0] = 0;
1952     //      ROT1[2][1] = 0;
1953     //      ROT1[2][2] = 1;
1954     //
1955     //      // rotate n along y to z
1956     //      TVector3 n1(n);
1957     //      n1.RotateZ(phi);
1958     //      float theta = -atan2(n1.X(), n1.Z());
1959     //      ROT2[0][0] = cos(theta);
1960     //      ROT2[0][1] = 0;
1961     //      ROT2[0][2] = sin(theta);
1962     //      ROT2[1][0] = 0;
1963     //      ROT2[1][1] = 1;
1964     //      ROT2[1][2] = 0;
1965     //      ROT2[2][0] = -sin(theta);
1966     //      ROT2[2][1] = 0;
1967     //      ROT2[2][2] = cos(theta);
1968     //
1969     //      // rotate u along z to x
1970     //      TVector3 u2(u);
1971     //      u2.RotateZ(phi);
1972     //      u2.RotateY(theta);
1973     //      float phip = -atan2(u2.Y(), u2.X());
1974     //      ROT3[0][0] = cos(phip);
1975     //      ROT3[0][1] = -sin(phip);
1976     //      ROT3[0][2] = 0;
1977     //      ROT3[1][0] = sin(phip);
1978     //      ROT3[1][1] = cos(phip);
1979     //      ROT3[1][2] = 0;
1980     //      ROT3[2][0] = 0;
1981     //      ROT3[2][1] = 0;
1982     //      ROT3[2][2] = 1;
1983     //
1984     //      // R: rotation from u,v,n to (z X n), v', z
1985     //      R = ROT3 * ROT2 * ROT1;
1986     //
1987     //      R.Invert();
1988     //      LogDebug("TpcPrototypeGenFitTrkFitter::get_rotation_matrix: Home Brew:");
1989     //      R.Print();
1990     //      cout<<"R.Determinant() = "<<R.Determinant()<<"\n";
1991   }
1992   catch (...)
1993   {
1994     if (Verbosity() > 0)
1995       LogWarning("Can't get rotation matrix");
1996 
1997     return R;
1998   }
1999 
2000   return R;
2001 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!