Number of sigma pedestal cut increased to 4
[u/mrichter/AliRoot.git] / STEER / STEER / AliParamSolver.cxx
1 #include "AliParamSolver.h"
2 #include "AliSymMatrix.h"
3 #include "AliLog.h"
4 #include <TMath.h>
5
6 ClassImp(AliParamSolver)
7
8 //______________________________________________________________________________________
9 AliParamSolver::AliParamSolver()
10 : fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(0),fNGlobal(0),fNPoints(0),fMaxPoints(0),
11   fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
12
13   // default constructor
14 }
15
16 //______________________________________________________________________________________
17 AliParamSolver::AliParamSolver(Int_t maxglo,Int_t locbuff)
18 : fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(maxglo),fNGlobal(maxglo),fNPoints(0),fMaxPoints(0),
19   fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
20
21   // constructor for nglo globals
22   Init(locbuff);
23 }
24
25 //______________________________________________________________________________________
26 AliParamSolver::AliParamSolver(AliParamSolver& src)
27   : TObject(src),fMatrix(0),fSolGlo(0),fSolLoc(0),fMaxGlobal(src.fMaxGlobal),fNGlobal(src.fNGlobal),
28     fNPoints(0),fMaxPoints(0),fRHSGlo(0),fRHSLoc(0),fMatGamma(0),fMatG(0),fCovDGl(0)
29
30   // copy constructor 
31   if (src.fMatrix) {
32     Init(src.fMaxPoints);
33     (*this) = src;
34   }
35 }
36
37 //______________________________________________________________________________________
38 AliParamSolver& AliParamSolver::operator=(const AliParamSolver& src)
39 {
40   // assignment operator
41   if (this==&src) return *this;
42   TObject::operator=(src);
43   if (src.fMatrix && (fNGlobal!=src.fNGlobal || fMaxPoints<src.fNPoints)) {
44     fNGlobal   = src.fNGlobal;
45     fMaxGlobal = src.fMaxGlobal;
46     if (fMatrix)   delete   fMatrix; fMatrix = 0;
47     if (fSolGlo)   delete[] fSolGlo; fSolGlo = 0;
48     if (fSolLoc)   delete[] fSolLoc; fSolLoc = 0;
49     if (fRHSGlo)   delete[] fRHSGlo; fRHSGlo = 0;
50     if (fRHSLoc)   delete[] fRHSLoc; fRHSLoc = 0;
51     if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
52     if (fMatG)     delete[] fMatG; fMatG = 0;
53     if (fCovDGl)   delete[] fCovDGl; fCovDGl = 0;
54     Init(src.fMaxPoints);
55   }
56   if (src.fMatrix) {
57     (*fMatrix) = *(src.fMatrix);
58     memcpy(fSolGlo,src.fSolGlo,fNGlobal*sizeof(double));
59     memcpy(fSolLoc,src.fSolLoc,fNPoints*sizeof(double));
60     memcpy(fRHSGlo,src.fRHSGlo,fNGlobal*sizeof(double));
61     memcpy(fRHSLoc,src.fRHSLoc,fNPoints*sizeof(double));
62     memcpy(fMatGamma,src.fMatGamma,fNPoints*sizeof(double));
63     memcpy(fMatG,src.fMatG,fNPoints*fNGlobal*sizeof(double));
64   }
65   //
66   return *this;
67 }
68
69 //______________________________________________________________________________________
70 AliParamSolver::~AliParamSolver()
71
72   // destructor
73   delete fMatrix;
74   delete[] fSolGlo;
75   delete[] fSolLoc;
76   delete[] fRHSGlo;
77   delete[] fRHSLoc;
78   delete[] fMatGamma;
79   delete[] fMatG;
80   delete[] fCovDGl;
81 }
82
83 //______________________________________________________________________________________
84 Bool_t AliParamSolver::SolveGlobals(Bool_t obtainCov)
85 {
86   if (fNPoints<fNGlobal/3) {
87     AliError(Form("Number of points: %d is not enough for %d globals",fNPoints,fNGlobal));
88     return kFALSE;
89   }
90   //
91   if (!TestBit(kBitGloSol)) {
92     if (!fMatrix->SolveChol(fRHSGlo, fSolGlo, obtainCov)) {
93       AliError("Solution Failed");
94       return kFALSE;
95     }
96     SetBit(kBitGloSol);
97     if (obtainCov) SetBit(kBitCInv);
98   }
99   return kTRUE;
100 }
101
102 //______________________________________________________________________________________
103 Bool_t AliParamSolver::SolveLocals()
104 {
105   const double kTiny = 1e-16;
106   if (TestBit(kBitLocSol)) return kTRUE;
107   if (!TestBit(kBitGloSol)) {
108     AliError("Cannot solve for Locals before SolveGlobals is called");
109     return kFALSE;
110   }
111   for (int i=fNPoints;i--;) {
112     if (TMath::Abs(fMatGamma[i])<kTiny) {fSolLoc[i] = 0; continue;}
113     double beta = fRHSLoc[i];
114     double *mtG = fMatG + i*fNGlobal;  // G_i
115     for (int j=fNGlobal;j--;) beta -= mtG[j]*fSolGlo[j];
116     fSolLoc[i] = beta/fMatGamma[i];   // Gamma^-1 * (beta - G_i * a)
117   }
118   SetBit(kBitLocSol);
119   return kTRUE;
120 }
121
122 //______________________________________________________________________________________
123 AliSymMatrix* AliParamSolver::GetCovMatrix()
124 {
125   if (!TestBit(kBitGloSol)) {
126     AliError("Cannot obtain Cov.Matrix before SolveGlobals is called");
127     return 0;
128   }
129   if (TestBit(kBitCInv)) return fMatrix;
130   //
131   if (fMatrix->InvertChol()) {
132     SetBit(kBitCInv);
133     return fMatrix;
134   }
135   return 0;
136 }
137
138 //______________________________________________________________________________________
139 Bool_t AliParamSolver::Solve(Bool_t obtainCov)
140 {
141   return (SolveGlobals(obtainCov) && SolveLocals()) ? kTRUE : kFALSE; 
142 }
143
144 //______________________________________________________________________________________
145 void AliParamSolver::AddEquation(const Double_t* dGl,const Double_t *dLc,const Double_t *res, const Double_t *covI,Double_t sclErrI)
146 {
147   // add the measured point to chi2 normal equations
148   // Input: 
149   // dGl : NGlo x 3 matrix of derivative for each of the 3 coordinates vs global params
150   // dLc : 3-vector of derivative for each coordinate vs local param
151   // res : residual of the point (extrapolated - measured)
152   // covI: 3 x (3+1)/2 matrix of the (inverted) errors (symmetric)
153   // sclErrI: scaling coefficient to apply to inverted errors (used if >0)
154   // The contribution of the point to chi2 is  V * covI * V 
155   // with component of the vector V(i) = dGl[i]*glob + dLc[i]*loc - res[i] 
156   //
157   // Instead of the NGlo + NMeasPoints size matrix we create only NGlo size matrix using the 
158   // reduction a la millepede : http://www.desy.de/~blobel/millepede1.ps
159   const double kTiny = 1e-16;
160   //
161   if (fNPoints+1 == fMaxPoints) ExpandStorage((fNPoints+1)*2);
162   ResetBit(kBitGloSol|kBitLocSol);
163   //
164   if (TestBit(kBitCInv)) { // solution was obtained and the matrix was inverted for previous points
165     fMatrix->InvertChol();
166     ResetBit(kBitCInv);
167   }
168   //
169   double *mtG   = fMatG + fNPoints*fNGlobal;  // G_i
170   double &beta  = fRHSLoc[fNPoints];
171   double &gamma = fMatGamma[fNPoints];
172   double cDl[3];
173   //
174   // cov * dR/dloc
175   cDl[kX] = covI[kXX]*dLc[kX] + covI[kXY]*dLc[kY] + covI[kXZ]*dLc[kZ];
176   cDl[kY] = covI[kXY]*dLc[kX] + covI[kYY]*dLc[kY] + covI[kYZ]*dLc[kZ];
177   cDl[kZ] = covI[kXZ]*dLc[kX] + covI[kYZ]*dLc[kY] + covI[kZZ]*dLc[kZ];
178   if (sclErrI>0) { cDl[kX] *= sclErrI; cDl[kY] *= sclErrI; cDl[kZ] *= sclErrI;}
179   //
180   for (int i=fNGlobal;i--;) {
181     const double *dGli = dGl+i*3;  // derivatives of XYZ vs i-th global param
182     double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
183     cDgi[kX] = covI[kXX]*dGli[kX] + covI[kXY]*dGli[kY] + covI[kXZ]*dGli[kZ];
184     cDgi[kY] = covI[kXY]*dGli[kX] + covI[kYY]*dGli[kY] + covI[kYZ]*dGli[kZ];
185     cDgi[kZ] = covI[kXZ]*dGli[kX] + covI[kYZ]*dGli[kY] + covI[kZZ]*dGli[kZ];
186     if (sclErrI>0) { cDgi[kX] *= sclErrI; cDgi[kY] *= sclErrI; cDgi[kZ] *= sclErrI;}
187     //
188     mtG[i]   = cDl[kX]*dGli[kX] + cDl[kY]*dGli[kY] + cDl[kZ]*dGli[kZ];  // dR/dGl_i * cov * dR/dLoc
189   }
190   beta    = res[kX]*cDl[kX] + res[kY]*cDl[kY] + res[kZ]*cDl[kZ];  //RHS: res*cov*dR/dloc
191   gamma   = dLc[kX]*cDl[kX] + dLc[kY]*cDl[kY] + dLc[kZ]*cDl[kZ];  //RHS: dR/dloc*cov*dR/dloc
192   double locSol  = TMath::Abs(gamma)<kTiny ? 0. : beta/gamma;     //local solution: gamma^-1 beta
193   //
194   AliSymMatrix &matC = *fMatrix;
195   for (int i=fNGlobal;i--;) {
196     const double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
197     //
198     fRHSGlo[i] += cDgi[kX]*res[kX] + cDgi[kY]*res[kY] + cDgi[kZ]*res[kZ]      // b_i = dR/dGi * cov * res
199       -           mtG[i]*locSol;                                              //     - [G gamma^-1 beta ]_i
200     //
201     for (int j=i+1;j--;) {
202       //      const double *cDgj = fCovDGl+j*3;  // cov * dR/dGl_j
203       const double *dGlj = dGl+j*3;        // derivatives of XYZ vs i-th global param
204       double add = dGlj[kX]*cDgi[kX] + dGlj[kY]*cDgi[kY] + dGlj[kZ]*cDgi[kZ];  // C_ij = dR/dGi * cov * dR/dGj
205       if (TMath::Abs(gamma)>kTiny) add -= mtG[i]*mtG[j]/gamma;                 //        - [G gamma^-1 T(G) ]_ij      
206       matC(i,j) += add;       
207     }
208   }
209   //
210   fNPoints++;
211   //
212 }
213
214 //______________________________________________________________________________________
215 void AliParamSolver::AddConstraint(Int_t parID, Double_t val, Double_t err2inv)
216 {
217   // add gassian constriant to parameter parID
218   if (parID>=fNGlobal) {
219     AliError(Form("Attempt to constraint non-existing parameter %d",parID));
220     return;
221   }
222   //
223   (*fMatrix)(parID,parID) += err2inv;
224   fRHSGlo[parID] += val*err2inv;
225   //
226 }
227
228 //______________________________________________________________________________________
229 void AliParamSolver::Init(Int_t npini)
230 {
231   // create storage assuming maximum npini measured points
232   fMatrix = new AliSymMatrix(fMaxGlobal);
233   fSolGlo = new Double_t[fMaxGlobal];
234   fRHSGlo = new Double_t[fMaxGlobal];
235   //
236   fCovDGl = new Double_t[3*fMaxGlobal];
237   ExpandStorage(npini);
238   fMatrix->SetSizeUsed(fNGlobal);
239   //
240 }
241
242 //______________________________________________________________________________________
243 void AliParamSolver::ExpandStorage(Int_t newSize)
244 {
245   // increase space to newSize measured points
246   newSize = newSize>fMaxPoints ? newSize : fMaxPoints+1;
247   double* tmp;
248   tmp = new Double_t[newSize];
249   if (fSolLoc) delete[] fSolLoc; 
250   fSolLoc = tmp;
251   //
252   tmp = new Double_t[newSize];
253   if (fMatGamma) {
254     memcpy(tmp,fMatGamma,fNPoints*sizeof(Double_t));
255     delete[] fMatGamma;
256   }
257   fMatGamma = tmp;
258   //
259   tmp = new Double_t[newSize];
260   if (fRHSLoc) {
261     memcpy(tmp,fRHSLoc,fNPoints*sizeof(Double_t));
262     delete[] fRHSLoc;
263   }
264   fRHSLoc = tmp;
265   //
266   tmp = new Double_t[newSize*fMaxGlobal];
267   if (fMatG) {
268     memcpy(tmp,fMatG,fNPoints*fMaxGlobal*sizeof(Double_t));
269     delete[] fMatG;
270   }
271   fMatG = tmp;
272   //
273   fMaxPoints = newSize;
274   //
275 }
276
277 //______________________________________________________________________________________
278 void AliParamSolver::Clear(Option_t*)
279 {
280   fNPoints = 0;
281   fMatrix->Reset();
282   for (int i=fNGlobal;i--;) fRHSGlo[i] = 0;
283   ResetBit(kBitGloSol | kBitLocSol | kBitCInv);
284 }
285
286 //______________________________________________________________________________________
287 void AliParamSolver::Print(Option_t*) const
288 {
289   AliInfo(Form("Solver with %d globals for %d points",fNGlobal,fNPoints));
290 }
291
292 //______________________________________________________________________________________
293 void AliParamSolver::SetNGlobal(Int_t n) 
294 {
295   if (n>fMaxGlobal) {
296     AliError(Form("Maximum number of globals was set to %d",fMaxGlobal));
297     return;
298   }
299   fNGlobal = n;
300   fMatrix->SetSizeUsed(fNGlobal);
301 }
302
303 //______________________________________________________________________________________
304 void AliParamSolver::SetMaxGlobal(Int_t n) 
305 {
306   if (n>0 && n==fMaxGlobal) return;
307   fMaxGlobal = n;
308   fNGlobal = n;
309   if (fMatrix)   delete   fMatrix;   fMatrix = 0;
310   if (fSolGlo)   delete[] fSolGlo;   fSolGlo = 0;
311   if (fSolLoc)   delete[] fSolLoc;   fSolLoc = 0;
312   if (fRHSGlo)   delete[] fRHSGlo;   fRHSGlo = 0;
313   if (fRHSLoc)   delete[] fRHSLoc;   fRHSLoc = 0;
314   if (fMatGamma) delete[] fMatGamma; fMatGamma = 0;
315   if (fMatG)     delete[] fMatG;     fMatG = 0;
316   if (fCovDGl)   delete[] fCovDGl;   fCovDGl = 0;
317   n = TMath::Max(16,fMaxPoints);
318   Init(n);
319   //
320 }
321