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