BOSS_7.0.2: /home/bes3soft/bes3soft/Boss/7.0.2/dist/7.0.2/Mdc/MdcAlignAlg/MdcAlignAlg-00-01-04/src/MilleAlign.cxx Source File

00001 #include "MdcAlignAlg/MilleAlign.h"
00002 #include "MdcAlignAlg/MdcAlignAlg.h"
00003 #include "MdcAlignAlg/Alignment.h"
00004 
00005 #include "GaudiKernel/IMessageSvc.h"
00006 #include "GaudiKernel/StatusCode.h"
00007 #include "GaudiKernel/ISvcLocator.h"
00008 #include "GaudiKernel/Bootstrap.h"
00009 
00010 #include "Identifier/MdcID.h"
00011 
00012 #include <iostream>
00013 #include <fstream>
00014 #include <iomanip>
00015 #include <string> 
00016 #include <cstring>
00017 #include <cmath>
00018 
00019 #ifndef ENABLE_BACKWARDS_COMPATIBILITY
00020 //  backwards compatblty wll be enabled ONLY n CLHEP 1.9
00021 typedef HepGeom::Point3D<double> HepPoint3D;
00022 #endif
00023  
00024 #include "TSpline.h"
00025 
00026 using namespace std;
00027 
00028 MilleAlign::MilleAlign(){
00029      for(int lay=0; lay<LAYERNMAX; lay++){
00030           m_resiCut[lay] = 1.5;
00031           if(lay<8){
00032                m_docaCut[lay][0] = 0.5;
00033                m_docaCut[lay][1] = 5.5;
00034           } else{
00035                m_docaCut[lay][0] = 0.5;
00036                m_docaCut[lay][1] = 7.5;
00037           }
00038      }
00039 }
00040 
00041 MilleAlign::~MilleAlign(){
00042 }
00043 
00044 void MilleAlign::clear(){
00045      delete m_hresAll;
00046      delete m_hresInn;
00047      delete m_hresStp;
00048      delete m_hresOut;
00049      for(int lay=0; lay<LAYERNMAX; lay++) delete m_hresLay[lay];
00050      delete m_hresAllRec;
00051      for(int lay=0; lay<LAYERNMAX; lay++) delete m_hresLayRec[lay];
00052      delete m_hddoca;
00053      delete m_pMilleAlign;
00054 }
00055 
00056 void MilleAlign::initialize(TObjArray* hlist, IMdcGeomSvc* mdcGeomSvc,
00057                             IMdcCalibFunSvc* mdcFunSvc){
00058      IMessageSvc* msgSvc;
00059      Gaudi::svcLocator() -> service("MessageSvc", msgSvc);
00060      MsgStream log(msgSvc, "MilleAlign");
00061      log << MSG::INFO << "MilleAlign::initialize()" << endreq;
00062 
00063      // Initialze MdcUtilitySvc
00064      IMdcUtilitySvc* imdcUtilitySvc;
00065      StatusCode sc = Gaudi::svcLocator() -> service ("MdcUtilitySvc",imdcUtilitySvc);
00066      m_mdcUtilitySvc= dynamic_cast<MdcUtilitySvc*> (imdcUtilitySvc);
00067      if ( sc.isFailure() ){
00068           log << MSG::FATAL << "Could not load MdcUtilitySvc!" << endreq;
00069      }
00070 
00071      m_hlist = hlist;
00072      m_mdcGeomSvc = mdcGeomSvc;
00073      m_mdcFunSvc = mdcFunSvc;
00074 
00075      // initialize hitograms
00076      m_hresAll = new TH1F("HResAllInc", "", 200, -1.0, 1.0);
00077      m_hlist->Add(m_hresAll);
00078 
00079      m_hresInn = new TH1F("HResInnInc", "", 200, -1.0, 1.0);
00080      m_hlist->Add(m_hresInn);
00081 
00082      m_hresStp = new TH1F("HResStpInc", "", 200, -1.0, 1.0);
00083      m_hlist->Add(m_hresStp);
00084 
00085      m_hresOut = new TH1F("HResOutInc", "", 200, -1.0, 1.0);
00086      m_hlist->Add(m_hresOut);
00087 
00088      char hname[200];
00089      for(int lay=0; lay<LAYERNMAX; lay++){
00090           sprintf(hname, "Res_Layer%02d", lay);
00091           m_hresLay[lay] = new TH1F(hname, "", 200, -1.0, 1.0);
00092           m_hlist->Add(m_hresLay[lay]);
00093      }
00094 
00095      m_hresAllRec = new TH1F("HResAllRecInc", "", 200, -1.0, 1.0);
00096      m_hlist->Add(m_hresAllRec);
00097      for(int lay=0; lay<LAYERNMAX; lay++){
00098           sprintf(hname, "Res_LayerRec%02d", lay);
00099           m_hresLayRec[lay] = new TH1F(hname, "", 200, -1.0, 1.0);
00100           m_hlist->Add(m_hresLayRec[lay]);
00101      }
00102 
00103      // for debug
00104      m_hddoca = new TH1F("delt_doca", "", 200, -1.0, 1.0);
00105      m_hlist->Add(m_hddoca);
00106 
00107      for(int lay=0; lay<LAYERNMAX; lay++){
00108           sprintf(hname, "delt_docaLay%02d", lay);
00109           m_hddocaLay[lay] = new TH1F(hname, "", 200, -1.0, 1.0);
00110           m_hlist->Add(m_hddocaLay[lay]);
00111      }
00112 
00113      // initialize millepede
00114      m_nglo = NEP;
00115      m_nloc = NTRKPAR;
00116      m_nGloHit = 2 * NDOFALIGN;
00117      m_npar = NDOFALIGN * m_nglo;
00118 
00119      int i;
00120      for(i=0; i<NDOFALIGN; i++){
00121           m_dofs[i] = g_dofs[i];
00122           m_sigm[i] = g_Sigm[i];
00123      }
00124 
00125      m_pMilleAlign = new Millepede();
00126      m_pMilleAlign -> InitMille(&m_dofs[0], &m_sigm[0], m_nglo, m_nloc,
00127                                 g_start_chi_cut, 3, g_res_cut, g_res_cut_init);
00128 
00129      m_derGB.resize(m_npar);
00130      m_derNonLin.resize(m_npar);
00131      m_par.resize(m_npar);
00132      m_error.resize(m_npar);
00133      m_pull.resize(m_npar);
00134 
00135      m_derLC.resize(m_nloc);
00136 
00137      // contraints
00138      std::vector<double> constTX;
00139      std::vector<double> constTY;
00140      std::vector<double> constRZ;
00141 
00142      std::vector<double> constTXE;
00143      std::vector<double> constTXW;
00144      std::vector<double> constTYE;
00145      std::vector<double> constTYW;
00146      std::vector<double> constRZE;
00147      std::vector<double> constRZW;
00148 
00149      constTX.resize(m_npar);
00150      constTY.resize(m_npar);
00151      constRZ.resize(m_npar);
00152 
00153      constTXE.resize(m_npar);
00154      constTXW.resize(m_npar);
00155      constTYE.resize(m_npar);
00156      constTYW.resize(m_npar);
00157      constRZE.resize(m_npar);
00158      constRZW.resize(m_npar);
00159 
00160      for(i=0; i<m_npar; i++){
00161           constTX[i] = 0.0;
00162           constTY[i] = 0.0;
00163           constRZ[i] = 0.0;
00164 
00165           constTXE[i] = 0.0;
00166           constTXW[i] = 0.0;
00167           constTYE[i] = 0.0;
00168           constTYW[i] = 0.0;
00169           constRZE[i] = 0.0;
00170           constRZW[i] = 0.0;
00171      }
00172      constTX[7] = 1.0;
00173      constTX[15] = 1.0;
00174      constTY[23] = 1.0;
00175      constTY[31] = 1.0;
00176      constRZ[39] = 1.0;
00177      constRZ[47] = 1.0;
00178 
00179      constTXE[7] = 1.0;
00180      constTXW[15] = 1.0;
00181      constTYE[23] = 1.0;
00182      constTYW[31] = 1.0;
00183      constRZE[39] = 1.0;
00184      constRZW[47] = 1.0;
00185 
00186      //m_pMilleAlign -> ConstF(&constTX[0], 0.0);
00187      //m_pMilleAlign -> ConstF(&constTY[0], 0.0);
00188 //      m_pMilleAlign -> ConstF(&constRZ[0], 0.0);
00189 
00190      m_pMilleAlign -> ConstF(&constTXE[0], 0.0);
00191      m_pMilleAlign -> ConstF(&constTXW[0], 0.0);
00192      m_pMilleAlign -> ConstF(&constTYE[0], 0.0);
00193      m_pMilleAlign -> ConstF(&constTYW[0], 0.0);
00194      m_pMilleAlign -> ConstF(&constRZE[0], 0.0);
00195      m_pMilleAlign -> ConstF(&constRZW[0], 0.0);
00196 }
00197 
00198 bool MilleAlign::fillHist(MdcAliEvent* event){
00199      IMessageSvc* msgSvc;
00200      Gaudi::svcLocator() -> service("MessageSvc", msgSvc);
00201      MsgStream log(msgSvc, "MilleAlign");
00202      log << MSG::DEBUG << "MilleAlign::fillTree()" << endreq;
00203 
00204      int recFlag;
00205      int itrk;
00206      int ihit;
00207      int fgGetDoca;
00208      int lr;
00209      int lay;
00210      int cel;
00211      double doca;
00212      double dmeas;
00213      double zhit;
00214      double resi;
00215      double resiRec;
00216      double deri;
00217      double hitSigm;
00218      double hitpos[3];
00219      double wpos[7];            // wpos[6] is wire tension
00220      const MdcGeoWire* pWire;
00221 
00222      double trkpar[NTRKPAR];
00223      double trkparms[NTRKPARALL];       // track parameters and errors
00224 
00225      // numerical derivative
00226      int ipar;
00227      int iparGB;
00228 
00229      MdcAliRecTrk* rectrk;
00230      MdcAliRecHit* rechit;
00231 
00232      int ntrk = event -> getNTrk();
00233      if( (ntrk<m_param.nTrkCut[0]) || (ntrk>m_param.nTrkCut[1])) return false;
00234 
00235      for(itrk=0; itrk<ntrk; itrk++){
00236           rectrk = event->getRecTrk(itrk);
00237           recFlag = rectrk->getStat();
00238 
00239           trkpar[0] = rectrk -> getDr();
00240           trkpar[1] = rectrk -> getPhi0();
00241           trkpar[2] = rectrk -> getKappa();
00242           trkpar[3] = rectrk -> getDz();
00243           trkpar[4] = rectrk -> getTanLamda();
00244 
00245           int nHit = rectrk -> getNHits();
00246           if(nHit < m_param.nHitCut) continue;
00247           if(fabs(trkpar[0]) > m_param.drCut) continue;
00248           if(fabs(trkpar[3]) > m_param.dzCut) continue;
00249 
00250           HepVector rechelix = rectrk->getHelix();
00251           HepVector helix = rectrk->getHelix();
00252           HepSymMatrix helixErr = rectrk->getHelixErr();
00253 
00254           int nHitUse = 0;
00255           for(ihit=0; ihit<nHit; ihit++){
00256                rechit = rectrk -> getRecHit(ihit);
00257                lr = rechit->getLR();
00258                lay = rechit -> getLayid();
00259                cel = rechit -> getCellid();
00260                pWire = m_mdcGeomSvc -> Wire(lay, cel); 
00261                dmeas = rechit -> getDmeas();
00262                zhit = rechit -> getZhit();
00263                hitSigm = rechit -> getErrDmeas();
00264 
00265                wpos[0] = pWire -> Backward().x(); // east end
00266                wpos[1] = pWire -> Backward().y();
00267                wpos[2] = pWire -> Backward().z();
00268                wpos[3] = pWire -> Forward().x(); // west end
00269                wpos[4] = pWire -> Forward().y();
00270                wpos[5] = pWire -> Forward().z();
00271                wpos[6] = pWire -> Tension();
00272 
00273                double docaRec = rechit->getDocaInc();
00274                double doca = (m_mdcUtilitySvc->doca(lay, cel, helix, helixErr))*10.0;
00275 
00276                resi = -1.0*dmeas - doca;
00277                if ((fabs(doca) < m_docaCut[lay][0]) || (fabs(doca) > m_docaCut[lay][1]) || 
00278                    (fabs(resi) > m_resiCut[lay])) continue;
00279                nHitUse++;
00280 
00281                resiRec = rechit -> getResiIncLR();
00282 
00283                double dd = fabs(doca) - fabs(rechit->getDocaInc());
00284                m_hddoca -> Fill(dd);
00285                m_hddocaLay[lay] -> Fill(dd);
00286 
00287                // fill histograms
00288                m_hresAll->Fill(resi);
00289                if(lay < 8) m_hresInn->Fill(resi);
00290                else if(lay < 20) m_hresStp->Fill(resi);
00291                else m_hresOut->Fill(resi);
00292                m_hresLay[lay]->Fill(resi);
00293 
00294                m_hresAllRec->Fill(resiRec);
00295                m_hresLayRec[lay]->Fill(resiRec);
00296 
00297                // reset the derivatives arrays
00298                m_pMilleAlign -> ZerLoc(&m_derGB[0], &m_derLC[0], &m_derNonLin[0]);
00299 
00300                // derivatives of local parameters
00301                for(ipar=0; ipar<m_nloc; ipar++){
00302                     if( ! getDeriLoc(ipar, lay, cel ,rechelix, helixErr, deri) ){
00303                          cout << "getDeriLoc == false!" << setw(12) << itrk << setw(12) << ipar << endl; 
00304                          return false;
00305                     }
00306                     m_derLC[ipar] = deri;
00307                }
00308 
00309                // derivatives of global parameters
00310                // ipar 0 - 5: dx_east, dx_west, dy_east, dy_west, rz_east, rz_west
00311                for(ipar=0; ipar<m_nGloHit; ipar++){
00312                     iparGB = getAlignParId(lay, ipar);
00313                     if( ! getDeriGlo(ipar, iparGB, lay, cel, helix, helixErr, wpos, deri) )
00314                     {
00315                          cout << "getDeriGlo == false!" << setw(12) << itrk << setw(12) << ipar << endl; 
00316                          return false;
00317                     }
00318                     m_derGB[iparGB] = deri;
00319                }
00320                m_pMilleAlign -> EquLoc(&m_derGB[0], &m_derLC[0], &m_derNonLin[0], resi, hitSigm);
00321           } // loop of nhit
00322 
00323           // local fit in Millepede
00324           bool sc = m_pMilleAlign -> FitLoc(m_pMilleAlign->GetTrackNumber(), trkparms, 0);
00325           if(sc) m_pMilleAlign -> SetTrackNumber( m_pMilleAlign->GetTrackNumber()+1 );
00326      } // track loop 
00327      return true;
00328 }
00329 
00330 
00331 void MilleAlign::updateConst(MdcAlignPar* alignPar){
00332      IMessageSvc* msgSvc;
00333      Gaudi::svcLocator() -> service("MessageSvc", msgSvc);
00334      MsgStream log(msgSvc, "MilleAlign");
00335      log << MSG::INFO << "MilleAlign::updateConst()" << endreq;
00336 
00337      m_pMilleAlign -> MakeGlobalFit(&m_par[0], &m_error[0], &m_pull[0]);
00338 
00339      int iEP;
00340      int ipar;
00341      double val;
00342      double err;
00343      for(int i=0; i<NDOFALIGN; i++){
00344           for(iEP=0; iEP<NEP; iEP++){
00345                ipar = i * NEP + iEP;
00346                if(m_dofs[i]){
00347                     val = m_par[ipar];
00348                     err = m_error[ipar];
00349                } else{
00350                     val = 0.0;
00351                     err = 0.0;
00352                }
00353 
00354                if(0 == i){
00355                     alignPar->setDelDx(iEP, val);
00356                     alignPar->setErrDx(iEP, err);
00357                } else if(1 == i){
00358                     alignPar->setDelDy(iEP, val);
00359                     alignPar->setErrDy(iEP, err);
00360                } else if(2 == i){
00361                     alignPar->setDelRz(iEP, val/1000.0); // mrad -> rad
00362                     alignPar->setErrRz(iEP, err/1000.0); // mrad -> rad
00363                }
00364           }
00365      }
00366 }
00367 
00368 int MilleAlign::getAlignParId(int lay, int iparHit){
00369      int ip;
00370      if(lay < 8) ip = 0;
00371      else if(lay < 10) ip = 1;
00372      else if(lay < 12) ip = 2;
00373      else if(lay < 14) ip = 3;
00374      else if(lay < 16) ip = 4;
00375      else if(lay < 18) ip = 5;
00376      else if(lay < 20) ip = 6;
00377      else ip = 7;
00378 
00379      // iparHit 0 - 5: dx_east, dx_west, dy_east, dy_west, rz_east, rz_west
00380      int ipar = iparHit * 8 + ip;
00381      return ipar;
00382 }
00383 
00384 bool MilleAlign::getDeriLoc(int ipar, int lay, int cel, HepVector rechelix, HepSymMatrix &helixErr, double &deri){
00385      int i;
00386      double doca;
00387      HepVector sampar(NTRKPAR, 0);
00388      double xxLC[gNsamLC];
00389      double yyLC[gNsamLC];
00390 
00391      for(i=0; i<m_nloc; i++) sampar[i] = rechelix[i];
00392      double startpar = rechelix[ipar] - 0.5*gStepLC[ipar]*(double)gNsamLC;
00393 
00394      for(i=0; i<gNsamLC; i++){
00395           sampar[ipar] = startpar + (double)i * gStepLC[ipar];
00396           xxLC[i] = sampar[ipar];
00397           if(0==ipar || 3==ipar) xxLC[i] *= 10.;        // cm -> mm
00398 
00399           HepVector helix = sampar;
00400           bool passCellRequired = false;
00401           doca = (m_mdcUtilitySvc->doca(lay, cel, helix, helixErr,passCellRequired))*10.0;
00402 
00403           if(NULL == doca){
00404 //           cout << "in getDeriLoc, doca = " << doca << endl;
00405                return false;
00406           }
00407           yyLC[i] = doca;
00408      }
00409 
00410      if (0==ipar || 3==ipar) rechelix[ipar] *= 10.; // cm -> mm
00411      TSpline3* pSpline3 = new TSpline3("deri", xxLC, yyLC, gNsamLC);
00412      deri = pSpline3->Derivative(rechelix[ipar]);
00413      delete pSpline3;
00414      return true;
00415 }
00416 
00417 bool MilleAlign::getDeriGlo(int iparHit, int iparGB, int lay, int cel, HepVector helix, 
00418                             HepSymMatrix &helixErr, double wpos[], double &deri){
00419      int i;
00420      double doca;
00421      double xxGB[gNsamGB];
00422      double yyGB[gNsamGB];
00423      double dAlignPar;
00424      double dAlignParini = 0.0;
00425      double startpar = dAlignParini - 0.5*gStepGB[iparGB]*(double)gNsamGB;
00426      double wposSam[7];
00427      for(i=0; i<7; i++) wposSam[i] = wpos[i]; // 0-2:east; 3-5:west
00428 
00429      for(i=0; i<gNsamGB; i++){
00430           dAlignPar = startpar + (double)i * gStepGB[iparGB];
00431           xxGB[i] = dAlignPar;
00432           if(0 == iparHit){     // dx_east
00433                wposSam[0] = wpos[0] + dAlignPar;
00434           } else if(1 == iparHit){      // dx_west
00435                wposSam[3] = wpos[3] + dAlignPar;
00436           } else if(2 == iparHit){      // dy_east
00437                wposSam[1] = wpos[1] + dAlignPar;
00438           } else if(3 == iparHit){      // dy_west
00439                wposSam[4] = wpos[4] + dAlignPar;
00440           } else if(4 == iparHit){      // rz_east
00441                wposSam[0] = wpos[0] - (wpos[1] * dAlignPar * 0.001);
00442                wposSam[1] = wpos[1] + (wpos[0] * dAlignPar * 0.001);
00443           } else if(5 == iparHit){      // rz_west
00444                wposSam[3] = wpos[3] - (wpos[4] * dAlignPar * 0.001);
00445                wposSam[4] = wpos[4] + (wpos[3] * dAlignPar * 0.001);
00446           }
00447 
00448           HepPoint3D eastP(wposSam[0]/10., wposSam[1]/10., wposSam[2]/10.);
00449           HepPoint3D westP(wposSam[3]/10., wposSam[4]/10., wposSam[5]/10.);
00450           doca = (m_mdcUtilitySvc->doca(lay, cel, eastP, westP, helix, helixErr))*10.0; // cm->mm
00451 
00452           if(NULL == doca) return false; 
00453  
00454           yyGB[i] = doca;
00455      }
00456 
00457      TSpline3* pSpline3 = new TSpline3("deri", xxGB, yyGB, gNsamGB);
00458      deri = pSpline3 -> Derivative(dAlignParini);
00459      delete pSpline3;
00460      return true;
00461 }
00462