Added methods for finding a given module in the DDL map (F. Prino)
[u/mrichter/AliRoot.git] / STEER / AliTrackResidualsLinear.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 //   Implementation of the derived class for track residuals
20 //   based on linear chi2 minimization 
21 //  The minimization relies on the fact that the alignment parameters     
22 //   (angles and translations) are small.                                  
23 //   TLinearFitter used for minimization
24 //   Possibility to fix Paramaters
25 //   FixParameter()ReleaseParameter();
26 //   Possibility to define fraction of outliers to be skipped
27 //
28 //   marian.ivanov@cern.ch
29 //
30 //-----------------------------------------------------------------
31
32 #include <TMath.h>
33 #include <TGeoMatrix.h>
34
35 #include "AliLog.h"
36 #include "AliAlignObj.h"
37 #include "AliTrackPointArray.h"
38 #include "AliTrackResidualsLinear.h"
39 #include "AliAlignObj.h"
40 #include "TLinearFitter.h"
41 #include  "TDecompSVD.h"
42
43 ClassImp(AliTrackResidualsLinear)
44
45 //______________________________________________________________________________
46 AliTrackResidualsLinear::AliTrackResidualsLinear():
47   AliTrackResiduals(),
48   fFitter(0),
49   fFraction(-1),
50   fChi2Orig(0)
51 {
52   // Default constructor
53   for (Int_t ipar=0; ipar<6; ipar++){
54     fParams[ipar]  = 0;
55   }  
56   for (Int_t icov=0; icov<36; icov++){ fCovar[icov]=0;}
57 }
58
59 //______________________________________________________________________________
60 AliTrackResidualsLinear::AliTrackResidualsLinear(Int_t ntracks):
61   AliTrackResiduals(ntracks),
62   fFitter(new TLinearFitter(6,"hyp6")),
63   fFraction(-1),
64   fChi2Orig(0)
65 {
66   // Constructor
67   for (Int_t ipar=0; ipar<6; ipar++){
68     fParams[ipar]  = 0;
69   }
70   for (Int_t icov=0; icov<36; icov++){ fCovar[icov]=0;}
71 }
72  
73 //______________________________________________________________________________
74 AliTrackResidualsLinear::AliTrackResidualsLinear(const AliTrackResidualsLinear &res):
75   AliTrackResiduals(res),
76   fFitter(new TLinearFitter(*(res.fFitter))),
77   fFraction(res.fFraction),
78   fChi2Orig(res.fChi2Orig)
79 {
80   // Copy constructor
81   //..
82   for (Int_t ipar=0; ipar<6; ipar++){
83     fParams[ipar]  = res.fParams[ipar];
84   }
85   for (Int_t icov=0; icov<36; icov++){ fCovar[icov]= res.fCovar[icov];}
86   fChi2Orig = res.fChi2Orig;
87 }
88
89 //______________________________________________________________________________
90 AliTrackResidualsLinear &AliTrackResidualsLinear::operator= (const AliTrackResidualsLinear& res)
91 {
92   // Assignment operator
93  ((AliTrackResiduals *)this)->operator=(res);
94  return *this;
95 }
96 //______________________________________________________________________________
97 AliTrackResidualsLinear::~AliTrackResidualsLinear()
98 {
99   //
100   //
101   //
102   delete fFitter;
103 }
104
105
106 //______________________________________________________________________________
107 Bool_t AliTrackResidualsLinear::Minimize()
108 {
109   // Implementation of fast linear Chi2 minimizer
110   // based on TLinear fitter
111   //
112   if (!fFitter) fFitter = new TLinearFitter(6,"hyp6");
113   fFitter->StoreData(kTRUE);
114   fFitter->ClearPoints();
115   fChi2Orig = 0;
116   AliTrackPoint p1,p2;
117   for (Int_t itrack = 0; itrack < fLast; itrack++) {
118     if (!fVolArray[itrack] || !fTrackArray[itrack]) continue;
119     for (Int_t ipoint = 0; ipoint < fVolArray[itrack]->GetNPoints(); ipoint++) {
120       fVolArray[itrack]->GetPoint(p1,ipoint);
121       fTrackArray[itrack]->GetPoint(p2,ipoint);
122       AddPoints(p1,p2);
123     }
124   }
125   Bool_t isOK = Update();
126   if (!isOK) return isOK;
127   //
128   TGeoHMatrix  matrix;
129   fAlignObj->GetMatrix(matrix);
130   return isOK;
131 }
132
133
134
135 //______________________________________________________________________________
136 void AliTrackResidualsLinear::AddPoints(AliTrackPoint &p, AliTrackPoint &pprime)
137 {
138   //
139   // add points to linear fitter - option with correlation betwee measurement in different dimensions
140   // p1      - local point
141   // pprime  - track extrapolation point
142   //
143   Float_t xyz[3],xyzp[3];
144   Float_t cov[6],covp[6];
145   p.GetXYZ(xyz,cov); pprime.GetXYZ(xyzp,covp);
146   //
147   //
148   TMatrixD mcov(3,3);   // local point covariance
149   mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
150   mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
151   mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
152   TMatrixD mcovp(3,3); //  extrapolation point covariance  
153   mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
154   mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
155   mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
156   mcov+=mcovp;
157   //mcov.Invert();
158   if (!mcov.IsValid()) return; 
159   TMatrixD mcovBack = mcov;  // for debug purposes
160   //
161   // decompose matrix
162   //
163   TDecompSVD svd(mcov);              // mcov  = svd.fU * covDiagonal * svd.fV.Invert   
164   if (!svd.Decompose()) return;      // decomposition failed
165   TMatrixD   matrixV = svd.GetV();   // transformation matrix to diagonalize covariance matrix
166   Double_t   covDiagonal[3] = {svd.GetSig()[0],svd.GetSig()[1],svd.GetSig()[2]};    // diagonalized covariance matrix
167   //
168   // residual vector 
169   TMatrixD  deltaR(3,1);
170   deltaR(0,0) = (xyzp[0]-xyz[0]); 
171   deltaR(1,0) = (xyzp[1]-xyz[1]);
172   deltaR(2,0) = (xyzp[2]-xyz[2]);   
173   //
174   // parametrization matrix
175   //
176   TMatrixD        mparam(3,6);
177   mparam(0,0) = 1;      mparam(1,0) = 0;       mparam(2,0) = 0;            // xshift
178   mparam(0,1) = 0;      mparam(1,1) = 1;       mparam(2,1) = 0;            // yshift
179   mparam(0,2) = 0;      mparam(1,2) = 0;       mparam(2,2) = 1;            // zshift
180   mparam(0,3) = 0;      mparam(1,3) =-xyz[2];  mparam(2,3) = xyz[1];       // x rotation
181   mparam(0,4) = xyz[2]; mparam(1,4) = 0;       mparam(2,4) =-xyz[0];       // y rotation
182   mparam(0,5) =-xyz[1]; mparam(1,5) = xyz[0];  mparam(2,5) = 0;            // z rotation
183   //
184   
185   TMatrixD  deltaT(matrixV, TMatrixD::kTransposeMult, deltaR);   // tranformed delta
186   TMatrixD  mparamT(matrixV,TMatrixD::kTransposeMult, mparam);   // tranformed linear transformation
187   if (AliLog::GetDebugLevel("","AliTrackResidualsLinear")>2){    
188     //
189     // debug part
190     //
191     //   covDiag = U^-1 * mcov * V      -- diagonalization of covariance matrix
192
193     TMatrixD   matrixU = svd.GetU();                      // transformation matrix to diagonalize covariance matrix
194     TMatrixD   matrixUI= svd.GetU(); 
195     matrixUI.Invert();
196     //
197     TMatrixD   test0   = matrixUI*matrixV;                // test matrix - should be unit matrix
198     TMatrixD   test1   = matrixUI*mcovBack*matrixV;       // test matrix - diagonal - should be diagonal with covDiagonal on diag
199     TMatrixD   test2   = matrixU.T()*matrixV;             // test ortogonality - shoul be unit
200     printf("Test matrix 2 - should be unit\n");
201     test2.Print();
202     printf("Test matrix 0 - should be unit\n"); 
203     test0.Print();
204     printf("Test matrix 1 - should be diagonal\n"); 
205     test1.Print();
206     printf("Diagonal matrix\n"); 
207     svd.GetSig().Print();
208     printf("Original param matrix\n"); 
209     mparam.Print();
210     printf("Rotated  param matrix\n"); 
211     mparamT.Print();
212     //
213     //
214     printf("Trans Matrix:\n");
215     matrixV.Print();
216     printf("Delta Orig\n");
217     deltaR.Print();
218     printf("Delta Rotated");
219     deltaT.Print();
220     //
221     //    
222   }
223   //
224   Double_t sumChi2=0;
225   for (Int_t idim = 1; idim<3; idim++){
226     Double_t yf;     // input points to fit in TLinear fitter
227     Double_t xf[6];  // input points to fit
228     yf = deltaT(idim,0);
229     for (Int_t ipar =0; ipar<6; ipar++) xf[ipar] = mparamT(idim,ipar); 
230     if (covDiagonal[idim]>0.){
231       fFitter->AddPoint(xf,yf, TMath::Sqrt(covDiagonal[idim]));
232       // accumulate chi2
233       Double_t chi2       = (yf*yf)/covDiagonal[idim];
234       fChi2Orig += (yf*yf)/covDiagonal[idim];  
235       if (chi2>100 && AliLog::GetDebugLevel("","AliTrackResidualsLinear")>1){
236         printf("Too big chi2- %f\n",chi2);
237         printf("Delta Orig\n");
238         deltaR.Print();
239         printf("Delta Rotated");
240         deltaT.Print();
241         matrixV.Print();
242         printf("Too big chi2 - End\n"); 
243       }
244       sumChi2+=chi2;
245     }
246     else{
247       printf("Bug\n");
248     }
249   }
250   if (AliLog::GetDebugLevel("","AliTrackResidualsLinear")>1){
251     TMatrixD matChi0=(mcov.Invert()*deltaR);
252     TMatrixD matChi2=deltaR.T()*matChi0;
253     printf("Chi2:\t%f\t%f", matChi2(0,0), sumChi2);
254   }
255
256   fNdf +=2;
257 }
258
259 //______________________________________________________________________________
260 Bool_t AliTrackResidualsLinear::Update()
261 {
262   // Find the alignment parameters
263   // using TLinear fitter  + fill data containers
264   // 
265   //
266   fFitter->Eval();
267   //
268   // TLinear fitter put as first parameter offset - fixing parameter shifted by one
269   //
270   fFitter->FixParameter(0);
271   for (Int_t ipar =0; ipar<6; ipar++){
272     if (fBFixed[ipar])  fFitter->FixParameter(ipar+1,fFixed[ipar]);
273   }
274   if (fFraction>0.5) {
275     fFitter->EvalRobust(fFraction);
276   }else{
277     fFitter->Eval();
278   }
279   //
280   fFitter->ReleaseParameter(0);
281   for (Int_t ipar=0; ipar<6; ipar++) {
282     if (fBFixed[ipar])  fFitter->ReleaseParameter(ipar+1);
283   }
284     
285
286  
287   fChi2 = fFitter->GetChisquare();
288   fNdf -= 6;
289   TVectorD vector(7);
290   fFitter->GetParameters(vector);
291   fParams[0] = vector[1];
292   fParams[1] = vector[2];
293   fParams[2] = vector[3];  
294   fParams[3] = vector[4];
295   fParams[4] = vector[5];
296   fParams[5] = vector[6];
297   TMatrixD covar(7,7);
298   fFitter->GetCovarianceMatrix(covar);
299   
300   for (Int_t i0=0; i0 <6; i0++){
301     for (Int_t j0=0; j0 <6; j0++){
302       fCovar[i0*6+j0] = covar(i0+1,j0+1);
303     }
304   }
305   Double_t covmatrarray[21];
306   for(Int_t j=0;j<6;j++){
307     for(Int_t i=j;i<6;i++){
308       covmatrarray[i*(i+1)/2+j]=fCovar[i+6*j];
309     }
310   }
311
312  
313   fAlignObj->SetPars(fParams[0], fParams[1], fParams[2],
314                      TMath::RadToDeg()*fParams[3],
315                      TMath::RadToDeg()*fParams[4],
316                      TMath::RadToDeg()*fParams[5]);
317   fAlignObj->SetCorrMatrix(covmatrarray);
318   
319   return kTRUE;
320 }
321
322