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