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