MilleAlign.cxx

Go to the documentation of this file.

#include "MdcAlignAlg/MilleAlign.h"
#include "MdcAlignAlg/Alignment.h"
#include "MdcAlignAlg/MdcAlignAlg.h"
 
#include "GaudiKernel/Bootstrap.h"
#include "GaudiKernel/IMessageSvc.h"
#include "GaudiKernel/ISvcLocator.h"
#include "GaudiKernel/StatusCode.h"
 
#include "Identifier/MdcID.h"
 
#include <cmath>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
 
#ifndef ENABLE_BACKWARDS_COMPATIBILITY
//  backwards compatblty wll be enabled ONLY n CLHEP 1.9
typedef HepGeom::Point3D<double> HepPoint3D;
#endif
 
#include "TSpline.h"
 
using namespace std;
 
MilleAlign::MilleAlign() {
  for ( int lay = 0; lay < LAYERNMAX; lay++ )
  {
    m_resiCut[lay] = 1.5;
    if ( lay < 8 )
    {
      m_docaCut[lay][0] = 0.5;
      m_docaCut[lay][1] = 5.5;
    }
    else
    {
      m_docaCut[lay][0] = 0.5;
      m_docaCut[lay][1] = 7.5;
    }
  }
}
 
MilleAlign::~MilleAlign() {}
 
void MilleAlign::clear() {
  delete m_hresAll;
  delete m_hresInn;
  delete m_hresStp;
  delete m_hresOut;
  for ( int lay = 0; lay < LAYERNMAX; lay++ ) delete m_hresLay[lay];
  delete m_hresAllRec;
  for ( int lay = 0; lay < LAYERNMAX; lay++ ) delete m_hresLayRec[lay];
  delete m_hddoca;
  delete m_pMilleAlign;
}
 
void MilleAlign::initialize( TObjArray* hlist, IMdcGeomSvc* mdcGeomSvc,
                             IMdcCalibFunSvc* mdcFunSvc, IMdcUtilitySvc* mdcUtilitySvc ) {
  IMessageSvc* msgSvc;
  Gaudi::svcLocator()->service( "MessageSvc", msgSvc );
  MsgStream log( msgSvc, "MilleAlign" );
  log << MSG::INFO << "MilleAlign::initialize()" << endmsg;
 
  m_hlist         = hlist;
  m_mdcGeomSvc    = mdcGeomSvc;
  m_mdcFunSvc     = mdcFunSvc;
  m_mdcUtilitySvc = mdcUtilitySvc;
 
  // initialize hitograms
  m_hresAll = new TH1F( "HResAllInc", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hresAll );
 
  m_hresInn = new TH1F( "HResInnInc", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hresInn );
 
  m_hresStp = new TH1F( "HResStpInc", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hresStp );
 
  m_hresOut = new TH1F( "HResOutInc", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hresOut );
 
  char hname[200];
  for ( int lay = 0; lay < LAYERNMAX; lay++ )
  {
    sprintf( hname, "Res_Layer%02d", lay );
    m_hresLay[lay] = new TH1F( hname, "", 200, -1.0, 1.0 );
    m_hlist->Add( m_hresLay[lay] );
  }
 
  m_hresAllRec = new TH1F( "HResAllRecInc", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hresAllRec );
  for ( int lay = 0; lay < LAYERNMAX; lay++ )
  {
    sprintf( hname, "Res_LayerRec%02d", lay );
    m_hresLayRec[lay] = new TH1F( hname, "", 200, -1.0, 1.0 );
    m_hlist->Add( m_hresLayRec[lay] );
  }
 
  // for debug
  m_hddoca = new TH1F( "delt_doca", "", 200, -1.0, 1.0 );
  m_hlist->Add( m_hddoca );
 
  for ( int lay = 0; lay < LAYERNMAX; lay++ )
  {
    sprintf( hname, "delt_docaLay%02d", lay );
    m_hddocaLay[lay] = new TH1F( hname, "", 200, -1.0, 1.0 );
    m_hlist->Add( m_hddocaLay[lay] );
  }
 
  // initialize millepede
  m_nglo    = NEP;
  m_nloc    = NTRKPAR;
  m_nGloHit = 2 * NDOFALIGN;
  m_npar    = NDOFALIGN * m_nglo;
 
  int i;
  for ( i = 0; i < NDOFALIGN; i++ )
  {
    m_dofs[i] = g_dofs[i];
    m_sigm[i] = g_Sigm[i];
  }
 
  m_pMilleAlign = new Millepede();
  m_pMilleAlign->InitMille( &m_dofs[0], &m_sigm[0], m_nglo, m_nloc, g_start_chi_cut, 3,
                            g_res_cut, g_res_cut_init );
 
  m_derGB.resize( m_npar );
  m_derNonLin.resize( m_npar );
  m_par.resize( m_npar );
  m_error.resize( m_npar );
  m_pull.resize( m_npar );
 
  m_derLC.resize( m_nloc );
 
  // contraints
  std::vector<double> constTX;
  std::vector<double> constTY;
  std::vector<double> constRZ;
 
  std::vector<double> constTXE;
  std::vector<double> constTXW;
  std::vector<double> constTYE;
  std::vector<double> constTYW;
  std::vector<double> constRZE;
  std::vector<double> constRZW;
 
  constTX.resize( m_npar );
  constTY.resize( m_npar );
  constRZ.resize( m_npar );
 
  constTXE.resize( m_npar );
  constTXW.resize( m_npar );
  constTYE.resize( m_npar );
  constTYW.resize( m_npar );
  constRZE.resize( m_npar );
  constRZW.resize( m_npar );
 
  for ( i = 0; i < m_npar; i++ )
  {
    constTX[i] = 0.0;
    constTY[i] = 0.0;
    constRZ[i] = 0.0;
 
    constTXE[i] = 0.0;
    constTXW[i] = 0.0;
    constTYE[i] = 0.0;
    constTYW[i] = 0.0;
    constRZE[i] = 0.0;
    constRZW[i] = 0.0;
  }
  constTX[7]  = 1.0;
  constTX[15] = 1.0;
  constTY[23] = 1.0;
  constTY[31] = 1.0;
  constRZ[39] = 1.0;
  constRZ[47] = 1.0;
 
  constTXE[7]  = 1.0;
  constTXW[15] = 1.0;
  constTYE[23] = 1.0;
  constTYW[31] = 1.0;
  constRZE[39] = 1.0;
  constRZW[47] = 1.0;
 
  // m_pMilleAlign -> ConstF(&constTX[0], 0.0);
  // m_pMilleAlign -> ConstF(&constTY[0], 0.0);
  //      m_pMilleAlign -> ConstF(&constRZ[0], 0.0);
 
  m_pMilleAlign->ConstF( &constTXE[0], 0.0 );
  m_pMilleAlign->ConstF( &constTXW[0], 0.0 );
  m_pMilleAlign->ConstF( &constTYE[0], 0.0 );
  m_pMilleAlign->ConstF( &constTYW[0], 0.0 );
  m_pMilleAlign->ConstF( &constRZE[0], 0.0 );
  m_pMilleAlign->ConstF( &constRZW[0], 0.0 );
}
 
bool MilleAlign::fillHist( MdcAliEvent* event ) {
  IMessageSvc* msgSvc;
  Gaudi::svcLocator()->service( "MessageSvc", msgSvc );
  MsgStream log( msgSvc, "MilleAlign" );
  log << MSG::DEBUG << "MilleAlign::fillTree()" << endmsg;
 
  int               recFlag;
  int               itrk;
  int               ihit;
  int               fgGetDoca;
  int               lr;
  int               lay;
  int               cel;
  double            doca;
  double            dmeas;
  double            zhit;
  double            resi;
  double            resiRec;
  double            deri;
  double            hitSigm;
  double            hitpos[3];
  double            wpos[7]; // wpos[6] is wire tension
  const MdcGeoWire* pWire;
 
  double trkpar[NTRKPAR];
  double trkparms[NTRKPARALL]; // track parameters and errors
 
  // numerical derivative
  int ipar;
  int iparGB;
 
  MdcAliRecTrk* rectrk;
  MdcAliRecHit* rechit;
 
  int ntrk = event->getNTrk();
  if ( ( ntrk < m_param.nTrkCut[0] ) || ( ntrk > m_param.nTrkCut[1] ) ) return false;
 
  for ( itrk = 0; itrk < ntrk; itrk++ )
  {
    rectrk  = event->getRecTrk( itrk );
    recFlag = rectrk->getStat();
 
    trkpar[0] = rectrk->getDr();
    trkpar[1] = rectrk->getPhi0();
    trkpar[2] = rectrk->getKappa();
    trkpar[3] = rectrk->getDz();
    trkpar[4] = rectrk->getTanLamda();
 
    int nHit = rectrk->getNHits();
    if ( nHit < m_param.nHitCut ) continue;
    if ( fabs( trkpar[0] ) > m_param.drCut ) continue;
    if ( fabs( trkpar[3] ) > m_param.dzCut ) continue;
 
    HepVector    rechelix = rectrk->getHelix();
    HepVector    helix    = rectrk->getHelix();
    HepSymMatrix helixErr = rectrk->getHelixErr();
 
    int nHitUse = 0;
    for ( ihit = 0; ihit < nHit; ihit++ )
    {
      rechit  = rectrk->getRecHit( ihit );
      lr      = rechit->getLR();
      lay     = rechit->getLayid();
      cel     = rechit->getCellid();
      pWire   = m_mdcGeomSvc->Wire( lay, cel );
      dmeas   = rechit->getDmeas();
      zhit    = rechit->getZhit();
      hitSigm = rechit->getErrDmeas();
 
      wpos[0] = pWire->Backward().x(); // east end
      wpos[1] = pWire->Backward().y();
      wpos[2] = pWire->Backward().z();
      wpos[3] = pWire->Forward().x(); // west end
      wpos[4] = pWire->Forward().y();
      wpos[5] = pWire->Forward().z();
      wpos[6] = pWire->Tension();
 
      double docaRec = rechit->getDocaInc();
      double doca    = ( m_mdcUtilitySvc->doca( lay, cel, helix, helixErr ) ) * 10.0;
 
      resi = -1.0 * dmeas - doca;
      if ( ( fabs( doca ) < m_docaCut[lay][0] ) || ( fabs( doca ) > m_docaCut[lay][1] ) ||
           ( fabs( resi ) > m_resiCut[lay] ) )
        continue;
      nHitUse++;
 
      resiRec = rechit->getResiIncLR();
 
      double dd = fabs( doca ) - fabs( rechit->getDocaInc() );
      m_hddoca->Fill( dd );
      m_hddocaLay[lay]->Fill( dd );
 
      // fill histograms
      m_hresAll->Fill( resi );
      if ( lay < 8 ) m_hresInn->Fill( resi );
      else if ( lay < 20 ) m_hresStp->Fill( resi );
      else m_hresOut->Fill( resi );
      m_hresLay[lay]->Fill( resi );
 
      m_hresAllRec->Fill( resiRec );
      m_hresLayRec[lay]->Fill( resiRec );
 
      // reset the derivatives arrays
      m_pMilleAlign->ZerLoc( &m_derGB[0], &m_derLC[0], &m_derNonLin[0] );
 
      // derivatives of local parameters
      for ( ipar = 0; ipar < m_nloc; ipar++ )
      {
        if ( !getDeriLoc( ipar, lay, cel, rechelix, helixErr, deri ) )
        {
          cout << "getDeriLoc == false!" << setw( 12 ) << itrk << setw( 12 ) << ipar << endl;
          return false;
        }
        m_derLC[ipar] = deri;
      }
      // derivatives of global parameters
      // ipar 0 - 5: dx_east, dx_west, dy_east, dy_west, rz_east, rz_west
      for ( ipar = 0; ipar < m_nGloHit; ipar++ )
      {
        iparGB = getAlignParId( lay, ipar );
        if ( !getDeriGlo( ipar, iparGB, lay, cel, helix, helixErr, wpos, deri ) )
        {
          cout << "getDeriGlo == false!" << setw( 12 ) << itrk << setw( 12 ) << ipar << endl;
          return false;
        }
        m_derGB[iparGB] = deri;
      }
      m_pMilleAlign->EquLoc( &m_derGB[0], &m_derLC[0], &m_derNonLin[0], resi, hitSigm );
    } // loop of nhit
 
    // local fit in Millepede
    bool sc = m_pMilleAlign->FitLoc( m_pMilleAlign->GetTrackNumber(), trkparms, 0 );
    if ( sc ) m_pMilleAlign->SetTrackNumber( m_pMilleAlign->GetTrackNumber() + 1 );
  } // track loop
  return true;
}
 
void MilleAlign::updateConst( MdcAlignPar* alignPar ) {
  IMessageSvc* msgSvc;
  Gaudi::svcLocator()->service( "MessageSvc", msgSvc );
  MsgStream log( msgSvc, "MilleAlign" );
  log << MSG::INFO << "MilleAlign::updateConst()" << endmsg;
 
  m_pMilleAlign->MakeGlobalFit( &m_par[0], &m_error[0], &m_pull[0] );
 
  int    iEP;
  int    ipar;
  double val;
  double err;
  for ( int i = 0; i < NDOFALIGN; i++ )
  {
    for ( iEP = 0; iEP < NEP; iEP++ )
    {
      ipar = i * NEP + iEP;
      if ( m_dofs[i] )
      {
        val = m_par[ipar];
        err = m_error[ipar];
      }
      else
      {
        val = 0.0;
        err = 0.0;
      }
 
      if ( 0 == i )
      {
        alignPar->setDelDx( iEP, val );
        alignPar->setErrDx( iEP, err );
      }
      else if ( 1 == i )
      {
        alignPar->setDelDy( iEP, val );
        alignPar->setErrDy( iEP, err );
      }
      else if ( 2 == i )
      {
        alignPar->setDelRz( iEP, val / 1000.0 ); // mrad -> rad
        alignPar->setErrRz( iEP, err / 1000.0 ); // mrad -> rad
      }
    }
  }
}
 
int MilleAlign::getAlignParId( int lay, int iparHit ) {
  int ip;
  if ( lay < 8 ) ip = 0;
  else if ( lay < 10 ) ip = 1;
  else if ( lay < 12 ) ip = 2;
  else if ( lay < 14 ) ip = 3;
  else if ( lay < 16 ) ip = 4;
  else if ( lay < 18 ) ip = 5;
  else if ( lay < 20 ) ip = 6;
  else ip = 7;
 
  // iparHit 0 - 5: dx_east, dx_west, dy_east, dy_west, rz_east, rz_west
  int ipar = iparHit * 8 + ip;
  return ipar;
}
 
bool MilleAlign::getDeriLoc( int ipar, int lay, int cel, HepVector rechelix,
                             HepSymMatrix& helixErr, double& deri ) {
  int       i;
  double    doca;
  HepVector sampar( NTRKPAR, 0 );
  double    xxLC[gNsamLC];
  double    yyLC[gNsamLC];
 
  for ( i = 0; i < m_nloc; i++ ) sampar[i] = rechelix[i];
  double startpar = rechelix[ipar] - 0.5 * gStepLC[ipar] * (double)gNsamLC;
 
  for ( i = 0; i < gNsamLC; i++ )
  {
    sampar[ipar] = startpar + (double)i * gStepLC[ipar];
    xxLC[i]      = sampar[ipar];
    if ( 0 == ipar || 3 == ipar ) xxLC[i] *= 10.; // cm -> mm
 
    HepVector helix            = sampar;
    bool      passCellRequired = false;
    doca = ( m_mdcUtilitySvc->doca( lay, cel, helix, helixErr, passCellRequired ) ) * 10.0;
 
    // maqm modify if(NULL == doca){
    if ( doca == 0 )
    {
      //         cout << "in getDeriLoc, doca = " << doca << endl;
      return false;
    }
    yyLC[i] = doca;
  }
 
  if ( 0 == ipar || 3 == ipar ) rechelix[ipar] *= 10.; // cm -> mm
  TSpline3* pSpline3 = new TSpline3( "deri", xxLC, yyLC, gNsamLC );
  deri               = pSpline3->Derivative( rechelix[ipar] );
  delete pSpline3;
  return true;
}
 
bool MilleAlign::getDeriGlo( int iparHit, int iparGB, int lay, int cel, HepVector helix,
                             HepSymMatrix& helixErr, double wpos[], double& deri ) {
  int    i;
  double doca;
  double xxGB[gNsamGB];
  double yyGB[gNsamGB];
  double dAlignPar;
  double dAlignParini = 0.0;
  double startpar     = dAlignParini - 0.5 * gStepGB[iparGB] * (double)gNsamGB;
  double wposSam[7];
  for ( i = 0; i < 7; i++ ) wposSam[i] = wpos[i]; // 0-2:east; 3-5:west
 
  for ( i = 0; i < gNsamGB; i++ )
  {
    dAlignPar = startpar + (double)i * gStepGB[iparGB];
    xxGB[i]   = dAlignPar;
    if ( 0 == iparHit )
    { // dx_east
      wposSam[0] = wpos[0] + dAlignPar;
    }
    else if ( 1 == iparHit )
    { // dx_west
      wposSam[3] = wpos[3] + dAlignPar;
    }
    else if ( 2 == iparHit )
    { // dy_east
      wposSam[1] = wpos[1] + dAlignPar;
    }
    else if ( 3 == iparHit )
    { // dy_west
      wposSam[4] = wpos[4] + dAlignPar;
    }
    else if ( 4 == iparHit )
    { // rz_east
      wposSam[0] = wpos[0] - ( wpos[1] * dAlignPar * 0.001 );
      wposSam[1] = wpos[1] + ( wpos[0] * dAlignPar * 0.001 );
    }
    else if ( 5 == iparHit )
    { // rz_west
      wposSam[3] = wpos[3] - ( wpos[4] * dAlignPar * 0.001 );
      wposSam[4] = wpos[4] + ( wpos[3] * dAlignPar * 0.001 );
    }
 
    //    bool passCellRequired = false;
    HepPoint3D eastP( wposSam[0] / 10., wposSam[1] / 10., wposSam[2] / 10. );
    HepPoint3D westP( wposSam[3] / 10., wposSam[4] / 10., wposSam[5] / 10. );
    doca =
        ( m_mdcUtilitySvc->doca( lay, cel, eastP, westP, helix, helixErr ) ) * 10.0; // cm->mm
 
    // maqm modify
    // if(NULL == doca) return false;
    if ( doca == 0 ) return false;
    yyGB[i] = doca;
  }
 
  TSpline3* pSpline3 = new TSpline3( "deri", xxGB, yyGB, gNsamGB );
  deri               = pSpline3->Derivative( dAlignParini );
  delete pSpline3;
  return true;
}