Fixes for bug #52499: Field polarities inconsistiency
[u/mrichter/AliRoot.git] / STEER / AliTrackFitterRieman.cxx
1 /**************************************************************************
2  * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
3  *                                                                        *
4  * Author: The ALICE Off-line Project.                                    *
5  * Contributors are mentioned in the code where appropriate.              *
6  *                                                                        *
7  * Permission to use, copy, modify and distribute this software and its   *
8  * documentation strictly for non-commercial purposes is hereby granted   *
9  * without fee, provided that the above copyright notice appears in all   *
10  * copies and that both the copyright notice and this permission notice   *
11  * appear in the supporting documentation. The authors make no claims     *
12  * about the suitability of this software for any purpose. It is          *
13  * provided "as is" without express or implied warranty.                  *
14  **************************************************************************/
15
16 /* $Id$ */
17
18 ///////////////////////////////////////////////////////////////////////////////
19 //
20 // Class to the track points on the Riemann sphere. Inputs are
21 // the set of id's (volids) of the volumes in which residuals are
22 // calculated to construct a chi2 function to be minimized during 
23 // the alignment procedures. For the moment the track extrapolation is 
24 // taken at the space-point reference plane. The reference plane is
25 // found using the covariance matrix of the point
26 // (assuming sigma(x)=0 at the reference coordinate system.
27 //
28 // Internal usage of AliRieman class for minimization
29 //
30 //////////////////////////////////////////////////////////////////////////////
31
32 #include <TArrayI.h>
33 #include <TLinearFitter.h>
34 #include <TMath.h>
35 #include <TMatrixD.h>
36 #include <TMatrixDSym.h>
37 #include <TRandom.h>
38 #include <TTreeStream.h>
39
40 #include "AliLog.h"
41 #include "AliLog.h"
42 #include "AliRieman.h"
43 #include "AliTrackFitterRieman.h"
44
45 ClassImp(AliTrackFitterRieman)
46
47
48 AliTrackFitterRieman::AliTrackFitterRieman():
49   AliTrackFitter(),
50   fBCorrection(kFALSE),
51   fAlpha(0.),
52   fNUsed(0),
53   fConv(kFALSE),
54   fMaxDelta(3),
55   fRieman(new AliRieman(10000)),  // allocate rieman
56   fMinPointRadius(2.),
57   fMaxPointRadius(500.),
58   fDebugStream(new TTreeSRedirector("RiemanAlignDebug.root"))
59 {
60   //
61   // default constructor
62   //
63 }
64
65
66 AliTrackFitterRieman::AliTrackFitterRieman(AliTrackPointArray *array, Bool_t owner):
67   AliTrackFitter(array,owner),
68   fBCorrection(kFALSE),
69   fAlpha(0.),
70   fNUsed(0),
71   fConv(kFALSE),
72   fMaxDelta(3),
73   fRieman(new AliRieman(10000)),  //allocate rieman
74   fMinPointRadius(2.),
75   fMaxPointRadius(500.),
76   fDebugStream(0)
77 {
78   //
79   // Constructor
80   //
81   if (AliLog::GetDebugLevel("","AliTrackFitterRieman")) fDebugStream = new TTreeSRedirector("RiemanAlignDebug.root");
82 }
83
84 AliTrackFitterRieman::AliTrackFitterRieman(const AliTrackFitterRieman &rieman):
85   AliTrackFitter(rieman),
86   fBCorrection(rieman.fBCorrection),
87   fAlpha(rieman.fAlpha),
88   fNUsed(rieman.fNUsed),
89   fConv(rieman.fConv),
90   fMaxDelta(rieman.fMaxDelta),
91   fRieman(new AliRieman(*(rieman.fRieman))),
92   fMinPointRadius(rieman.fMinPointRadius),
93   fMaxPointRadius(rieman.fMaxPointRadius),
94   fDebugStream(0)
95 {
96   //
97   // copy constructor
98   //
99   if (AliLog::GetDebugLevel("","AliTrackFitterRieman")) fDebugStream = new TTreeSRedirector("RiemanAlignDebug.root");
100 }
101
102 //_____________________________________________________________________________
103 AliTrackFitterRieman &AliTrackFitterRieman::operator =(const AliTrackFitterRieman& rieman)
104 {
105   //
106   // Assignment operator
107   //
108   if(this==&rieman) return *this;
109   ((AliTrackFitter *)this)->operator=(rieman);
110
111   fBCorrection = rieman.fBCorrection;
112   fAlpha  = rieman.fAlpha;
113   fNUsed  = rieman.fNUsed;
114   fConv   = rieman.fConv;
115   fMaxDelta = rieman.fMaxDelta;
116   fRieman = new AliRieman(*(rieman.fRieman));
117   fMinPointRadius = rieman.fMinPointRadius;
118   fMaxPointRadius = rieman.fMaxPointRadius;
119   fDebugStream = 0;
120   if (AliLog::GetDebugLevel("","AliTrackFitterRieman")) fDebugStream = new TTreeSRedirector("RiemanAlignDebug.root");
121   return *this;
122 }
123
124
125 AliTrackFitterRieman::~AliTrackFitterRieman(){
126   //
127   //
128   //
129   delete fRieman;
130   delete fDebugStream;
131 }
132
133 void AliTrackFitterRieman::Reset()
134 {
135   // Reset the track parameters and
136   // rieman sums
137   //
138   AliTrackFitter::Reset();
139   fRieman->Reset();
140   fAlpha = 0.;
141   fNUsed = 0;
142   fConv =kFALSE;
143 }
144
145 Bool_t AliTrackFitterRieman::Fit(const TArrayI *volIds,const TArrayI *volIdsFit,
146                                  AliGeomManager::ELayerID layerRangeMin,
147                          AliGeomManager::ELayerID layerRangeMax)
148 {
149   // Fit the track points. The method takes as an input
150   // the set of id's (volids) of the volumes in which
151   // one wants to calculate the residuals.
152   // The following parameters are used to define the
153   // range of volumes to be used in the fitting
154   // As a result two AliTrackPointArray's obects are filled.
155   // The first one contains the space points with
156   // volume id's from volids list. The second array of points represents
157   // the track extrapolations corresponding to the space points
158   // in the first array. The two arrays can be used to find
159   // the residuals in the volids and consequently construct a
160   // chi2 function to be minimized during the alignment
161   // procedures. For the moment the track extrapolation is taken
162   // at the space-point reference plane. The reference plane is
163   // found using the covariance matrix of the point
164   // (assuming sigma(x)=0 at the reference coordinate system.
165   Int_t debugLevel = AliLog::GetDebugLevel("","AliTrackFitterRieman");
166   
167   //  Float_t debugRatio = 1./(1.+debugLevel);
168   Float_t debugRatio = debugLevel? 1.0/debugLevel : 1.0;
169
170   Int_t npoints = fPoints->GetNPoints();
171   if ( npoints<fMinNPoints) return kFALSE;
172   //
173   // fast count points
174   if (volIdsFit != 0x0) {
175     Int_t countFit = 0;
176     Int_t countPoint = 0;
177     for (Int_t ipoint = 0; ipoint < npoints; ipoint++) {
178       if (FindVolId(volIds,fPoints->GetVolumeID()[ipoint]))
179         countPoint++;
180       if (volIdsFit != 0x0) {
181         if (FindVolId(volIdsFit,fPoints->GetVolumeID()[ipoint]))
182           countFit++;
183       }
184     }
185     if (countPoint==0) return kFALSE;
186     if ((countFit<fMinNPoints) && (volIdsFit != 0x0)) return kFALSE;
187   }
188   //
189
190   Reset();
191
192   if (fPoints && AliLog::GetDebugLevel("","AliTrackFitterRieman")>1&& gRandom->Rndm()<debugRatio){
193     Int_t nVol    = volIds->GetSize();
194     Int_t nVolFit = volIdsFit->GetSize();
195     Int_t volId  = volIds->At(0);    
196     (*fDebugStream)<<"PInput"<<
197       "NPoints="<<npoints<<    // number of points
198       "VolId="<<volId<<        // first vol ID
199       "NVol="<<nVol<<          // number of volumes 
200       "NvolFit="<<nVolFit<<    // number of volumes to fit
201       "fPoints.="<<fPoints<<   // input points
202       "\n";
203   }
204
205   Bool_t isAlphaCalc = kFALSE;
206   AliTrackPoint p,plocal;
207 //   fPoints->GetPoint(p,0);
208 //   fAlpha = TMath::ATan2(p.GetY(),p.GetX());
209
210   Int_t npVolId = 0;
211   fNUsed = 0;
212   Int_t *pindex = new Int_t[npoints];
213   for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
214     {
215       fPoints->GetPoint(p,ipoint);
216       UShort_t iVolId = p.GetVolumeID();
217       if (FindVolId(volIds,iVolId)) {
218         pindex[npVolId] = ipoint;
219         npVolId++;
220       }
221       if (volIdsFit != 0x0) {
222         if (!FindVolId(volIdsFit,iVolId)) continue;
223       }
224       else {
225         if (iVolId < AliGeomManager::LayerToVolUID(layerRangeMin,0) ||
226             iVolId > AliGeomManager::LayerToVolUID(layerRangeMax,
227                                                 AliGeomManager::LayerSize(layerRangeMax))) continue;
228       }
229       if (!isAlphaCalc) {
230         fAlpha = p.GetAngle();
231         isAlphaCalc = kTRUE;
232       }
233       plocal = p.Rotate(fAlpha);
234       if (TMath::Abs(plocal.GetX())>fMaxPointRadius || TMath::Abs(plocal.GetX())<fMinPointRadius || plocal.GetCov()[3]<=0 ||plocal.GetCov()[5]<=0 ){
235         printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<</n");
236         p.Dump();
237         plocal.Dump();
238         printf("Problematic point\n");  
239         printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<</n");
240       }else{
241         AddPoint(plocal.GetX(),plocal.GetY(),plocal.GetZ(),
242                  TMath::Sqrt(plocal.GetCov()[3]),TMath::Sqrt(plocal.GetCov()[5]));
243       }
244       // fNUsed++;  AddPoint should be responsible
245     }
246
247   if (npVolId == 0 || fNUsed < fMinNPoints) {
248     delete [] pindex;
249     return kFALSE;
250   }
251
252   Update();
253  
254   if (!fConv) {
255     delete [] pindex;
256     return kFALSE;
257   }
258
259   if ((fParams[0] == 0) ||
260       ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0)) {
261     delete [] pindex;
262     return kFALSE;
263   }
264
265
266   if (fNUsed < fMinNPoints) {
267     delete [] pindex;
268     return kFALSE;
269   }
270
271   fPVolId = new AliTrackPointArray(npVolId);
272   fPTrack = new AliTrackPointArray(npVolId);
273   AliTrackPoint p2;
274   for (Int_t ipoint = 0; ipoint < npVolId; ipoint++)
275     {
276       Int_t index = pindex[ipoint];
277       fPoints->GetPoint(p,index);
278       if (GetPCA(p,p2) && (
279           TMath::Abs(p.GetX()-p2.GetX())<fMaxDelta && 
280           TMath::Abs(p.GetY()-p2.GetY())<fMaxDelta &&
281           TMath::Abs(p.GetZ()-p2.GetZ())<fMaxDelta
282           )) {
283         Float_t xyz[3],xyz2[3];
284         p.GetXYZ(xyz); p2.GetXYZ(xyz2);
285         //      printf("residuals %f %d %d %f %f %f %f %f %f\n",fChi2,fNUsed,fConv,xyz[0],xyz[1],xyz[2],xyz2[0]-xyz[0],xyz2[1]-xyz[1],xyz2[2]-xyz[2]);  
286         fPVolId->AddPoint(ipoint,&p);
287         fPTrack->AddPoint(ipoint,&p2);
288       }else{
289         // what should be default bahavior -  
290         delete [] pindex;
291         delete fPVolId;
292         delete fPTrack;
293         fPVolId =0;
294         fPTrack =0;
295         return kFALSE;
296       }
297     }  
298   
299   if (AliLog::GetDebugLevel("","AliTrackFitterRieman")>0 && gRandom->Rndm()<debugRatio){
300     //
301     // Debug Info
302     //
303     AliTrackPointArray *lPVolId = new AliTrackPointArray(npVolId);
304     AliTrackPointArray *lPTrack = new AliTrackPointArray(npVolId);
305     AliTrackPointArray *lPTrackE = new AliTrackPointArray(npVolId);
306     AliRieman * residual = fRieman->MakeResiduals();
307     for (Int_t ipoint = 0; ipoint < npVolId; ipoint++){
308       AliTrackPoint p0, p0local;      
309       AliTrackPoint pFit, pFitlocal, pFitLocalE;      
310       fPVolId->GetPoint(p0,ipoint);
311       Float_t lAngle = p0.GetAngle();
312       p0local= p0.MasterToLocal();
313       fPTrack->GetPoint(pFit,ipoint);
314       pFitlocal= pFit.Rotate(lAngle);
315       //
316       Float_t xyz[3], cov[6];
317       xyz[0] = pFitlocal.GetX();
318       xyz[1] = pFitlocal.GetY();
319       xyz[2] = pFitlocal.GetZ();
320       for (Int_t icov=0; icov<6; icov++) cov[icov]=0;
321       cov[3] = GetErrY2at(xyz[0]);
322       cov[5] = GetErrZ2at(xyz[0]);      
323       pFitLocalE.SetXYZ(xyz,cov);
324       //
325       lPVolId->AddPoint(ipoint,&p0local);
326       lPTrack->AddPoint(ipoint,&pFitlocal);
327       lPTrackE->AddPoint(ipoint,&pFitLocalE);
328     }      
329     //
330     // debug info
331     //
332     Int_t nVol    = volIds->GetSize();
333     Int_t nVolFit = volIdsFit->GetSize();
334     Int_t volId   = volIds->At(0);   
335     Int_t modId   =0;
336     Int_t layer   =  AliGeomManager::VolUIDToLayer(volId,modId);
337     Int_t volIdFit   = volIdsFit->At(0);   
338     Int_t modIdFit   =0;
339     Int_t layerFit   =  AliGeomManager::VolUIDToLayer(volIdFit,modIdFit);
340     
341     (*fDebugStream)<<"Fit"<<
342       "VolId="<<volId<<        // volume ID
343       "Layer="<<layer<<        // layer ID
344       "Module="<<modId<<       // module ID
345       "LayerFit="<<layerFit<<        // layer ID fit
346       "ModuleFit="<<modIdFit<<       // module ID fit
347       "NVol="<<nVol<<          // number of volumes 
348       "NvolFit="<<nVolFit<<    // number of volumes to fit
349       "Points0.="<<fPVolId<<    // original points
350       "Points1.="<<fPTrack<<    // fitted points 
351       "LPoints0.="<<lPVolId<<   // original points - local frame
352       "LPoints1.="<<lPTrack<<   // fitted points   - local frame      
353       "LPointsE.="<<lPTrackE<<   // fitted points with ext error - local frame      
354       "Rieman.="<<this<<        // original rieman fit
355       "Res.="<<residual<<       // residuals of rieman fit  
356       "\n";
357     delete lPVolId;
358     delete lPTrack;
359     delete residual;
360   }
361
362   delete [] pindex;
363   return kTRUE;
364 }
365
366
367 void AliTrackFitterRieman::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
368 {
369   //
370   // add point to rieman fitter
371   //
372   fRieman->AddPoint(x,y,z,sy,sz);
373   fNUsed = fRieman->GetN();
374 }
375
376
377
378 Bool_t AliTrackFitterRieman::Update(){
379   //
380   // 
381   //
382   fRieman->Update();
383   fConv = kFALSE;
384   if (fRieman->IsValid()){
385     for (Int_t ipar=0; ipar<6; ipar++){
386       fParams[ipar] = fRieman->GetParam()[ipar];
387     }
388     fChi2  =  fRieman->GetChi2();
389     fNdf   =  fRieman->GetN()- 2;
390     fNUsed =  fRieman->GetN();
391     fConv = kTRUE;
392   }
393   //
394   //
395   //
396   TLinearFitter fitY(3,"pol2");
397   TLinearFitter fitZ(3,"pol2");
398   for (Int_t ip=0; ip<fRieman->GetN();ip++){
399     Double_t x = fRieman->GetX()[ip]; 
400     fitY.AddPoint(&x,fRieman->GetY()[ip]-fRieman->GetYat(x),1);
401     fitZ.AddPoint(&x,fRieman->GetZ()[ip]-fRieman->GetZat(x),1);    
402   }
403   fitY.Eval();
404   fitZ.Eval();
405   for (Int_t iparam=0; iparam<3; iparam++){
406     fCorrY[iparam]=fitY.GetParameter(iparam);
407     fCorrZ[iparam]=fitZ.GetParameter(iparam);
408   }
409   fCorrY[3]=fitY.GetChisquare()/Float_t(fRieman->GetN()-3);   
410   fCorrZ[3]=fitZ.GetChisquare()/Float_t(fRieman->GetN()-3);
411
412   return kTRUE;
413 }
414
415
416
417 //_____________________________________________________________________________
418 Bool_t AliTrackFitterRieman::GetPCA(const AliTrackPoint &p, AliTrackPoint &p2) const
419 {
420   //
421   // Get the closest to a given spacepoint track trajectory point
422   // Look for details in the description of the Fit() method
423   //
424   if (!fConv) return kFALSE;
425
426   // First X and Y coordinates
427   Double_t sin = TMath::Sin(fAlpha);
428   Double_t cos = TMath::Cos(fAlpha);
429   //  fParam[0]  = 1/y0
430   //  fParam[1]  = -x0/y0
431   //  fParam[2]  = - (R^2 - x0^2 - y0^2)/y0
432   if (fParams[0] == 0) return kFALSE;
433   // Track parameters in the global coordinate system
434   Double_t x0 = -fParams[1]/fParams[0]*cos -         1./fParams[0]*sin;
435   Double_t y0 =          1./fParams[0]*cos - fParams[1]/fParams[0]*sin;
436   if ((-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1) <= 0) return kFALSE;
437   Double_t r  = TMath::Sqrt(-fParams[2]*fParams[0]+fParams[1]*fParams[1]+1)/
438                 fParams[0];
439
440   // Define space-point refence plane
441   Double_t alphap = p.GetAngle();
442   Double_t sinp = TMath::Sin(alphap);
443   Double_t cosp = TMath::Cos(alphap);
444   Double_t x  = p.GetX()*cosp + p.GetY()*sinp;
445   Double_t y  = p.GetY()*cosp - p.GetX()*sinp;
446   Double_t x0p= x0*cosp + y0*sinp;
447   Double_t y0p= y0*cosp - x0*sinp;
448   if ((r*r - (x-x0p)*(x-x0p))<0) {
449     AliWarning(Form("Track extrapolation failed ! (Track radius = %f, track circle x = %f, space-point x = %f, reference plane angle = %f\n",r,x0p,x,alphap));
450     return kFALSE;
451   }
452   Double_t temp = TMath::Sqrt(r*r - (x-x0p)*(x-x0p));
453   Double_t y1 = y0p + temp;
454   Double_t y2 = y0p - temp;
455   Double_t yprime = y1;
456   if(TMath::Abs(y2-y) < TMath::Abs(y1-y)) yprime = y2;
457
458   // Back to the global coordinate system
459   Double_t xsecond = x*cosp - yprime*sinp;
460   Double_t ysecond = yprime*cosp + x*sinp;
461
462   // Now Z coordinate and track angles
463   Double_t x2 = xsecond*cos + ysecond*sin;
464   Double_t zsecond = GetZat(x2);
465   Double_t dydx = GetDYat(x2);
466   Double_t dzdx = GetDZat(x2);
467
468   // Fill the cov matrix of the track extrapolation point
469   Double_t cov[6] = {0,0,0,0,0,0};
470   Double_t sigmax = 100*100.;
471   cov[0] = sigmax;           cov[1] = sigmax*dydx;      cov[2] = sigmax*dzdx;
472   cov[3] = sigmax*dydx*dydx; cov[4] = sigmax*dydx*dzdx;
473   cov[5] = sigmax*dzdx*dzdx;
474
475   Double_t sigmay2 = GetErrY2at(x2);
476   Double_t sigmaz2 = GetErrZ2at(x2);
477   cov[3] += sigmay2;
478   cov[5] += sigmaz2;
479    
480
481   Float_t  newcov[6];
482   newcov[0] = cov[0]*cos*cos-
483             2*cov[1]*sin*cos+
484               cov[3]*sin*sin;
485   newcov[1] = cov[1]*(cos*cos-sin*sin)-
486              (cov[3]-cov[0])*sin*cos;
487   newcov[2] = cov[2]*cos-
488               cov[4]*sin;
489   newcov[3] = cov[0]*sin*sin+
490             2*cov[1]*sin*cos+
491               cov[3]*cos*cos;
492   newcov[4] = cov[4]*cos+
493               cov[2]*sin;
494   newcov[5] = cov[5];
495
496   p2.SetXYZ(xsecond,ysecond,zsecond,newcov);
497   Int_t debugLevel = AliLog::GetDebugLevel("","AliTrackFitterRieman");
498   Float_t debugRatio = 1./(1.+debugLevel);
499   if (AliLog::GetDebugLevel("","AliTrackFitterRieman")>0 && gRandom->Rndm()<debugRatio){
500     AliTrackPoint lp0(p);
501     AliTrackPoint lp2(p2);
502     AliTrackPoint localp0(p);
503     AliTrackPoint localp2(p2);
504     Float_t lAngle = lp0.GetAngle();
505     localp0 = localp0.Rotate(lAngle);
506     localp2 = localp2.Rotate(lAngle);
507
508     (*fDebugStream)<<"PCA"<<
509       "P0.="<<&lp0<<  //global position
510       "P2.="<<&lp2<<
511       "LP0.="<<&localp0<<  //local position
512       "LP2.="<<&localp2<<
513       "\n";
514   }
515   return kTRUE;
516 }
517
518 Double_t AliTrackFitterRieman::GetYat(Double_t x) const  {
519   //
520   // get y position at given point 
521   //
522   Double_t correction=0;
523   if (fBCorrection){ // systematic effect correction
524     correction  =  fCorrY[0]+fCorrY[1]*x +fCorrY[2]*x*x;
525   }
526   return fRieman->GetYat(x)+correction;
527 }
528
529 Double_t AliTrackFitterRieman::GetZat(Double_t x) const  {
530   //
531   // get z position at given point 
532   //
533   Double_t correction=0;
534   if (fBCorrection){ // systematic effect correction
535     correction  =  fCorrZ[0]+fCorrZ[1]*x +fCorrZ[2]*x*x;
536   }
537   return fRieman->GetZat(x)+correction;
538 }
539
540 Double_t AliTrackFitterRieman::GetErrY2at(Double_t x) const {
541   //
542   // get estimate of extrapolation error
543   //
544   Double_t error = fRieman->GetErrY(x);
545   Double_t correction=0;
546   if (fBCorrection){ // everestimate error due systematic effect
547     error      *=fCorrY[3];
548     correction  =  fCorrY[0]+fCorrY[1]*x +fCorrY[2]*x*x;
549     correction *=correction;
550   }
551   return TMath::Sqrt(error+correction);
552 }
553
554 Double_t AliTrackFitterRieman::GetErrZ2at(Double_t x) const {
555   //
556   // get estimate of extrapolation error
557   //
558   Double_t error = fRieman->GetErrZ(x)*fCorrZ[3];
559   Double_t correction=0;
560   if (fBCorrection){  
561      error    *=  fCorrZ[3];
562     correction =  fCorrZ[0]+fCorrZ[1]*x +fCorrZ[2]*x*x;
563     correction*=  correction;
564   }
565   return TMath::Sqrt(error+correction);
566 }
567
568 void AliTrackFitterRieman::SetParam(Int_t i, Double_t par) {
569   if (i<0 || i>5) return;
570   fParams[i]=par;
571   fRieman->GetParam()[i]=par;
572 }