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;
  47     if (fSolGlo)   delete[] fSolGlo;
  48     if (fSolLoc)   delete[] fSolLoc;
  49     if (fRHSGlo)   delete[] fRHSGlo;
  50     if (fRHSLoc)   delete[] fRHSLoc;
  51     if (fMatGamma) delete[] fMatGamma;
  52     if (fMatG)     delete[] fMatG;
  53     if (fCovDGl)   delete[] fCovDGl;
  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   if (TestBit(kBitLocSol)) return kTRUE;
 106   if (!TestBit(kBitGloSol)) {
 107     AliError("Cannot solve for Locals before SolveGlobals is called");
 108     return kFALSE;
 109   }
 110   for (int i=fNPoints;i--;) {
 111     double beta = fRHSLoc[i];
 112     double *mtG = fMatG + i*fNGlobal;  // G_i
 113     for (int j=fNGlobal;j--;) beta -= mtG[j]*fSolGlo[j];
 114     fSolLoc[i] = beta/fMatGamma[i];   // Gamma^-1 * (beta - G_i * a)
 115   }
 116   SetBit(kBitLocSol);
 117   return kTRUE;
 118 }
 119
 120 //______________________________________________________________________________________
 121 AliSymMatrix* AliParamSolver::GetCovMatrix()
 122 {
 123   if (!TestBit(kBitGloSol)) {
 124     AliError("Cannot obtain Cov.Matrix before SolveGlobals is called");
 125     return 0;
 126   }
 127   if (TestBit(kBitCInv)) return fMatrix;
 128   //
 129   if (fMatrix->InvertChol()) {
 130     SetBit(kBitCInv);
 131     return fMatrix;
 132   }
 133   return 0;
 134 }
 135
 136 //______________________________________________________________________________________
 137 Bool_t AliParamSolver::Solve(Bool_t obtainCov)
 138 {
 139   return (SolveGlobals(obtainCov) && SolveLocals()) ? kTRUE : kFALSE;
 140 }
 141
 142 //______________________________________________________________________________________
 143 void AliParamSolver::AddEquation(const Double_t* dGl,const Double_t *dLc,const Double_t *res, const Double_t *covI)
 144 {
 145   // add the measured point to chi2 normal equations
 146   // Input:
 147   // dGl : NGlo x 3 matrix of derivative for each of the 3 coordinates vs global params
 148   // dLc : 3-vector of derivative for each coordinate vs local param
 149   // res : residual of the point (extrapolated - measured)
 150   // covI: 3 x (3+1)/2 matrix of the (inverted) errors (symmetric)
 151   //
 152   // The contribution of the point to chi2 is  V * covI * V
 153   // with component of the vector V(i) = dGl[i]*glob + dLc[i]*loc - res[i]
 154   //
 155   // Instead of the NGlo + NMeasPoints size matrix we create only NGlo size matrix using the
 156   // reduction a la millepede : http://www.desy.de/~blobel/millepede1.ps
 157   const double kTiny = 1e-16;
 158   //
 159   if (fNPoints+1 == fMaxPoints) ExpandStorage((fNPoints+1)*2);
 160   ResetBit(kBitGloSol|kBitLocSol);
 161   //
 162   if (TestBit(kBitCInv)) { // solution was obtained and the matrix was inverted for previous points
 163     fMatrix->InvertChol();
 164     ResetBit(kBitCInv);
 165   }
 166   //
 167   double *mtG   = fMatG + fNPoints*fNGlobal;  // G_i
 168   double &beta  = fRHSLoc[fNPoints];
 169   double &gamma = fMatGamma[fNPoints];
 170   double cDl[3];
 171   //
 172   // cov * dR/dloc
 173   cDl[kX] = covI[kXX]*dLc[kX] + covI[kXY]*dLc[kY] + covI[kXZ]*dLc[kZ];
 174   cDl[kY] = covI[kXY]*dLc[kX] + covI[kYY]*dLc[kY] + covI[kYZ]*dLc[kZ];
 175   cDl[kZ] = covI[kXZ]*dLc[kX] + covI[kYZ]*dLc[kY] + covI[kZZ]*dLc[kZ];
 176   //
 177   for (int i=fNGlobal;i--;) {
 178     const double *dGli = dGl+i*3;  // derivatives of XYZ vs i-th global param
 179     double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
 180     cDgi[kX] = covI[kXX]*dGli[kX] + covI[kXY]*dGli[kY] + covI[kXZ]*dGli[kZ];
 181     cDgi[kY] = covI[kXY]*dGli[kX] + covI[kYY]*dGli[kY] + covI[kYZ]*dGli[kZ];
 182     cDgi[kZ] = covI[kXZ]*dGli[kX] + covI[kYZ]*dGli[kY] + covI[kZZ]*dGli[kZ];
 183     //
 184     mtG[i]   = cDl[kX]*dGli[kX] + cDl[kY]*dGli[kY] + cDl[kZ]*dGli[kZ];  // dR/dGl_i * cov * dR/dLoc
 185   }
 186   beta    = res[kX]*cDl[kX] + res[kY]*cDl[kY] + res[kZ]*cDl[kZ];  //RHS: res*cov*dR/dloc
 187   gamma   = dLc[kX]*cDl[kX] + dLc[kY]*cDl[kY] + dLc[kZ]*cDl[kZ];  //RHS: dR/dloc*cov*dR/dloc
 188   double locSol  = TMath::Abs(gamma)<kTiny ? 0. : beta/gamma;     //local solution: gamma^-1 beta
 189   //
 190   AliSymMatrix &matC = *fMatrix;
 191   for (int i=fNGlobal;i--;) {
 192     const double *cDgi = fCovDGl+i*3;    // cov * dR/dGl_i
 193     //
 194     fRHSGlo[i] += cDgi[kX]*res[kX] + cDgi[kY]*res[kY] + cDgi[kZ]*res[kZ]      // b_i = dR/dGi * cov * res
 195       -           mtG[i]*locSol;                                              //     - [G gamma^-1 beta ]_i
 196     //
 197     for (int j=i+1;j--;) {
 198       //      const double *cDgj = fCovDGl+j*3;  // cov * dR/dGl_j
 199       const double *dGlj = dGl+j*3;        // derivatives of XYZ vs i-th global param
 200       matC(i,j) += dGlj[kX]*cDgi[kX] + dGlj[kY]*cDgi[kY] + dGlj[kZ]*cDgi[kZ]  // C_ij = dR/dGi * cov * dR/dGj
 201         - mtG[i]*mtG[j]/gamma;                                                //        - [G gamma^-1 T(G) ]_ij
 202     }
 203   }
 204   //
 205   fNPoints++;
 206   //
 207 }
 208
 209 //______________________________________________________________________________________
 210 void AliParamSolver::AddConstraint(Int_t parID, Double_t val, Double_t err2inv)
 211 {
 212   // add gassian constriant to parameter parID
 213   if (parID>=fNGlobal) {
 214     AliError(Form("Attempt to constraint non-existing parameter %d",parID));
 215     return;
 216   }
 217   //
 218   (*fMatrix)(parID,parID) += err2inv;
 219   fRHSGlo[parID] += val*err2inv;
 220   //
 221 }
 222
 223 //______________________________________________________________________________________
 224 void AliParamSolver::Init(Int_t npini)
 225 {
 226   // create storage assuming maximum npini measured points
 227   fMatrix = new AliSymMatrix(fMaxGlobal);
 228   fSolGlo = new Double_t[fMaxGlobal];
 229   fRHSGlo = new Double_t[fMaxGlobal];
 230   //
 231   fCovDGl = new Double_t[3*fMaxGlobal];
 232   ExpandStorage(npini);
 233   fMatrix->SetSizeUsed(fNGlobal);
 234   //
 235 }
 236
 237 //______________________________________________________________________________________
 238 void AliParamSolver::ExpandStorage(Int_t newSize)
 239 {
 240   // increase space to newSize measured points
 241   newSize = newSize>fMaxPoints ? newSize : fMaxPoints+1;
 242   double* tmp;
 243   tmp = new Double_t[newSize];
 244   if (fSolLoc) delete[] fSolLoc;
 245   fSolLoc = tmp;
 246   //
 247   tmp = new Double_t[newSize];
 248   if (fMatGamma) {
 249     memcpy(tmp,fMatGamma,fNPoints*sizeof(Double_t));
 250     delete[] fMatGamma;
 251   }
 252   fMatGamma = tmp;
 253   //
 254   tmp = new Double_t[newSize];
 255   if (fRHSLoc) {
 256     memcpy(tmp,fRHSLoc,fNPoints*sizeof(Double_t));
 257     delete[] fRHSLoc;
 258   }
 259   fRHSLoc = tmp;
 260   //
 261   tmp = new Double_t[newSize*fMaxGlobal];
 262   if (fMatG) {
 263     memcpy(tmp,fMatG,fNPoints*fMaxGlobal*sizeof(Double_t));
 264     delete[] fMatG;
 265   }
 266   fMatG = tmp;
 267   //
 268   fMaxPoints = newSize;
 269   //
 270 }
 271
 272 //______________________________________________________________________________________
 273 void AliParamSolver::Clear(Option_t*)
 274 {
 275   fNPoints = 0;
 276   fMatrix->Reset();
 277   for (int i=fNGlobal;i--;) fRHSGlo[i] = 0;
 278   ResetBit(kBitGloSol | kBitLocSol | kBitCInv);
 279 }
 280
 281 //______________________________________________________________________________________
 282 void AliParamSolver::Print(Option_t*) const
 283 {
 284   AliInfo(Form("Solver with %d globals for %d points",fNGlobal,fNPoints));
 285 }
 286
 287 //______________________________________________________________________________________
 288 void AliParamSolver::SetNGlobal(Int_t n)
 289 {
 290   if (n>fMaxGlobal) {
 291     AliError(Form("Maximum number of globals was set to %n",fMaxGlobal));
 292     return;
 293   }
 294   fNGlobal = n;
 295   fMatrix->SetSizeUsed(fNGlobal);
 296 }
 297
 298 //______________________________________________________________________________________
 299 void AliParamSolver::SetMaxGlobal(Int_t n)
 300 {
 301   if (n>0 && n==fMaxGlobal) return;
 302   fMaxGlobal = n;
 303   fNGlobal = n;
 304   if (fMatrix)   delete   fMatrix;
 305   if (fSolGlo)   delete[] fSolGlo;
 306   if (fSolLoc)   delete[] fSolLoc;
 307   if (fRHSGlo)   delete[] fRHSGlo;
 308   if (fRHSLoc)   delete[] fRHSLoc;
 309   if (fMatGamma) delete[] fMatGamma;
 310   if (fMatG)     delete[] fMatG;
 311   if (fCovDGl)   delete[] fCovDGl;
 312   n = TMath::Max(16,fMaxPoints);
 313   Init(n);
 314   //
 315 }
 316