trf++/src/gtrfit
diff -N GTrackSmoother.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ GTrackSmoother.cpp 11 Aug 2011 00:04:40 -0000 1.1
@@ -0,0 +1,365 @@
+#include "gtrfit/GTrackSmoother.hpp"
+#include "gtrbase/GTrack.hpp"
+#include "trfbase/Propagator.h"
+// cng #include "d0cluster_evt/ChunkClusterPtr.hpp"
+#include "gtrbase/GTrackState.hpp"
+#include "trfutil/STLprint.h"
+#include "trfbase/ETrack.h"
+#include "trfbase/PropStat.h"
+#include "trffit/HTrack.h"
+#include "trfbase/Hit.h"
+// cng #include "d0cluster/D0ClusterRegistry.hpp"
+#include "trfcyl/SurfCylinder.h"
+#include <cassert>
+#include <iostream>
+
+using std::cerr;
+using std::endl;
+using namespace trf;
+
+namespace {
+enum { INVALID=0,
+ FORWARD=1,
+ BACKWARD=2
+};
+
+class ConstIterator {
+public:
+ ConstIterator(const GTrack::StateList&,int dir);
+ bool end() const;
+ void begin();
+ const ConstIterator& operator++() ;
+ ConstIterator operator++(int);
+ const ConstIterator& operator--() ;
+ ConstIterator operator--(int);
+ const GTrackState* operator->() const;
+ const GTrackState& operator*() const;
+
+protected:
+ const GTrack::StateList& _slist;
+ GTrack::StateList::const_iterator _iforw;
+ GTrack::StateList::const_reverse_iterator _iback;
+ int _dir;
+};
+
+int get_direction(const GTrack& trg);
+
+int check_error(const TrackError&);
+
+GTrackState get_next_state(const GTrackState& smoothed,
+ const GTrackState& unsmoothed,
+ const ETrack& tre_smoothed_pred,
+ const TrackDerivative& deriv,
+ int& ierr);
+
+}
+
+
+//*************************************************************************
+
+GTrackSmoother::GTrackSmoother(const trf::PropagatorPtr& pprop /* cng ,
+ D0ClusterRegistry* preg */ ) : _pprop(pprop)/* cng , _preg(preg) */ {
+}
+
+//*************************************************************************
+
+GTrackSmoother::GTrackSmoother(const GTrackSmoother& smoother) : _pprop(smoother._pprop)/* cng ,_preg(smoother._preg) */ {
+}
+
+//*************************************************************************
+
+GTrackSmoother::~GTrackSmoother() {
+}
+
+//*************************************************************************
+
+int GTrackSmoother::smooth(GTrack& output,int n_nomiss_states) const {
+
+ const GTrack& trg = output;
+
+ // determine tracks direction
+ int direction = get_direction(trg);
+
+ if( direction == INVALID ) {
+ output = GTrack();
+ return 1;
+ }
+
+ const GTrack::StateList& oldstates = trg.get_states();
+ GTrack::StateList newstates;
+
+ ConstIterator iter(oldstates,direction);
+
+ // loop till a first State with no miss
+
+ for(iter.begin() ; !iter.end() && !iter->cluster() ; ++iter ) {
+ assert( iter->fit_status() == GTrackState::OPTIMAL);
+ newstates.insert(*iter);
+ }
+
+ // exit if no more states left
+
+ if( iter.end() ) {
+ output = GTrack(newstates, _pprop->get_bfield());
+ return 0;
+ }
+
+ // remember the first smoothed state
+
+ const GTrackState* smoothed = &(*iter);
+ assert( smoothed->fit_status() == GTrackState::OPTIMAL);
+ newstates.insert(*smoothed);
+ ETrack tre_smoothed_pred = smoothed->get_nohit_track();
+ assert(smoothed->smoother() != 0);
+ TrackDerivative deriv = smoothed->smoother()->deriv();
+ int nstates=1;
+
+ bool first = true;
+ bool error = false;
+ for( ++iter ; !iter.end() ; ++iter ) {
+ int ierr=0;
+ bool has_cluster = iter->cluster();
+ if(has_cluster) {
+ if( nstates >= n_nomiss_states && n_nomiss_states > 0 ) {
+ output = GTrack(newstates, _pprop->get_bfield());
+ return 0;
+ }
+ }
+
+ if(!error) {
+ GTrackState newstate =
+ get_next_state(*smoothed, *iter, tre_smoothed_pred, deriv, ierr);
+ if( ierr ) {
+ cerr << "GTrackSmoother: smoothing error " << 10+ierr << endl;
+ error = true;
+ }
+ else
+ newstates.insert(newstate);
+ }
+ if(error)
+ newstates.insert(*iter);
+
+ if(has_cluster)
+ ++nstates;
+
+ if( direction == FORWARD )
+ smoothed = &(*(newstates.rbegin()));
+ else
+ smoothed = &(*(newstates.begin()));
+ tre_smoothed_pred = iter->get_nohit_track();
+ assert(iter->smoother() != 0);
+ deriv = iter->smoother()->deriv();
+
+ first = false;
+ }
+
+ output = GTrack(newstates, _pprop->get_bfield());
+
+ return 0;
+}
+
+
+//*************************************************************************
+
+
+namespace {
+
+//*****************************
+
+ConstIterator::ConstIterator(const GTrack::StateList& slist,int dir) :
+ _slist(slist),_dir(dir) {
+ assert( _dir == FORWARD || _dir == BACKWARD );
+}
+
+//*****************************
+
+bool ConstIterator::end() const {
+ if( _iforw == _slist.end() || _iback == _slist.rend() ) {
+ assert(_iforw == _slist.end() && _iback == _slist.rend() );
+ return true;
+ }
+ return false;
+}
+
+//*****************************
+void ConstIterator::begin() {
+ _iforw = _slist.begin();
+ _iback = _slist.rbegin();
+}
+
+//*****************************
+
+const ConstIterator& ConstIterator::operator++() {
+ _iforw++;
+ _iback++;
+ return *this;
+}
+
+//*****************************
+ConstIterator ConstIterator::operator++(int i) {
+ ConstIterator iter(*this);
+ _iforw++;
+ _iback++;
+ return iter;
+}
+
+//*****************************
+
+const ConstIterator& ConstIterator::operator--() {
+ _iforw--;
+ _iback--;
+ return *this;
+}
+
+//*****************************
+
+ConstIterator ConstIterator::operator--(int i) {
+ ConstIterator iter(*this);
+ _iforw--;
+ _iback--;
+ return iter;
+}
+
+//*****************************
+
+const GTrackState* ConstIterator::operator->() const {
+ return &(ConstIterator::operator*());
+}
+
+//*****************************
+
+const GTrackState& ConstIterator::operator*() const {
+ if( _dir == FORWARD )
+ return *_iforw;
+ return *_iback;
+}
+
+//*****************************
+
+int get_direction(const GTrack& trg) {
+ const GTrack::StateList& states = trg.get_states();
+ GTrack::StateList::const_iterator iter;
+
+ int dir = INVALID;
+
+ if( states.size() == 0 )
+ return dir;
+
+ if( (states.begin())->fit_status() == GTrackState::OPTIMAL )
+ dir = FORWARD;
+
+ if( (--(states.end()))->fit_status() == GTrackState::OPTIMAL ) {
+ if( dir == FORWARD )
+ return INVALID;
+ dir = BACKWARD;
+ }
+
+ if( dir == INVALID )
+ return dir;
+
+ // loop from the begining of the list and end the end of the list till
+ // the first non optimal fit
+
+ GTrack::StateList::const_iterator imin=states.begin();
+ GTrack::StateList::const_iterator imax=states.end();
+ for( ; imin != states.end() && imin->fit_status() == GTrackState::OPTIMAL ; ++imin);
+ for( imax--; imax != states.begin() && imax->fit_status() == GTrackState::OPTIMAL ; --imax);
+ imax++;
+
+ for(iter = imin ; iter != imax ; ++iter ) {
+
+ if( dir == FORWARD && iter == states.begin() )
+ continue;
+
+ if( dir == BACKWARD && iter == --(states.end()) )
+ break;
+
+ if( dir == FORWARD && iter->fit_status() != GTrackState::BACKWARD && iter->cluster() )
+ return INVALID;
+
+ if( dir == BACKWARD && iter->fit_status() != GTrackState::FORWARD && iter->cluster() )
+ return INVALID;
+ }
+
+ return dir;
+}
+
+//*****************************
+
+GTrackState get_next_state(const GTrackState& smoothed,
+ const GTrackState& unsmoothed,
+ const ETrack& tre_kp1,
+ const TrackDerivative& deriv,
+ int& ierr) {
+ ierr=0;
+
+ const ETrack& tre_k = unsmoothed.track();
+ const ETrack& smoothed_kp1 = smoothed.track();
+
+ const TrfSMatrix& err_k = tre_k.get_error().get_matrix();
+ const TrfSMatrix& err_kp1 = tre_kp1.get_error().get_matrix();
+ const TrfSMatrix& err_smoothed_kp1 = smoothed_kp1.get_error().get_matrix();
+
+ TrfSMatrix ierr_kp1 = err_kp1;
+
+ if ( geninvert(ierr_kp1) ) {
+ ierr= 3;
+ return GTrackState();
+ }
+
+ TrfMatrix F = deriv.get_matrix();
+
+ TrfMatrix gain = err_k*F.transpose()*ierr_kp1;
+
+ const TrfVector& vec_k = tre_k.get_vector().get_vector();
+ const TrfVector& vec_smoothed_kp1 = smoothed_kp1.get_vector().get_vector();
+ const TrfVector& vec_kp1 = tre_kp1.get_vector().get_vector();
+
+ TrfVector vec_smoothed_k = vec_k + gain*(vec_smoothed_kp1 - vec_kp1);
+
+ TrfSMatrix err_tot = err_smoothed_kp1 - err_kp1;
+
+ TrfSMatrix err_smoothed_k = err_k + gain%err_tot;
+
+
+ if( check_error(err_smoothed_k) ) {
+ ierr=4;
+ return GTrackState();
+ }
+
+ ETrack smoothed_k(tre_k.get_surface(),vec_smoothed_k,err_smoothed_k);
+ smoothed_k.set_forward();
+ if( tre_k.is_backward() ) smoothed_k.set_backward();
+
+ double chisq = smoothed.chi_square();
+ double spath = unsmoothed.s();
+
+ if(unsmoothed.cluster())
+ return GTrackState(spath,smoothed_k,GTrackState::OPTIMAL, chisq,
+ unsmoothed.cluster());
+ else
+ return GTrackState(spath,smoothed_k,GTrackState::OPTIMAL, chisq,
+ unsmoothed.miss());
+}
+
+//******************************************
+
+int check_error(const TrackError& new_err)
+ // check error matrix
+{
+ int nbad = 0;
+ for ( int i=0; i<5; ++i ) {
+ if ( new_err(i,i) < 0.0 ) ++nbad;
+ double eii = new_err(i,i);
+ for ( int j=0; j<i; ++j ) {
+ double ejj = new_err(j,j);
+ double eij = new_err(j,i);
+ if ( fabs(eij*eij) > eii*ejj ) ++nbad;
+ }
+ }
+ return nbad ;
+}
+
+//********************************************
+
+}
trf++/src/gtrfit
diff -N SimpleGTrackFitter.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ SimpleGTrackFitter.cpp 11 Aug 2011 00:04:40 -0000 1.1
@@ -0,0 +1,594 @@
+// SimpleGTrackFitter.cpp
+
+#include "gtrfit/SimpleGTrackFitter.hpp"
+//cng#include "d0cluster_evt/ChunkClusterPtr.hpp"
+#include "gtrbase/GTrackState.hpp"
+#include "trfutil/STLprint.h"
+#include "trfbase/ETrack.h"
+#include "trfbase/PropStat.h"
+#include "trffit/HTrack.h"
+#include "trfbase/Hit.h"
+#include "trfdca/SurfDCA.h"
+//cng#include "smt_hitconverter/TrfClusterConverter.hpp"
+//cng#include "smt_hitconverter/TrfClusterConverter1D.hpp"
+//cng#include "d0cluster/D0ClusterRegistry.hpp"
+#include "trfcyl/SurfCylinder.h"
+#include <iostream>
+#include <cmath>
+#include <cstdlib>
+
+using std::cout;
+using std::cerr;
+using std::endl;
+using std::abs;
+using std::max;
+using trf::PropagatorPtr;
+using trf::Propagator;
+using trf::PropStat;
+using trf::AddFitter;
+using trf::AddFitKalman;
+using trf::HTrack;
+using trf::ETrack;
+using trf::ClusterPtr;
+using trf::HitList;
+using trf::HitPtr;
+using trf::MissPtr;
+using trf::SurfacePtr;
+using trf::SurfCylinder;
+using trf::TrackError;
+using trf::SurfDCA;
+using trf::TrackDerivative;
+
+//**********************************************************************
+// Local definitions.
+//**********************************************************************
+
+namespace {
+
+int FORWARD = 1;
+int BACKWARD = -1;
+double ERRFAC = 100.;
+bool DEBUG = false;
+
+// Update the fit for an HTrack using a GTrackState.
+// The HTrack is propagated to the GTrackState surface.
+// If a cluster is present, it is used to update the HTrack fit.
+// If a miss is present, the miss probability is recalculated.
+// A new GTrackState is constructed using the new fit and the
+// existing hit or miss.
+//
+// Return an invalid state for failure.
+//
+// trh = HTrack to be modified
+// errstat = error status code (0 for success).
+// spath = path distance
+// preg = cluster registry
+// pprop = propagator
+// pfit = fitter
+// chsq_max = chi-square limit for successful fit
+// gfit_stat = fit status to be assigned to the new state if the
+// fit is successful
+// instate = incoming GTrackState
+// first = flag indicating tis is the first state for this track.
+//
+GTrackState
+ update_fit(HTrack& trh, int& errstat, double& spath,
+ /*cng D0ClusterRegistry* preg,*/ const PropagatorPtr& pprop,
+ AddFitter* pfit, double chsq_max,
+ GTrackState::FitStatus gfit_stat,
+ const GTrackState& instate, bool first, int order) {
+
+ // Initialize error return.
+ errstat = 0;
+
+ // Remember the original path length.
+ double old_spath = spath;
+
+ // Determine if the incoming state has a cluster or miss, or is at dca.
+ const SurfacePtr& psrf_in = instate.track().get_surface();
+ MissPtr pmiss = instate.miss();
+ const ClusterPtr& icclu = instate.cluster();
+ bool has_miss = pmiss != 0;
+ bool has_cluster = icclu;
+ bool at_dca = false;
+ if ( psrf_in )
+ at_dca = psrf_in->get_type() == SurfDCA::get_static_type();
+
+ // Require that only one of the cluster, miss or dca are true.
+ // This is presently required by the GTrackState interface.
+ // We need to do a little work here if that changes.
+ if(at_dca) {
+ assert(!has_miss && !has_cluster);
+ if(has_miss || has_cluster) {
+ errstat = 1;
+ return GTrackState();
+ }
+ }
+ else {
+ assert( ! (has_miss && has_cluster) );
+ if ( has_miss && has_cluster ) {
+ errstat = 1;
+ return GTrackState();
+ }
+ }
+
+ // Require that at least one of a cluster, miss or dca are true.
+ // This a long with the previous condition protects against
+ // inaccessible clusters, e.g. from a missing chunk.
+ // We may want to drop this later and add a check on the
+ // number of clusters or measurements used in the fit.
+ assert( has_miss || has_cluster || at_dca );
+ if ( ! (has_miss || has_cluster || at_dca) ) {
+ errstat = 2;
+ return GTrackState();
+ }
+
+ // If a cluster is accessible, extract and refit.
+ if ( has_cluster ) {
+ assert( icclu );
+
+
+ /* cng
+ // Extract the TRF++ cluster from the cluster index.
+ D0ClusterIndex iclu = icclu.get_object_index();
+ edm::ChunkID chkid = icclu.get_chunk_id();
+ ClusterPtr pclu(0);
+ // First look in the registry.
+ if ( preg ) {
+ pclu = preg->get_cluster(chkid, iclu);
+ }
+ // If registry lookup failed, then create a new TRF++ cluster
+ // from the D0 cluster.
+ if ( pclu == 0 ) {
+ ChunkClusterPtr ppclu(icclu);
+ pclu = ppclu->new_trf_cluster();
+ if ( pclu == 0 ) {
+ errstat = 3;
+ return GTrackState();
+ }
+ // Register the new cluster.
+ if ( preg ) {
+ int stat =preg->register_cluster(pclu, chkid, iclu);
+ assert( stat == 0 );
+ if ( stat != 0 ) {
+ errstat = 4;
+ return GTrackState();
+ }
+ }
+ assert( pclu != 0 );
+ }
+ */
+ ClusterPtr pclu(0);
+//cng
+ // Propagate the fitted track to the cluster surface.
+ Propagator::PropDir dir_cluster;
+ SurfacePtr psrf_cluster(pclu->get_surface().new_surface());
+ if(psrf_cluster->get_pure_type() == SurfCylinder::get_static_type())
+ dir_cluster = Propagator::NEAREST_MOVE;
+ else
+ dir_cluster = Propagator::NEAREST;
+ if ( first ) dir_cluster = Propagator::NEAREST;
+ TrackDerivative deriv_clu;
+ TrackError noise_clu;
+ PropStat pstat_cluster =
+ trh.propagate(*pprop, *psrf_cluster, dir_cluster,
+ &deriv_clu, &noise_clu);
+ if ( ! pstat_cluster.success()) {
+ errstat = 5;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+ double ds_cluster = pstat_cluster.path_distance();
+
+ // Extract the hit.
+ HitList hits( pclu->predict( trh.get_track()) );
+ assert( hits.size() == 1 );
+ if ( hits.size() != 1 ) {
+ errstat = 6;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+ HitPtr phit( hits.front() );
+
+ // Propagate the fitted track to the hit surface.
+ Propagator::PropDir dir_hit = Propagator::NEAREST;
+ SurfacePtr psrf_hit(phit->get_surface().new_surface());
+ assert( psrf_hit != 0 );
+ TrackDerivative deriv_hit;
+ TrackError noise_hit;
+ PropStat pstat_hit = trh.propagate(*pprop, *psrf_hit, dir_hit,
+ &deriv_hit, &noise_hit);
+ if ( ! pstat_hit.success() ) {
+ errstat = 7;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+ double ds_hit = pstat_hit.path_distance();
+
+ // Update the path distance.
+ double ds = ds_cluster + ds_hit;
+ double ds_min = max(1.e-6, 1.e-10*abs(spath));
+
+ // Force the path length to be monotonic in agreement with the direction.
+ if(!first) {
+ if(order == FORWARD && ds <= ds_min) {
+ ds = ds_min;
+ assert(ds > -1.);
+ }
+ if(order == BACKWARD && ds >= -ds_min) {
+ ds = -ds_min;
+ assert(ds < 1.);
+ }
+ }
+ spath += ds;
+ if(!first && spath == old_spath) {
+ errstat = 12;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+
+ // Remember the prediciton (ETrack and chisquare).
+ ETrack tre_pred(trh.get_track());
+ double chisq_pred(trh.get_chi_square());
+
+ // Fit with the new hit.
+ int fit_stat = pfit->add_hit(trh,phit);
+ if ( fit_stat != 0 ) {
+ errstat = 8;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+ if ( trh.get_chi_square() > chsq_max ) {
+ errstat = 9;
+ if(pclu != 0 /*cng && preg == 0 */)
+ delete pclu;
+ return GTrackState();
+ }
+
+ // Return the outgoing state for a cluster.
+ const ETrack& tre = trh.get_track();
+ double chsq( trh.get_chi_square() );
+ GTrackState result(spath, tre, gfit_stat, chsq, icclu);
+ result.set_nohit_track(tre_pred, chisq_pred);
+ SmoothPtr psmooth;
+ if(ds_hit == 0.)
+ psmooth = new Smooth(deriv_clu, noise_clu);
+ else
+ psmooth = new Smooth(deriv_hit * deriv_clu,
+ noise_hit + noise_clu.Xform(deriv_hit));
+ result.set_smoother(psmooth);
+
+ if(pclu != 0 /* cng && preg == 0 */)
+ delete pclu;
+ return result;
+
+ } // end if has_cluster
+
+ // If the state has a miss, then propagate the track to the miss
+ // surface and update the miss probability.
+ if ( has_miss ) {
+
+ // Propagate the fitted track to the miss surface.
+ Propagator::PropDir dir;
+ SurfacePtr psrf(pmiss->get_surface().new_surface());
+ if(psrf->get_pure_type() == SurfCylinder::get_static_type())
+ dir = Propagator::NEAREST_MOVE;
+ else
+ dir = Propagator::NEAREST;
+ if ( first ) dir = Propagator::NEAREST;
+ TrackDerivative deriv_miss;
+ TrackError noise_miss;
+ PropStat pstat = trh.propagate(*pprop, *psrf, dir,
+ &deriv_miss, &noise_miss);
+ if ( ! pstat.success()) {
+ errstat = 10;
+ return GTrackState();
+ }
+ double ds = pstat.path_distance();
+
+ // Update the path distance.
+ // Force the path length to be monotonic in agreement with the direction.
+ if(!first) {
+ if(order == FORWARD && ds <= 1.e-6) {
+ ds = 1.e-6;
+ assert(ds > -1.);
+ }
+ if(order == BACKWARD && ds >= -1.e-6) {
+ ds = -1.e-6;
+ assert(ds < 1.);
+ }
+ }
+ spath += ds;
+ if(!first && spath == old_spath) {
+ errstat = 13;
+ return GTrackState();
+ }
+
+ // Return the outgoing state for a miss.
+ // No need to set nohit track & chisquare.
+ const ETrack& tre = trh.get_track();
+ double chsq( trh.get_chi_square() );
+ GTrackState result(spath, tre, gfit_stat, chsq, pmiss);
+ SmoothPtr psmooth(new Smooth(deriv_miss, noise_miss));
+ result.set_smoother(psmooth);
+ return result;
+
+ } // end if has_miss
+
+ // If the state is at dca, then propagate the track to the dca
+ // surface.
+ if ( at_dca ) {
+
+ // Propagate the fitted track to the dca surface.
+ Propagator::PropDir dir = Propagator::NEAREST;
+ SurfacePtr psrf(psrf_in->new_surface());
+ TrackDerivative deriv_dca;
+ TrackError noise_dca;
+ PropStat pstat = trh.propagate(*pprop, *psrf, dir, &deriv_dca, &noise_dca);
+ if ( ! pstat.success()) {
+ errstat = 11;
+ return GTrackState();
+ }
+ double ds = pstat.path_distance();
+
+ // Update the path distance.
+ // Force the path length to be monotonic in agreement with the direction.
+ if(!first) {
+ if(order == FORWARD && ds <= 1.e-6) {
+ ds = 1.e-6;
+ assert(ds > -1.);
+ }
+ if(order == BACKWARD && ds >= -1.e-6) {
+ ds = -1.e-6;
+ assert(ds < 1.);
+ }
+ }
+ spath += ds;
+ if(!first && spath == old_spath) {
+ errstat = 14;
+ return GTrackState();
+ }
+
+ // Return the outgoing state for a miss.
+ // No need to set nohit track & chisquare.
+ const ETrack& tre = trh.get_track();
+ double chsq( trh.get_chi_square() );
+ GTrackState result(spath, tre, gfit_stat, chsq);
+ SmoothPtr psmooth(new Smooth(deriv_dca, noise_dca));
+ result.set_smoother(psmooth);
+ return result;
+ }
+
+ // We have taken care of all cases above.
+ assert( false );
+ return GTrackState();
+
+}
+
+// Print states and assert false if DEBUG is enabled.
+void print_states(const GTrack::StateList& oldstates,
+ const GTrack::StateList& newstates) {
+ if ( DEBUG ) {
+ cout << endl;
+ print_list("Old states", oldstates);
+ cout << endl;
+ print_list("New states", newstates);
+ cout << endl;
+ assert(false);
+ }
+}
+
+} // end unnamed namespace
+
+//**********************************************************************
+// Member functions.
+//**********************************************************************
+
+// Constructor.
+
+SimpleGTrackFitter::
+SimpleGTrackFitter(const PropagatorPtr& pprop, int order,
+ /* cng D0ClusterRegistry* preg,*/ double chsq_max)
+: _pprop(pprop),
+ _order(order),
+ _pfit( new AddFitKalman() ),
+//cng _preg(preg),
+ _chsq_max(chsq_max) {
+ if ( _order!=FORWARD && _order!=BACKWARD ) {
+ bool invalid_order=true;
+ assert( invalid_order );
+ abort();
+ }
+
+//cng TrfClusterConverter conv;
+//cng TrfClusterConverter1D conv1d;
+}
+
+// Destructor
+SimpleGTrackFitter::~SimpleGTrackFitter(){
+ delete _pfit;
+}
+//**********************************************************************
+
+// Fit.
+
+int SimpleGTrackFitter::fit(GTrack& trg) const {
+
+ // Check bfield polarity
+ double bfield1 = trg.get_bfield();
+ double bfield2 = _pprop->get_bfield();
+ if(bfield1 * bfield2 < 0.) {
+ cerr << "SimpleGTrackFitter: Magnetic field has wrong polarity" << endl;
+ assert(false);
+ abort();
+ }
+
+ // Extract list of states.
+ GTrack::StateList oldstates = trg.get_states();
+
+ // Create the list of new states.
+ GTrack::StateList newstates;
+
+ // Require track to have one valid state.
+ if ( oldstates.size() < 1 ) {
+ trg = GTrack();
+ return 1;
+ }
+
+ // Extract starting track and error.
+ // We use the first or last state as the starting point for the fit.
+ GTrackState state0;
+ if ( _order == FORWARD ) state0 = *(oldstates.begin() );
+ if ( _order == BACKWARD ) state0 = *(oldstates.rbegin());
+
+ // The first state must be valid.
+ assert( state0.is_valid() );
+ if ( ! state0.is_valid() ) {
+ trg = GTrack();
+ return 2;
+ }
+
+ // Extract the startiong path distance and ETrack.
+ double s0 = state0.s();
+ ETrack tre0 = state0.track();
+ // Increase error.
+ {
+ TrackError err = tre0.get_error();
+ for ( int j=0; j<5; ++j ) {
+ for ( int i=0; i<=j; ++i ) {
+ err(i,j) *= ERRFAC;
+ }
+ }
+ tre0.set_error(err);
+ }
+
+ // Build starting HTrack.
+ HTrack trh(tre0);
+
+ // Loop over old states and create new states.
+ // For forward fit, loop forward.
+ if ( _order == FORWARD ) {
+
+ // Find the last cluster so we can record the fit as optimal
+ // for it and all following states.
+ GTrack::StateList::const_iterator istate_opt = oldstates.end();
+ for ( GTrack::StateList::const_iterator istate=oldstates.begin();
+ istate!=oldstates.end(); ++istate ) {
+ if ( istate->cluster() ) istate_opt = istate;
+ }
+ assert( istate_opt != oldstates.end() );
+
+ // Loop.
+ GTrackState::FitStatus gfit_stat = GTrackState::FORWARD;
+ bool first = true;
+ double spath = oldstates.begin()->s();
+ if(abs(spath) > 10000.)
+ spath = 0.;
+ for ( GTrack::StateList::const_iterator istate=oldstates.begin();
+ istate!=oldstates.end(); ++istate ) {
+ if ( istate == istate_opt ) gfit_stat = GTrackState::OPTIMAL;
+ int errstat = 0;
+ GTrackState newstate =
+ update_fit(trh, errstat, spath, /*_preg,*/ _pprop, _pfit,
+ _chsq_max, gfit_stat, *istate, first, _order);
+ if ( errstat ) {
+ trg = GTrack();
+ return 20 + errstat;
+ }
+ assert( newstate.is_valid() );
+ newstates.insert(newstate);
+ first = false;
+ }
+
+
+ // For backward fit, loop backward.
+ } else {
+
+ // Find the last cluster so we can record the fit as optimal
+ // for it and all following states.
+ GTrack::StateList::reverse_iterator istate_opt = oldstates.rend();
+ for ( GTrack::StateList::reverse_iterator istate= oldstates.rbegin();
+ istate!=oldstates.rend() ; ++istate ) {
+ if ( istate->cluster() ) istate_opt = istate;
+ }
+ assert( istate_opt != oldstates.rend() );
+
+ // Loop.
+ double spath = oldstates.rbegin()->s();
+ GTrackState::FitStatus gfit_stat = GTrackState::BACKWARD;
+ bool first = true;
+ if(abs(spath) > 10000.)
+ spath = 0.;
+ for ( GTrack::StateList::reverse_iterator istate= oldstates.rbegin();
+ istate!=oldstates.rend() ; ++istate ) {
+ if ( istate == istate_opt ) gfit_stat = GTrackState::OPTIMAL;
+ int errstat = 0;
+ GTrackState newstate =
+ update_fit(trh, errstat, spath, /*_preg,*/ _pprop, _pfit,
+ _chsq_max, gfit_stat, *istate, first, _order);
+ if ( errstat ) {
+ trg = GTrack();
+ return 40 + errstat;
+ }
+ assert( newstate.is_valid() );
+ newstates.insert(newstate);
+ first = false;
+ }
+ }
+
+ // Check that no states were lost.
+ if ( oldstates.size() != newstates.size() ) {
+ cout << endl << trg << endl << endl;
+ }
+
+ // Check that the last cluster and subsequent misses have
+ // optimal fits.
+ if ( _order == FORWARD ) {
+ for ( GTrack::StateList::reverse_iterator istate=newstates.rbegin();
+ istate!=newstates.rend(); ++istate ) {
+ const GTrackState& state = *istate;
+ if ( state.fit_status() != GTrackState::OPTIMAL ) {
+ print_states(oldstates, newstates);
+ return 4;
+ }
+ if ( state.cluster() ) break;
+ }
+ } else if ( _order == BACKWARD ) {
+ for ( GTrack::StateList::const_iterator istate=newstates.begin();
+ istate!=newstates.end(); ++istate ) {
+ const GTrackState& state = *istate;
+ if ( state.fit_status() != GTrackState::OPTIMAL ) {
+ print_states(oldstates, newstates);
+ return 5;
+ }
+ if ( state.cluster() ) break;
+ }
+ } else {
+ assert(false);
+ }
+
+ // Construct new GTrack.
+ trg = GTrack(newstates, bfield2);
+
+ // Check that no states were lost.
+ if ( oldstates.size() != newstates.size() ) {
+ cout << endl << trg << endl << endl;
+ }
+
+ // Check that no states were lost.
+ assert( oldstates.size() == newstates.size() );
+ if ( oldstates.size() != newstates.size() ) {
+ trg = GTrack();
+ return 3;
+ }
+
+ return 0;
+
+}
+
+//**********************************************************************
