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