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    : [email protected]
//-----------------------------------------------------------------------------
// 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 <iostream>
#include <float.h>
#include "TrkReco/T3DLineFitter.h"
#include "TrkReco/T3DLine.h"
#define HEP_SHORT_NAMES
//#include "panther/panther.h"
//#include MDC_H
#include "MdcTables/MdcTables.h"
#include "CLHEP/Matrix/Vector.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/Matrix.h"
#include "TrkReco/TMLink.h"
#include "TrkReco/TTrackBase.h"
 
#include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
#include "MdcCalibFunSvc/MdcCalibFunSvc.h"
 
#include "GaudiKernel/MsgStream.h"
#include "GaudiKernel/AlgFactory.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/IDataManagerSvc.h"
#include "GaudiKernel/SmartDataPtr.h"
#include "GaudiKernel/IDataProviderSvc.h"
#include "GaudiKernel/PropertyMgr.h"
#include "GaudiKernel/IJobOptionsSvc.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/StatusCode.h"
 
#include "GaudiKernel/ContainedObject.h"
#include "GaudiKernel/SmartRef.h"
#include "GaudiKernel/SmartRefVector.h"
#include "GaudiKernel/ObjectVector.h"
#include "EventModel/EventModel.h"
 
#include  "EvTimeEvent/RecEsTime.h"
 
using CLHEP::HepVector;
using CLHEP::HepSymMatrix;
using CLHEP::HepMatrix;
 
#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)
  : TMFitter(name),
//    _sag(true),_propagation(1),_tof(false){
    _sag(false),_propagation(0),_tof(true){   //Liuqg, tmp
 
}
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" << endreq;
    }
    //----------------------------------------------------//
 
  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;
}