[u/mrichter/AliRoot.git] / STEER / AliTrackResidualsLinear.cxx

/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

//-----------------------------------------------------------------
//   Implementation of the derived class for track residuals
//   based on linear chi2 minimization 
//  The minimization relies on the fact that the alignment parameters     
//   (angles and translations) are small.                                  
//   TLinearFitter used for minimization
//   Possibility to fix Paramaters
//   FixParameter()ReleaseParameter();
//   Possibility to define fraction of outliers to be skipped
//
//   marian.ivanov@cern.ch
//
//-----------------------------------------------------------------

#include <TMinuit.h>
#include <TGeoMatrix.h>

#include "AliLog.h"
#include "AliAlignObj.h"
#include "AliTrackPointArray.h"
#include "AliTrackResidualsLinear.h"
#include "AliAlignObj.h"
#include "TLinearFitter.h"
#include  "TDecompSVD.h"

ClassImp(AliTrackResidualsLinear)

//______________________________________________________________________________
AliTrackResidualsLinear::AliTrackResidualsLinear():
  AliTrackResiduals(),
  fFitter(0),
  fFraction(-1),
  fChi2Orig(0)
{
  // Default constructor
  for (Int_t ipar=0; ipar<6; ipar++){
    fBFixed[ipar] = kFALSE;
    fFixed[ipar]  = 0;;
    fParams[ipar]  = 0;
  }  
}

//______________________________________________________________________________
AliTrackResidualsLinear::AliTrackResidualsLinear(Int_t ntracks):
  AliTrackResiduals(ntracks),
  fFitter(new TLinearFitter(6,"hyp6")),
  fFraction(-1),
  fChi2Orig(0)
{
  // Constructor
  for (Int_t ipar=0; ipar<6; ipar++){
    fBFixed[ipar] = kFALSE;
    fFixed[ipar]  = 0;
    fParams[ipar]  = 0;
  }
}
 
//______________________________________________________________________________
AliTrackResidualsLinear::AliTrackResidualsLinear(const AliTrackResidualsLinear &res):
  AliTrackResiduals(res),
  fFitter(new TLinearFitter(*(res.fFitter))),
  fFraction(res.fFraction),
  fChi2Orig(res.fChi2Orig)
{
  // Copy constructor
  //..
  for (Int_t ipar=0; ipar<6; ipar++){
    fBFixed[ipar]  = res.fBFixed[ipar];
    fFixed[ipar]   = res.fFixed[ipar];
    fParams[ipar]  = res.fParams[ipar];
  }
}

//______________________________________________________________________________
AliTrackResidualsLinear &AliTrackResidualsLinear::operator= (const AliTrackResidualsLinear& res)
{
  // Assignment operator
 ((AliTrackResiduals *)this)->operator=(res);
 return *this;
}
//______________________________________________________________________________
AliTrackResidualsLinear::~AliTrackResidualsLinear()
{
  //
  //
  //
  delete fFitter;
}


//______________________________________________________________________________
Bool_t AliTrackResidualsLinear::Minimize()
{
  // Implementation of fast linear Chi2 minimizer
  // based on TLinear fitter
  //
  if (!fFitter) fFitter = new TLinearFitter(6,"hyp6");
  fFitter->StoreData(kTRUE);
  fFitter->ClearPoints();
  fChi2Orig = 0;
  AliTrackPoint p1,p2;
  for (Int_t itrack = 0; itrack < fLast; itrack++) {
    if (!fVolArray[itrack] || !fTrackArray[itrack]) continue;
    for (Int_t ipoint = 0; ipoint < fVolArray[itrack]->GetNPoints(); ipoint++) {
      fVolArray[itrack]->GetPoint(p1,ipoint);
      fTrackArray[itrack]->GetPoint(p2,ipoint);
      AddPoints(p1,p2);
    }
  }
  Bool_t isOK = Update();
  if (!isOK) return isOK;
  //
  TGeoHMatrix  matrix;
  fAlignObj->GetMatrix(matrix);
  return isOK;
}


//______________________________________________________________________________
void AliTrackResidualsLinear::AddPoints(AliTrackPoint &p, AliTrackPoint &pprime)
{
  //
  // add points to linear fitter - option with correlation betwee measurement in different dimensions
  // p1      - local point
  // pprime  - track extrapolation point
  //
  Float_t xyz[3],xyzp[3];
  Float_t cov[6],covp[6];
  p.GetXYZ(xyz,cov); pprime.GetXYZ(xyzp,covp);
  //
  //
  TMatrixD mcov(3,3);   // local point covariance
  mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
  mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
  mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
  TMatrixD mcovp(3,3); //  extrapolation point covariance  
  mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
  mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
  mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
  mcov+=mcovp;
  mcov.Invert();
  if (!mcov.IsValid()) return; 
  TMatrixD mcovBack = mcov;  // for debug purposes
  //
  // decompose matrix
  //
  TDecompSVD svd(mcov);              // mcov  = svd.fU * covDiagonal * svd.fV.Invert   
  if (!svd.Decompose()) return;      // decomposition failed
  TMatrixD   matrixV = svd.GetV();   // transformation matrix to diagonalize covariance matrix
  Double_t   covDiagonal[3] = {svd.GetSig()[0],svd.GetSig()[1],svd.GetSig()[2]};    // diagonalized covariance matrix
  //
  // residual vector 
  TMatrixD  deltaR(3,1);
  deltaR(0,0) = (xyzp[0]-xyz[0]); 
  deltaR(1,0) = (xyzp[1]-xyz[1]);
  deltaR(2,0) = (xyzp[2]-xyz[2]);   
  //
  // parametrization matrix
  //
  TMatrixD        mparam(3,6);
  mparam(0,0) = 1;      mparam(1,0) = 0;       mparam(2,0) = 0;            // xshift
  mparam(0,1) = 0;      mparam(1,1) = 1;       mparam(2,1) = 0;            // yshift
  mparam(0,2) = 0;      mparam(1,2) = 0;       mparam(2,2) = 1;            // zshift
  mparam(0,3) = 0;      mparam(1,3) =-xyz[2];  mparam(2,3) = xyz[1];       // x rotation
  mparam(0,4) = xyz[2]; mparam(1,4) = 0;       mparam(2,4) =-xyz[0];       // y rotation
  mparam(0,5) =-xyz[1]; mparam(1,5) = xyz[0];  mparam(2,5) = 0;            // z rotation
  //
  
  TMatrixD  deltaT(matrixV, TMatrixD::kTransposeMult, deltaR);   // tranformed delta
  TMatrixD  mparamT(matrixV,TMatrixD::kTransposeMult, mparam);   // tranformed linear transformation
  if (0){    
    //
    // debug part
    //
    //   covDiag = U^-1 * mcov * V      -- diagonalization of covariance matrix

    TMatrixD   matrixU = svd.GetU();                      // transformation matrix to diagonalize covariance matrix
    TMatrixD   matrixUI= svd.GetU(); 
    matrixUI.Invert();
    //
    TMatrixD   test0   = matrixUI*matrixV;                // test matrix - should be unit matrix
    TMatrixD   test1   = matrixUI*mcovBack*matrixV;       // test matrix - diagonal - should be diagonal with covDiagonal on diag
    TMatrixD   test2   = matrixU.T()*matrixV;             // test ortogonality - shoul be unit
    printf("Test matrix 2 - should be unit\n");
    test2.Print();
    printf("Test matrix 0 - should be unit\n"); 
    test0.Print();
    printf("Test matrix 1 - should be diagonal\n"); 
    test1.Print();
    printf("Diagonal matrix\n"); 
    svd.GetSig().Print();
    printf("Original param matrix\n"); 
    mparam.Print();
    printf("Rotated  param matrix\n"); 
    mparamT.Print();
    //
  }
  //
  for (Int_t idim = 0; idim<3; idim++){
    Double_t yf;     // input points to fit in TLinear fitter
    Double_t xf[6];  // input points to fit
    yf = deltaT(idim,0);
    for (Int_t ipar =0; ipar<6; ipar++) xf[ipar] = mparamT(idim,ipar); 
    if (covDiagonal[idim]>0.){
      fFitter->AddPoint(xf,yf, TMath::Sqrt(1/covDiagonal[idim]));
    }
    // accumulate chi2
    fChi2Orig += (yf*yf)*covDiagonal[idim];          
  }
  fNdf +=3;
}

//______________________________________________________________________________
Bool_t AliTrackResidualsLinear::Update()
{
  // Find the alignment parameters
  // using TLinear fitter  + fill data containers
  // 
  //
  fFitter->Eval();
  //
  // TLinear fitter put as first parameter offset - fixing parameter shifted by one
  //
  fFitter->FixParameter(0);
  for (Int_t ipar =0; ipar<6; ipar++){
    if (fBFixed[ipar])  fFitter->FixParameter(ipar+1,fFixed[ipar]);
  }
  if (fFraction>0.5) {
    fFitter->EvalRobust(fFraction);
  }else{
    fFitter->Eval();
  }
  //
  fFitter->ReleaseParameter(0);
  for (Int_t ipar=0; ipar<7; ipar++) {
    if (fBFixed[ipar])  fFitter->ReleaseParameter(ipar+1);
  }
    

  //
  fChi2 = fFitter->GetChisquare();
  fNdf -= 6;
  TVectorD vector(7);
  fFitter->GetParameters(vector);
  fParams[0] = vector[1];
  fParams[1] = vector[2];
  fParams[2] = vector[3];  
  fParams[3] = vector[4];
  fParams[4] = vector[5];
  fParams[5] = vector[6];
  //
  fAlignObj->SetPars(fParams[0], fParams[1], fParams[2],
		     TMath::RadToDeg()*fParams[3],
		     TMath::RadToDeg()*fParams[4],
		     TMath::RadToDeg()*fParams[5]);
  return kTRUE;
}
Commit	Line	Data
bf9a6ef9	1	/**************************************************************************
	2	* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
	3	* *
	4	* Author: The ALICE Off-line Project. *
	5	* Contributors are mentioned in the code where appropriate. *
	6	* *
	7	* Permission to use, copy, modify and distribute this software and its *
	8	* documentation strictly for non-commercial purposes is hereby granted *
	9	* without fee, provided that the above copyright notice appears in all *
	10	* copies and that both the copyright notice and this permission notice *
	11	* appear in the supporting documentation. The authors make no claims *
	12	* about the suitability of this software for any purpose. It is *
	13	* provided "as is" without express or implied warranty. *
	14	**************************************************************************/
	15
	16	//-----------------------------------------------------------------
	17	// Implementation of the derived class for track residuals
	18	// based on linear chi2 minimization
	19	// The minimization relies on the fact that the alignment parameters
	20	// (angles and translations) are small.
	21	// TLinearFitter used for minimization
	22	// Possibility to fix Paramaters
	23	// FixParameter()ReleaseParameter();
	24	// Possibility to define fraction of outliers to be skipped
	25	//
	26	// marian.ivanov@cern.ch
	27	//
	28	//-----------------------------------------------------------------
	29
	30	#include <TMinuit.h>
	31	#include <TGeoMatrix.h>
	32
	33	#include "AliLog.h"
	34	#include "AliAlignObj.h"
	35	#include "AliTrackPointArray.h"
	36	#include "AliTrackResidualsLinear.h"
	37	#include "AliAlignObj.h"
	38	#include "TLinearFitter.h"
	39	#include "TDecompSVD.h"
	40
	41	ClassImp(AliTrackResidualsLinear)
	42
	43	//______________________________________________________________________________
75e3794b	44	AliTrackResidualsLinear::AliTrackResidualsLinear():
	45	AliTrackResiduals(),
	46	fFitter(0),
	47	fFraction(-1),
	48	fChi2Orig(0)
bf9a6ef9	49	{
bf9a6ef9	50	// Default constructor
bf9a6ef9	51	for (Int_t ipar=0; ipar<6; ipar++){
	52	fBFixed[ipar] = kFALSE;
	53	fFixed[ipar] = 0;;
	54	fParams[ipar] = 0;
	55	}
	56	}
	57
	58	//______________________________________________________________________________
	59	AliTrackResidualsLinear::AliTrackResidualsLinear(Int_t ntracks):
75e3794b	60	AliTrackResiduals(ntracks),
	61	fFitter(new TLinearFitter(6,"hyp6")),
	62	fFraction(-1),
	63	fChi2Orig(0)
bf9a6ef9	64	{
bf9a6ef9	65	// Constructor
bf9a6ef9	66	for (Int_t ipar=0; ipar<6; ipar++){
	67	fBFixed[ipar] = kFALSE;
	68	fFixed[ipar] = 0;
	69	fParams[ipar] = 0;
	70	}
	71	}
	72
	73	//______________________________________________________________________________
	74	AliTrackResidualsLinear::AliTrackResidualsLinear(const AliTrackResidualsLinear &res):
75e3794b	75	AliTrackResiduals(res),
	76	fFitter(new TLinearFitter(*(res.fFitter))),
	77	fFraction(res.fFraction),
	78	fChi2Orig(res.fChi2Orig)
bf9a6ef9	79	{
	80	// Copy constructor
	81	//..
bf9a6ef9	82	for (Int_t ipar=0; ipar<6; ipar++){
	83	fBFixed[ipar] = res.fBFixed[ipar];
	84	fFixed[ipar] = res.fFixed[ipar];
	85	fParams[ipar] = res.fParams[ipar];
	86	}
bf9a6ef9	87	}
	88
	89	//______________________________________________________________________________
	90	AliTrackResidualsLinear &AliTrackResidualsLinear::operator= (const AliTrackResidualsLinear& res)
	91	{
	92	// Assignment operator
	93	((AliTrackResiduals *)this)->operator=(res);
	94	return *this;
	95	}
	96	//______________________________________________________________________________
	97	AliTrackResidualsLinear::~AliTrackResidualsLinear()
	98	{
	99	//
	100	//
	101	//
	102	delete fFitter;
	103	}
	104
	105
	106	//______________________________________________________________________________
	107	Bool_t AliTrackResidualsLinear::Minimize()
	108	{
	109	// Implementation of fast linear Chi2 minimizer
	110	// based on TLinear fitter
	111	//
	112	if (!fFitter) fFitter = new TLinearFitter(6,"hyp6");
	113	fFitter->StoreData(kTRUE);
	114	fFitter->ClearPoints();
	115	fChi2Orig = 0;
	116	AliTrackPoint p1,p2;
	117	for (Int_t itrack = 0; itrack < fLast; itrack++) {
	118	if (!fVolArray[itrack] \|\| !fTrackArray[itrack]) continue;
	119	for (Int_t ipoint = 0; ipoint < fVolArray[itrack]->GetNPoints(); ipoint++) {
	120	fVolArray[itrack]->GetPoint(p1,ipoint);
	121	fTrackArray[itrack]->GetPoint(p2,ipoint);
	122	AddPoints(p1,p2);
	123	}
	124	}
	125	Bool_t isOK = Update();
	126	if (!isOK) return isOK;
	127	//
	128	TGeoHMatrix matrix;
	129	fAlignObj->GetMatrix(matrix);
	130	return isOK;
	131	}
	132
	133
	134
	135	//______________________________________________________________________________
	136	void AliTrackResidualsLinear::AddPoints(AliTrackPoint &p, AliTrackPoint &pprime)
	137	{
	138	//
	139	// add points to linear fitter - option with correlation betwee measurement in different dimensions
	140	// p1 - local point
	141	// pprime - track extrapolation point
	142	//
	143	Float_t xyz[3],xyzp[3];
	144	Float_t cov[6],covp[6];
	145	p.GetXYZ(xyz,cov); pprime.GetXYZ(xyzp,covp);
	146	//
	147	//
	148	TMatrixD mcov(3,3); // local point covariance
	149	mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
	150	mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
151	mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
152	TMatrixD mcovp(3,3); // extrapolation point covariance
153	mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
154	mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
155	mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
156	mcov+=mcovp;
157	mcov.Invert();
158	if (!mcov.IsValid()) return;
159	TMatrixD mcovBack = mcov; // for debug purposes
160	//
161	// decompose matrix
162	//
163	TDecompSVD svd(mcov); // mcov = svd.fU * covDiagonal * svd.fV.Invert
164	if (!svd.Decompose()) return; // decomposition failed
165	TMatrixD matrixV = svd.GetV(); // transformation matrix to diagonalize covariance matrix
166	Double_t covDiagonal[3] = {svd.GetSig()[0],svd.GetSig()[1],svd.GetSig()[2]}; // diagonalized covariance matrix
167	//
168	// residual vector
169	TMatrixD deltaR(3,1);
170	deltaR(0,0) = (xyzp[0]-xyz[0]);
171	deltaR(1,0) = (xyzp[1]-xyz[1]);
172	deltaR(2,0) = (xyzp[2]-xyz[2]);
173	//
174	// parametrization matrix
175	//
176	TMatrixD mparam(3,6);
177	mparam(0,0) = 1; mparam(1,0) = 0; mparam(2,0) = 0; // xshift
178	mparam(0,1) = 0; mparam(1,1) = 1; mparam(2,1) = 0; // yshift
179	mparam(0,2) = 0; mparam(1,2) = 0; mparam(2,2) = 1; // zshift
180	mparam(0,3) = 0; mparam(1,3) =-xyz[2]; mparam(2,3) = xyz[1]; // x rotation
181	mparam(0,4) = xyz[2]; mparam(1,4) = 0; mparam(2,4) =-xyz[0]; // y rotation
182	mparam(0,5) =-xyz[1]; mparam(1,5) = xyz[0]; mparam(2,5) = 0; // z rotation
183	//
184
185	TMatrixD deltaT(matrixV, TMatrixD::kTransposeMult, deltaR); // tranformed delta
186	TMatrixD mparamT(matrixV,TMatrixD::kTransposeMult, mparam); // tranformed linear transformation
187	if (0){
188	//
189	// debug part
190	//
191	// covDiag = U^-1 * mcov * V -- diagonalization of covariance matrix
192
193	TMatrixD matrixU = svd.GetU(); // transformation matrix to diagonalize covariance matrix
194	TMatrixD matrixUI= svd.GetU();
195	matrixUI.Invert();
196	//
197	TMatrixD test0 = matrixUI*matrixV; // test matrix - should be unit matrix
198	TMatrixD test1 = matrixUImcovBackmatrixV; // test matrix - diagonal - should be diagonal with covDiagonal on diag
199	TMatrixD test2 = matrixU.T()*matrixV; // test ortogonality - shoul be unit
200	printf("Test matrix 2 - should be unit\n");
201	test2.Print();
202	printf("Test matrix 0 - should be unit\n");
203	test0.Print();
204	printf("Test matrix 1 - should be diagonal\n");
205	test1.Print();
206	printf("Diagonal matrix\n");
207	svd.GetSig().Print();
208	printf("Original param matrix\n");
209	mparam.Print();
210	printf("Rotated param matrix\n");
211	mparamT.Print();
212	//
213	}
214	//
215	for (Int_t idim = 0; idim<3; idim++){
216	Double_t yf; // input points to fit in TLinear fitter
217	Double_t xf[6]; // input points to fit
218	yf = deltaT(idim,0);
219	for (Int_t ipar =0; ipar<6; ipar++) xf[ipar] = mparamT(idim,ipar);
220	if (covDiagonal[idim]>0.){
221	fFitter->AddPoint(xf,yf, TMath::Sqrt(1/covDiagonal[idim]));
222	}
223	// accumulate chi2
224	fChi2Orig += (yfyf)covDiagonal[idim];
225	}
226	fNdf +=3;
227	}
228
229	//______________________________________________________________________________
230	Bool_t AliTrackResidualsLinear::Update()
231	{
232	// Find the alignment parameters
233	// using TLinear fitter + fill data containers
234	//
235	//
236	fFitter->Eval();
237	//
238	// TLinear fitter put as first parameter offset - fixing parameter shifted by one
239	//
240	fFitter->FixParameter(0);
241	for (Int_t ipar =0; ipar<6; ipar++){
242	if (fBFixed[ipar]) fFitter->FixParameter(ipar+1,fFixed[ipar]);
243	}
244	if (fFraction>0.5) {
245	fFitter->EvalRobust(fFraction);
246	}else{
247	fFitter->Eval();
248	}
249	//
250	fFitter->ReleaseParameter(0);
251	for (Int_t ipar=0; ipar<7; ipar++) {
252	if (fBFixed[ipar]) fFitter->ReleaseParameter(ipar+1);
253	}
254
255
256	//
257	fChi2 = fFitter->GetChisquare();
258	fNdf -= 6;
259	TVectorD vector(7);
260	fFitter->GetParameters(vector);
261	fParams[0] = vector[1];
262	fParams[1] = vector[2];
263	fParams[2] = vector[3];
264	fParams[3] = vector[4];
265	fParams[4] = vector[5];
266	fParams[5] = vector[6];
267	//
268	fAlignObj->SetPars(fParams[0], fParams[1], fParams[2],
269	TMath::RadToDeg()*fParams[3],
270	TMath::RadToDeg()*fParams[4],
271	TMath::RadToDeg()*fParams[5]);
272	return kTRUE;
273	}