T3DLineFitter.cxx

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// $Id: T3DLineFitter.cxx,v 1.10 2011/12/05 04:49:50 maoh Exp $
//-----------------------------------------------------------------------------
// Filename : T3DLineFitter.cc
// Section  : Tracking
// Owner    : Kenji Inami
// Email    : inami@bmail.kek.jp
//-----------------------------------------------------------------------------
// Description : A class to fit a TTrackBase object to a 3D line.
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------
 
#ifdef TRKRECO_DEBUG_DETAIL
#  ifndef TRKRECO_DEBUG
#    define TRKRECO_DEBUG
#  endif
#endif
#include "TrkReco/T3DLineFitter.h"
#include "TrkReco/T3DLine.h"
#include <float.h>
#include <iostream>
// #define HEP_SHORT_NAMES
// #include "panther/panther.h"
// #include MDC_H
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/Vector.h"
#include "MdcTables/MdcTables.h"
#include "TrkReco/TMLink.h"
#include "TrkReco/TTrackBase.h"
 
#include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
// #include "MdcCalibFunSvc/MdcCalibFunSvc.h"
 
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/IDataManagerSvc.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/PropertyMgr.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/StatusCode.h"
 
#include "EventModel/EventModel.h"
#include "GaudiKernel/ContainedObject.h"
#include "GaudiKernel/ObjectVector.h"
#include "GaudiKernel/SmartRef.h"
#include "GaudiKernel/SmartRefVector.h"
 
#include "EvTimeEvent/RecEsTime.h"
 
using CLHEP::HepMatrix;
using CLHEP::HepSymMatrix;
using CLHEP::HepVector;
 
#define T3DLine2DFit 2
 
/* ignore fortran codes ... zangsl 04/10/26
extern "C"
void
calcdc_driftdist_(int *,
                  int *,
                  int *,
                  float[3],
                  float[3],
                  float *,
                  float *,
                  float *);
 
extern "C"
void
calcdc_tof2_(int *, float *, float *, float *);
*/
 
T3DLineFitter::T3DLineFitter( const std::string& name ) // Liuqg, tmp
    : TMFitter( name ), _sag( false ), _propagation( 0 ), _tof( true ) {}
 
T3DLineFitter::T3DLineFitter( const std::string& name, bool m_sag, int m_prop, bool m_tof )
    : TMFitter( name ), _sag( m_sag ), _propagation( m_prop ), _tof( m_tof ) {}
 
T3DLineFitter::~T3DLineFitter() {}
 
void T3DLineFitter::sag( bool _in ) { _sag = _in; }
void T3DLineFitter::propagation( int _in ) { _propagation = _in; }
void T3DLineFitter::tof( bool _in ) { _tof = _in; }
 
void T3DLineFitter::drift( const T3DLine& t, const TMLink& l, float t0Offset, double& distance,
                           double& err ) const {
 
  // read t0 from TDS-------------------------------------//
  double       _t0_bes = -1.;
  IMessageSvc* msgSvc;
  Gaudi::svcLocator()->service( "MessageSvc", msgSvc );
  MsgStream log( msgSvc, "TCosmicFitter" );
 
  IDataProviderSvc* eventSvc = NULL;
  Gaudi::svcLocator()->service( "EventDataSvc", eventSvc );
 
  SmartDataPtr<RecEsTimeCol> aevtimeCol( eventSvc, "/Event/Recon/RecEsTimeCol" );
  if ( aevtimeCol )
  {
    RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
    _t0_bes                         = ( *iter_evt )->getTest();
  }
  else { log << MSG::WARNING << "Could not find RecEsTimeCol" << endmsg; }
  //----------------------------------------------------//
 
  const TMDCWireHit& h         = *l.hit();
  const HepPoint3D&  onTrack   = l.positionOnTrack();
  const HepPoint3D&  onWire    = l.positionOnWire();
  unsigned           leftRight = WireHitRight;
  //  if (onWire.cross(onTrack).z() < 0) leftRight = WireHitLeft;
  if ( ( onWire.x() * onTrack.y() - onWire.y() * onTrack.x() ) < 0 ) leftRight = WireHitLeft;
 
  //...No correction...
  /*  if ((t0Offset == 0.) && (_propagation==0) && (! _tof)) {
      distance = l.drift(leftRight);
      err = h.dDrift(leftRight);
      return;
    }*/
 
  //...TOF correction...
  //    momentum ???? or velocity -> assumued light velocity
  float tof = 0.;
  //  if (_tof) {
  //    double length = ((onTrack - t.x0())*t.k())/t.k().mag();
  /*    double tl = t.tanl();
      double length = ((onTrack - t.x0())*t.k())/sqrt(1+tl*tl);
      static const double Ic = 1/29.9792;   //1/[cm/ns]
      tof = length * Ic;
  */
  // cal the time pass through the chamber
  /*    const float Rad = 81; // cm
      float dRho = t.helix().a()[0];
      float Lproj = sqrt(Rad*Rad - dRho*dRho);
      float tlmd = t.helix().a()[4];
      float fct = sqrt(1. + tlmd * tlmd);
 
      float x[3]={ onWire.x(), onWire.y(), onWire.z()};
 
      float Rad_wire = sqrt(x[0]*x[0]+x[1]*x[1]); // cm
      float L_wire = sqrt(Rad_wire*Rad_wire - dRho*dRho);
      float flyLength = Lproj - L_wire;
      if (x[1]<0) flyLength = Lproj + L_wire;
      float Tfly = flyLength/29.98*sqrt(1.0+(0.105/10)*(0.105/10))*fct; //approxiate... cm/ns
  */
  double Tfly = _t0_bes / 220. * ( 110. - onWire.y() );
  //  }
 
  //...T0 and propagation corrections...
  int wire    = h.wire()->localId();
  int layerId = h.wire()->layerId();
  //  int wire = h.wire()->id();
  int side = leftRight;
  //  if (side==0) side = -1;
  //  float p[3] = {-t.sinPhi0(),t.cosPhi0(),t.tanl()};
  //  float x[3] = {onWire.x(), onWire.y(), onWire.z()};
  //  float time = h.reccdc()->tdc + t0Offset - tof;
  float            time = h.reccdc()->tdc - Tfly;
  float            dist;
  float            edist;
  IMdcCalibFunSvc* l_mdcCalFunSvc;
  Gaudi::svcLocator()->service( "MdcCalibFunSvc", l_mdcCalFunSvc );
  double T0     = l_mdcCalFunSvc->getT0( layerId, wire );
  double rawadc = 0.;
  rawadc        = h.reccdc()->adc;
 
  double timeWalk = l_mdcCalFunSvc->getTimeWalk( layerId, rawadc );
  //  int prop = _propagation;
  double drifttime = time - T0 - timeWalk;
  //  dist = l_mdcCalFunSvc->rawTimeNoTOFToDist(time, layerId, wire, side, 0.0);
  dist  = l_mdcCalFunSvc->driftTimeToDist( drifttime, layerId, wire, side, 0.0 );
  edist = l_mdcCalFunSvc->getSigma( layerId, side, dist, 0.0 );
  //  if(layerId<20) edist = 999999.;
  dist  = dist / 10.0; // mm->cm
  edist = edist / 10.0;
  /*zsl
    calcdc_driftdist_(& prop,
                      & wire,
                      & side,
                      p,
                      x,
                      & time,
                      & dist,
                      & edist);
   */
  distance = (double)dist;
  err      = (double)edist;
  return;
}
 
int T3DLineFitter::fit( TTrackBase& tb ) const { return fit( tb, 0 ); }
 
int T3DLineFitter::fit( TTrackBase& tb, float t0Offset ) const {
 
  // std::cout<<"T3DLineFitter::fit  start"<<std::endl;
 
  //...Type check...
  if ( tb.objectType() != Line3D ) return TFitUnavailable;
  T3DLine& t = (T3DLine&)tb;
 
  //...Already fitted ?...
  if ( t.fitted() ) return TFitAlreadyFitted;
 
  //...Count # of hits...
  AList<TMLink> cores        = t.cores();
  unsigned      nCores       = cores.length();
  unsigned      nStereoCores = NStereoHits( cores );
 
  //...Check # of hits...
  bool flag2D = false;
  if ( ( nStereoCores == 0 ) && ( nCores > 3 ) ) flag2D = true;
  else if ( ( nStereoCores < 2 ) || ( nCores - nStereoCores < 3 ) ) return TFitErrorFewHits;
 
  //...Move pivot to ORIGIN...
  const HepPoint3D pivot_bak = t.pivot();
  t.pivot( ORIGIN );
 
  //...Setup...
  Vector a( 4 ), da( 4 );
  a = t.a();
  Vector                 dxda( 4 );
  Vector                 dyda( 4 );
  Vector                 dzda( 4 );
  Vector                 dDda( 4 );
  Vector                 dchi2da( 4 );
  SymMatrix              d2chi2d2a( 4, 0 );
  static const SymMatrix zero4( 4, 0 );
  double                 chi2;
  double                 chi2Old = DBL_MAX;
  double                 factor  = 1.0;
  int                    err     = 0;
  SymMatrix              e( 2, 0 );
  Vector                 f( 2 );
 
  //...Fitting loop...
  unsigned nTrial = 0;
  while ( nTrial < 100 )
  {
 
    //...Set up...
    chi2 = 0;
    for ( unsigned j = 0; j < 4; j++ ) dchi2da[j] = 0;
    d2chi2d2a = zero4;
 
    //...Loop with hits...
    unsigned i = 0;
    while ( TMLink* l = cores[i++] )
    {
      const TMDCWireHit& h = *l->hit();
 
      //...Cal. closest points...
      t.approach( *l, _sag );
      const HepPoint3D& onTrack   = l->positionOnTrack();
      const HepPoint3D& onWire    = l->positionOnWire();
      unsigned          leftRight = WireHitRight;
      if ( onWire.cross( onTrack ).z() < 0. ) leftRight = WireHitLeft;
 
      //...Obtain drift distance and its error...
      double distance;
      double eDistance;
      drift( t, *l, t0Offset, distance, eDistance );
      l->drift( distance, 0 );
      l->drift( distance, 1 );
      l->dDrift( eDistance, 0 );
      l->dDrift( eDistance, 1 );
      double eDistance2 = eDistance * eDistance;
      // cout<<"distance: "<< distance << " eDistance: " << eDistance << endl;
 
      //...Residual...
      HepVector3D v         = onTrack - onWire;
      double      vmag      = v.mag();
      double      dDistance = vmag - distance;
 
      HepVector3D vw;
      //...dxda...
      this->dxda( *l, t, dxda, dyda, dzda, vw );
 
      //...Chi2 related...
      dDda = ( vmag > 0. ) ? ( ( v.x() * ( 1. - vw.x() * vw.x() ) - v.y() * vw.x() * vw.y() -
                                 v.z() * vw.x() * vw.z() ) *
                                   dxda +
                               ( v.y() * ( 1. - vw.y() * vw.y() ) - v.z() * vw.y() * vw.z() -
                                 v.x() * vw.y() * vw.x() ) *
                                   dyda +
                               ( v.z() * ( 1. - vw.z() * vw.z() ) - v.x() * vw.z() * vw.x() -
                                 v.y() * vw.z() * vw.y() ) *
                                   dzda ) /
                                 vmag
                           : Vector( 4, 0 );
      if ( vmag <= 0.0 )
      {
        std::cout << "    in fit " << onTrack << ", " << onWire;
        h.dump();
      }
      dchi2da += ( dDistance / eDistance2 ) * dDda;
      d2chi2d2a += vT_times_v( dDda ) / eDistance2;
      double pChi2 = dDistance * dDistance / eDistance2;
      chi2 += pChi2;
 
      //...Store results...
      l->update( onTrack, onWire, leftRight, pChi2 );
    }
 
    //...Check condition...
    double change = chi2Old - chi2;
 
    if ( fabs( change ) < 1.0e-5 ) break;
    if ( change < 0. )
    {
      a += factor * da; // recover
      factor *= 0.1;
    }
    else
    {
      chi2Old = chi2;
      if ( flag2D )
      {
        f     = dchi2da.sub( 1, 2 );
        e     = d2chi2d2a.sub( 1, 2 );
        f     = solve( e, f );
        da[0] = f[0];
        da[1] = f[1];
        da[2] = 0;
        da[3] = 0;
      }
      else
      {
        //...Cal. helix parameters for next loop...
        da = solve( d2chi2d2a, dchi2da );
      }
    }
    a -= factor * da;
    t.a( a );
    ++nTrial;
  }
 
  //...Cal. error matrix...
  SymMatrix Ea( 4, 0 );
  unsigned  dim;
  if ( flag2D )
  {
    dim = 2;
    SymMatrix Eb( 3, 0 ), Ec( 3, 0 );
    Eb       = d2chi2d2a.sub( 1, 3 );
    Ec       = Eb.inverse( err );
    Ea[0][0] = Ec[0][0];
    Ea[0][1] = Ec[0][1];
    Ea[0][2] = Ec[0][2];
    Ea[1][1] = Ec[1][1];
    Ea[1][2] = Ec[1][2];
    Ea[2][2] = Ec[2][2];
  }
  else
  {
    dim = 4;
    Ea  = d2chi2d2a.inverse( err );
  }
 
  //...Store information...
  if ( !err )
  {
    t.a( a );
    t.Ea( Ea );
    t._fitted = true;
    if ( flag2D ) err = T3DLine2DFit;
  }
  else { err = TFitFailed; }
 
  t._ndf  = nCores - dim;
  t._chi2 = chi2;
 
  //...Recover pivot...
  t.pivot( pivot_bak );
 
  return err;
}
 
int T3DLineFitter::dxda( const TMLink& l, const T3DLine& t, Vector& dxda, Vector& dyda,
                         Vector& dzda, HepVector3D& wireDirection ) const {
  //   onTrack = x0 + t * k
  //   onWire  = w0 + s * wireDirection
  //...Setup...
  const TMDCWire&   w       = *l.wire();
  const HepVector3D k       = t.k();
  const double      cosPhi0 = t.cosPhi0();
  const double      sinPhi0 = t.sinPhi0();
  const double      dr      = t.dr();
  const HepPoint3D& onWire  = l.positionOnWire();
  const HepPoint3D& onTrack = l.positionOnTrack();
  //  const Vector3 u = onTrack - onWire;
  const double t_t = ( onWire - t.x0() ).dot( k ) / k.mag2();
 
  //...Sag correction...
  HepPoint3D xw                   = w.xyPosition();
  HepPoint3D wireBackwardPosition = w.backwardPosition();
  wireDirection                   = w.direction();
  if ( _sag )
    w.wirePosition( onWire.z(), xw, wireBackwardPosition, (HepVector3D&)wireDirection );
  const HepVector3D& v = wireDirection;
 
  //   onTrack = x0 + t * k
  //   onWire  = w0 + s * v
 
  const double v_k = v.dot( k );
  const double tvk = v_k * v_k - k.mag2();
  if ( tvk == 0 ) return ( -1 );
 
  const HepVector3D& dxdt_a = k;
 
  const HepVector3D dxda_t[4] = {
      HepVector3D( cosPhi0, sinPhi0, 0 ),
      HepVector3D( -dr * sinPhi0 - t_t * cosPhi0, dr * cosPhi0 - t_t * sinPhi0, 0 ),
      HepVector3D( 0, 0, 1 ), HepVector3D( 0, 0, t_t ) };
 
  const HepVector3D dx0da[4] = { HepVector3D( cosPhi0, sinPhi0, 0 ),
                                 HepVector3D( -dr * sinPhi0, dr * cosPhi0, 0 ),
                                 HepVector3D( 0, 0, 1 ), HepVector3D( 0, 0, 0 ) };
 
  const HepVector3D dkda[4] = { HepVector3D( 0, 0, 0 ), HepVector3D( -cosPhi0, -sinPhi0, 0 ),
                                HepVector3D( 0, 0, 0 ), HepVector3D( 0, 0, 1 ) };
 
  const HepVector3D d    = t.x0() - wireBackwardPosition;
  const HepVector3D kvkv = k - v_k * v;
 
  for ( int i = 0; i < 4; i++ )
  {
    const double v_dkda = v.dot( dkda[i] );
 
    const double dtda =
        dx0da[i].dot( kvkv ) / tvk + d.dot( dkda[i] - v_dkda * v ) / tvk -
        d.dot( kvkv ) * 2 * ( v_k * v_dkda - k.dot( dkda[i] ) ) / ( tvk * tvk );
 
    const HepVector3D dxda3D = dxda_t[i] + dtda * dxdt_a;
 
    dxda[i] = dxda3D.x();
    dyda[i] = dxda3D.y();
    dzda[i] = dxda3D.z();
  }
 
  return 0;
}