[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()
{
  // Default constructor
  fFitter = 0;
  fFraction =-1;
  fChi2Orig =0;
  for (Int_t ipar=0; ipar<6; ipar++){
    fBFixed[ipar] = kFALSE;
    fFixed[ipar]  = 0;;
    fParams[ipar]  = 0;
  }  
}

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

//______________________________________________________________________________
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	//______________________________________________________________________________
	44	AliTrackResidualsLinear::AliTrackResidualsLinear():AliTrackResiduals()
	45	{
	46	// Default constructor
	47	fFitter = 0;
	48	fFraction =-1;
	49	fChi2Orig =0;
	50	for (Int_t ipar=0; ipar<6; ipar++){
	51	fBFixed[ipar] = kFALSE;
	52	fFixed[ipar] = 0;;
	53	fParams[ipar] = 0;
	54	}
	55	}
	56
	57	//______________________________________________________________________________
	58	AliTrackResidualsLinear::AliTrackResidualsLinear(Int_t ntracks):
	59	AliTrackResiduals(ntracks)
	60	{
	61	// Constructor
	62	fFitter = new TLinearFitter(6,"hyp6");
	63	fFraction =-1;
	64	fChi2Orig =0;
65	for (Int_t ipar=0; ipar<6; ipar++){
66	fBFixed[ipar] = kFALSE;
67	fFixed[ipar] = 0;
68	fParams[ipar] = 0;
69	}
70	}
71
72	//______________________________________________________________________________
73	AliTrackResidualsLinear::AliTrackResidualsLinear(const AliTrackResidualsLinear &res):
74	AliTrackResiduals(res)
75	{
76	// Copy constructor
77	//..
78	fFitter = new TLinearFitter(*(res.fFitter));
79	for (Int_t ipar=0; ipar<6; ipar++){
80	fBFixed[ipar] = res.fBFixed[ipar];
81	fFixed[ipar] = res.fFixed[ipar];
82	fParams[ipar] = res.fParams[ipar];
83	}
84	fChi2Orig = res.fChi2Orig;
85	}
86
87	//______________________________________________________________________________
88	AliTrackResidualsLinear &AliTrackResidualsLinear::operator= (const AliTrackResidualsLinear& res)
89	{
90	// Assignment operator
91	((AliTrackResiduals *)this)->operator=(res);
92	return *this;
93	}
94	//______________________________________________________________________________
95	AliTrackResidualsLinear::~AliTrackResidualsLinear()
96	{
97	//
98	//
99	//
100	delete fFitter;
101	}
102
103
104	//______________________________________________________________________________
105	Bool_t AliTrackResidualsLinear::Minimize()
106	{
107	// Implementation of fast linear Chi2 minimizer
108	// based on TLinear fitter
109	//
110	if (!fFitter) fFitter = new TLinearFitter(6,"hyp6");
111	fFitter->StoreData(kTRUE);
112	fFitter->ClearPoints();
113	fChi2Orig = 0;
114	AliTrackPoint p1,p2;
115	for (Int_t itrack = 0; itrack < fLast; itrack++) {
116	if (!fVolArray[itrack] \|\| !fTrackArray[itrack]) continue;
117	for (Int_t ipoint = 0; ipoint < fVolArray[itrack]->GetNPoints(); ipoint++) {
118	fVolArray[itrack]->GetPoint(p1,ipoint);
119	fTrackArray[itrack]->GetPoint(p2,ipoint);
120	AddPoints(p1,p2);
121	}
122	}
123	Bool_t isOK = Update();
124	if (!isOK) return isOK;
125	//
126	TGeoHMatrix matrix;
127	fAlignObj->GetMatrix(matrix);
128	return isOK;
129	}
130
131
132
133	//______________________________________________________________________________
134	void AliTrackResidualsLinear::AddPoints(AliTrackPoint &p, AliTrackPoint &pprime)
135	{
136	//
137	// add points to linear fitter - option with correlation betwee measurement in different dimensions
138	// p1 - local point
139	// pprime - track extrapolation point
140	//
141	Float_t xyz[3],xyzp[3];
142	Float_t cov[6],covp[6];
143	p.GetXYZ(xyz,cov); pprime.GetXYZ(xyzp,covp);
144	//
145	//
146	TMatrixD mcov(3,3); // local point covariance
147	mcov(0,0) = cov[0]; mcov(0,1) = cov[1]; mcov(0,2) = cov[2];
148	mcov(1,0) = cov[1]; mcov(1,1) = cov[3]; mcov(1,2) = cov[4];
149	mcov(2,0) = cov[2]; mcov(2,1) = cov[4]; mcov(2,2) = cov[5];
150	TMatrixD mcovp(3,3); // extrapolation point covariance
151	mcovp(0,0) = covp[0]; mcovp(0,1) = covp[1]; mcovp(0,2) = covp[2];
152	mcovp(1,0) = covp[1]; mcovp(1,1) = covp[3]; mcovp(1,2) = covp[4];
153	mcovp(2,0) = covp[2]; mcovp(2,1) = covp[4]; mcovp(2,2) = covp[5];
154	mcov+=mcovp;
155	mcov.Invert();
156	if (!mcov.IsValid()) return;
157	TMatrixD mcovBack = mcov; // for debug purposes
158	//
159	// decompose matrix
160	//
161	TDecompSVD svd(mcov); // mcov = svd.fU * covDiagonal * svd.fV.Invert
162	if (!svd.Decompose()) return; // decomposition failed
163	TMatrixD matrixV = svd.GetV(); // transformation matrix to diagonalize covariance matrix
164	Double_t covDiagonal[3] = {svd.GetSig()[0],svd.GetSig()[1],svd.GetSig()[2]}; // diagonalized covariance matrix
165	//
166	// residual vector
167	TMatrixD deltaR(3,1);
168	deltaR(0,0) = (xyzp[0]-xyz[0]);
169	deltaR(1,0) = (xyzp[1]-xyz[1]);
170	deltaR(2,0) = (xyzp[2]-xyz[2]);
171	//
172	// parametrization matrix
173	//
174	TMatrixD mparam(3,6);
175	mparam(0,0) = 1; mparam(1,0) = 0; mparam(2,0) = 0; // xshift
176	mparam(0,1) = 0; mparam(1,1) = 1; mparam(2,1) = 0; // yshift
177	mparam(0,2) = 0; mparam(1,2) = 0; mparam(2,2) = 1; // zshift
178	mparam(0,3) = 0; mparam(1,3) =-xyz[2]; mparam(2,3) = xyz[1]; // x rotation
179	mparam(0,4) = xyz[2]; mparam(1,4) = 0; mparam(2,4) =-xyz[0]; // y rotation
180	mparam(0,5) =-xyz[1]; mparam(1,5) = xyz[0]; mparam(2,5) = 0; // z rotation
181	//
182
183	TMatrixD deltaT(matrixV, TMatrixD::kTransposeMult, deltaR); // tranformed delta
184	TMatrixD mparamT(matrixV,TMatrixD::kTransposeMult, mparam); // tranformed linear transformation
185	if (0){
186	//
187	// debug part
188	//
189	// covDiag = U^-1 * mcov * V -- diagonalization of covariance matrix
190
191	TMatrixD matrixU = svd.GetU(); // transformation matrix to diagonalize covariance matrix
192	TMatrixD matrixUI= svd.GetU();
193	matrixUI.Invert();
194	//
195	TMatrixD test0 = matrixUI*matrixV; // test matrix - should be unit matrix
196	TMatrixD test1 = matrixUImcovBackmatrixV; // test matrix - diagonal - should be diagonal with covDiagonal on diag
197	TMatrixD test2 = matrixU.T()*matrixV; // test ortogonality - shoul be unit
198	printf("Test matrix 2 - should be unit\n");
199	test2.Print();
200	printf("Test matrix 0 - should be unit\n");
201	test0.Print();
202	printf("Test matrix 1 - should be diagonal\n");
203	test1.Print();
204	printf("Diagonal matrix\n");
205	svd.GetSig().Print();
206	printf("Original param matrix\n");
207	mparam.Print();
208	printf("Rotated param matrix\n");
209	mparamT.Print();
210	//
211	}
212	//
213	for (Int_t idim = 0; idim<3; idim++){
214	Double_t yf; // input points to fit in TLinear fitter
215	Double_t xf[6]; // input points to fit
216	yf = deltaT(idim,0);
217	for (Int_t ipar =0; ipar<6; ipar++) xf[ipar] = mparamT(idim,ipar);
218	if (covDiagonal[idim]>0.){
219	fFitter->AddPoint(xf,yf, TMath::Sqrt(1/covDiagonal[idim]));
220	}
221	// accumulate chi2
222	fChi2Orig += (yfyf)covDiagonal[idim];
223	}
224	fNdf +=3;
225	}
226
227	//______________________________________________________________________________
228	Bool_t AliTrackResidualsLinear::Update()
229	{
230	// Find the alignment parameters
231	// using TLinear fitter + fill data containers
232	//
233	//
234	fFitter->Eval();
235	//
236	// TLinear fitter put as first parameter offset - fixing parameter shifted by one
237	//
238	fFitter->FixParameter(0);
239	for (Int_t ipar =0; ipar<6; ipar++){
240	if (fBFixed[ipar]) fFitter->FixParameter(ipar+1,fFixed[ipar]);
241	}
242	if (fFraction>0.5) {
243	fFitter->EvalRobust(fFraction);
244	}else{
245	fFitter->Eval();
246	}
247	//
248	fFitter->ReleaseParameter(0);
249	for (Int_t ipar=0; ipar<7; ipar++) {
250	if (fBFixed[ipar]) fFitter->ReleaseParameter(ipar+1);
251	}
252
253
254	//
255	fChi2 = fFitter->GetChisquare();
256	fNdf -= 6;
257	TVectorD vector(7);
258	fFitter->GetParameters(vector);
259	fParams[0] = vector[1];
260	fParams[1] = vector[2];
261	fParams[2] = vector[3];
262	fParams[3] = vector[4];
263	fParams[4] = vector[5];
264	fParams[5] = vector[6];
265	//
266	fAlignObj->SetPars(fParams[0], fParams[1], fParams[2],
267	TMath::RadToDeg()*fParams[3],
268	TMath::RadToDeg()*fParams[4],
269	TMath::RadToDeg()*fParams[5]);
270	return kTRUE;
271	}