assert replacet with AliError (Laurent)
[u/mrichter/AliRoot.git] / STEER / AliTrackFitterStraight.cxx
CommitLineData
cc345ce3 1#include "TMatrixDSym.h"
2#include "TMatrixD.h"
3#include "AliTrackFitterStraight.h"
4
5ClassImp(AliTrackFitterStraight)
6
7AliTrackFitterStraight::AliTrackFitterStraight():
75e3794b 8 AliTrackFitter(),
9 fAlpha(0.),
10 fSumYY(0),
11 fSumZZ(0),
12 fNUsed(0),
13 fConv(kFALSE)
cc345ce3 14{
15 //
16 // default constructor
17 //
cc345ce3 18 for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
cc345ce3 19 for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
cc345ce3 20}
21
22
23AliTrackFitterStraight::AliTrackFitterStraight(AliTrackPointArray *array, Bool_t owner):
75e3794b 24 AliTrackFitter(array,owner),
25 fAlpha(0.),
26 fSumYY(0),
27 fSumZZ(0),
28 fNUsed(0),
29 fConv(kFALSE)
cc345ce3 30{
31 //
32 // Constructor
33 //
cc345ce3 34 for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
cc345ce3 35 for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
cc345ce3 36}
37
38AliTrackFitterStraight::AliTrackFitterStraight(const AliTrackFitterStraight &fitter):
75e3794b 39 AliTrackFitter(fitter),
40 fAlpha(fitter.fAlpha),
41 fSumYY(fitter.fSumYY),
42 fSumZZ(fitter.fSumZZ),
43 fNUsed(fitter.fNUsed),
44 fConv(fitter.fConv)
cc345ce3 45{
46 //
47 // copy constructor
48 //
cc345ce3 49 for (Int_t i=0;i<5;i++) fSumXY[i] = fitter.fSumXY[i];
cc345ce3 50 for (Int_t i=0;i<5;i++) fSumXZ[i] = fitter.fSumXZ[i];
cc345ce3 51}
52
53//_____________________________________________________________________________
54AliTrackFitterStraight &AliTrackFitterStraight::operator =(const AliTrackFitterStraight& fitter)
55{
56 // assignment operator
57 //
58 if(this==&fitter) return *this;
59 ((AliTrackFitter *)this)->operator=(fitter);
60
61 fAlpha = fitter.fAlpha;
62 for (Int_t i=0;i<5;i++) fSumXY[i] = fitter.fSumXY[i];
63 fSumYY = fitter.fSumYY;
64 for (Int_t i=0;i<5;i++) fSumXZ[i] = fitter.fSumXZ[i];
65 fSumZZ = fitter.fSumZZ;
66 fNUsed = fitter.fNUsed;
67 fConv = fitter.fConv;
68
69 return *this;
70}
71
72AliTrackFitterStraight::~AliTrackFitterStraight()
73{
74 // destructor
75 //
76}
77
78void AliTrackFitterStraight::Reset()
79{
80 // Reset the track parameters and
81 // sums
82 AliTrackFitter::Reset();
83 fAlpha = 0.;
84 for (Int_t i=0;i<5;i++) fSumXY[i] = 0;
85 fSumYY = 0;
86 for (Int_t i=0;i<5;i++) fSumXZ[i] = 0;
87 fSumZZ = 0;
88 fNUsed = 0;
89 fConv =kFALSE;
90}
91
92Bool_t AliTrackFitterStraight::Fit(const TArrayI *volIds,const TArrayI *volIdsFit,
93 AliAlignObj::ELayerID layerRangeMin,
94 AliAlignObj::ELayerID layerRangeMax)
95{
96 // Fit the track points. The method takes as an input
97 // the set of id's (volids) of the volumes in which
98 // one wants to calculate the residuals.
99 // The following parameters are used to define the
100 // range of volumes to be used in the fitting
101 // As a result two AliTrackPointArray's obects are filled.
102 // The first one contains the space points with
103 // volume id's from volids list. The second array of points represents
104 // the track extrapolations corresponding to the space points
105 // in the first array. The two arrays can be used to find
106 // the residuals in the volids and consequently construct a
107 // chi2 function to be minimized during the alignment
108 // procedures. For the moment the track extrapolation is taken
109 // at the space-point reference plane. The reference plane is
110 // found using the covariance matrix of the point
111 // (assuming sigma(x)=0 at the reference coordinate system.
112
113 Reset();
114
115 Int_t npoints = fPoints->GetNPoints();
116 if (npoints < 2) return kFALSE;
117
118 Bool_t isAlphaCalc = kFALSE;
119 AliTrackPoint p,plocal;
120
121 Int_t npVolId = 0;
122 fNUsed = 0;
123 Int_t *pindex = new Int_t[npoints];
124 for (Int_t ipoint = 0; ipoint < npoints; ipoint++)
125 {
126 fPoints->GetPoint(p,ipoint);
127 UShort_t iVolId = p.GetVolumeID();
128 if (FindVolId(volIds,iVolId)) {
129 pindex[npVolId] = ipoint;
130 npVolId++;
131 }
132 if (volIdsFit != 0x0) {
133 if (!FindVolId(volIdsFit,iVolId)) continue;
134 }
135 else {
136 if (iVolId < AliAlignObj::LayerToVolUID(layerRangeMin,0) ||
137 iVolId > AliAlignObj::LayerToVolUID(layerRangeMax,
c041444f 138 AliAlignObj::LayerSize(layerRangeMax))) continue;
cc345ce3 139 }
140 if (!isAlphaCalc) {
141 fAlpha = p.GetAngle();
142 isAlphaCalc = kTRUE;
143 }
144 plocal = p.Rotate(fAlpha);
145 AddPoint(plocal.GetX(),plocal.GetY(),plocal.GetZ(),
146 TMath::Sqrt(plocal.GetCov()[3]),TMath::Sqrt(plocal.GetCov()[5]));
147 fNUsed++;
148 }
149
150 if (fNUsed < 2) {
151 delete [] pindex;
152 return kFALSE;
153 }
154
155 Update();
156
157 if (!fConv) {
158 delete [] pindex;
159 return kFALSE;
160 }
161
162 if (fNUsed < fMinNPoints ) {
163 delete [] pindex;
164 return kFALSE;
165 }
166
167 fPVolId = new AliTrackPointArray(npVolId);
168 fPTrack = new AliTrackPointArray(npVolId);
169 AliTrackPoint p2;
170 for (Int_t ipoint = 0; ipoint < npVolId; ipoint++)
171 {
172 Int_t index = pindex[ipoint];
173 fPoints->GetPoint(p,index);
174 if (GetPCA(p,p2)) {
175 Float_t xyz[3],xyz2[3];
176 p.GetXYZ(xyz); p2.GetXYZ(xyz2);
177 // printf("residuals %f %d %d %f %f %f %f %f %f\n",fChi2,fNUsed,fConv,xyz[0],xyz[1],xyz[2],xyz2[0]-xyz[0],xyz2[1]-xyz[1],xyz2[2]-xyz[2]);
178 fPVolId->AddPoint(ipoint,&p);
179 fPTrack->AddPoint(ipoint,&p2);
180 }
181 }
182
183 delete [] pindex;
184
185 return kTRUE;
186}
187
188void AliTrackFitterStraight::AddPoint(Float_t x, Float_t y, Float_t z, Float_t sy, Float_t sz)
189{
190 // Straight track fitter
191 // The method add a point to the sums
192 // used to extract track parameters
193
194 //
195 // XY part
196 //
197 Double_t weight = 1./(sy*sy);
198 fSumXY[0] +=weight;
199 fSumXY[1] +=x*weight; fSumXY[2] +=x*x*weight;
200 fSumXY[3] +=y*weight; fSumXY[4] +=x*y*weight;
201 fSumYY += y*y*weight;
202 //
203 // XZ part
204 //
205 weight = 1./(sz*sz);
206 fSumXZ[0] +=weight;
207 fSumXZ[1] +=x*weight; fSumXZ[2] +=x*x*weight;
208 fSumXZ[3] +=z*weight; fSumXZ[4] +=x*z*weight;
209 fSumZZ += z*z*weight;
210}
211
212void AliTrackFitterStraight::Update(){
213 //
214 // Track fitter update
215 //
216 //
217 for (Int_t i=0;i<6;i++)fParams[i]=0;
218 fChi2 = 0;
219 fNdf = 0;
220 Int_t conv=0;
221 //
222 // XY part
223 //
224 TMatrixDSym smatrix(2);
225 TMatrixD sums(1,2);
226 smatrix(0,0) = fSumXY[0]; smatrix(1,1)=fSumXY[2];
3bbfb291 227 smatrix(0,1) = fSumXY[1]; smatrix(1,0)=fSumXY[1];
cc345ce3 228 sums(0,0) = fSumXY[3]; sums(0,1) =fSumXY[4];
229 smatrix.Invert();
230 if (smatrix.IsValid()){
231 for (Int_t i=0;i<2;i++)
232 for (Int_t j=0;j<=i;j++){
233 (*fCov)(i,j)=smatrix(i,j);
234 }
235 TMatrixD res = sums*smatrix;
236 fParams[0] = res(0,0);
237 fParams[1] = res(0,1);
238 TMatrixD tmp = res*sums.T();
239 fChi2 += fSumYY - tmp(0,0);
240 fNdf += fNUsed - 2;
241 conv++;
242 }
243 //
244 // XZ part
245 //
246 TMatrixDSym smatrixz(2);
247 TMatrixD sumsxz(1,2);
248 smatrixz(0,0) = fSumXZ[0]; smatrixz(1,1) = fSumXZ[2];
3bbfb291 249 smatrixz(0,1) = fSumXZ[1]; smatrixz(1,0) = fSumXZ[1];
cc345ce3 250 sumsxz(0,0) = fSumXZ[3]; sumsxz(0,1) = fSumXZ[4];
251 smatrixz.Invert();
252 if (smatrixz.IsValid()){
253 TMatrixD res = sumsxz*smatrixz;
254 fParams[2] = res(0,0);
255 fParams[3] = res(0,1);
256 fParams[4] = fParams[5] = 0;
257 for (Int_t i=0;i<2;i++)
258 for (Int_t j=0;j<=i;j++){
259 (*fCov)(i+2,j+2)=smatrixz(i,j);
260 }
261 TMatrixD tmp = res*sumsxz.T();
262 fChi2 += fSumZZ - tmp(0,0);
263 fNdf += fNUsed - 2;
264 conv++;
265 }
266
267 if (conv>1)
268 fConv =kTRUE;
269 else
270 fConv=kFALSE;
271}
272
273Double_t AliTrackFitterStraight::GetYat(Double_t x) const {
274 if (!fConv) return 0.;
275 return (fParams[0]+x*fParams[1]);
276}
277
278Double_t AliTrackFitterStraight::GetDYat(Double_t x) const {
279 if (!fConv) return 0.;
280 return fParams[1]+0.*x;
281}
282
283
284
285Double_t AliTrackFitterStraight::GetZat(Double_t x) const {
286 if (!fConv) return 0.;
287 return (fParams[2]+x*fParams[3]);
288}
289
290Double_t AliTrackFitterStraight::GetDZat(Double_t x) const {
291 if (!fConv) return 0.;
292 return fParams[3]+0.*x;
293}
294
295Bool_t AliTrackFitterStraight::GetXYZat(Double_t r, Float_t *xyz) const {
296 if (!fConv) return kFALSE;
297 Double_t y = (fParams[0]+r*fParams[1]);
298 Double_t z = (fParams[2]+r*fParams[3]);
299
300 Double_t sin = TMath::Sin(fAlpha);
301 Double_t cos = TMath::Cos(fAlpha);
302 xyz[0] = r*cos - y*sin;
303 xyz[1] = y*cos + r*sin;
304 xyz[2] = z;
305
306 return kTRUE;
307}
308
309Bool_t AliTrackFitterStraight::GetPCA(const AliTrackPoint &p, AliTrackPoint &p2) const
310{
311 // Get the closest to a given spacepoint track trajectory point
312 // Look for details in the description of the Fit() method
313
314 if (!fConv) return kFALSE;
315
316 // First X and Y coordinates
317 Double_t sin = TMath::Sin(fAlpha);
318 Double_t cos = TMath::Cos(fAlpha);
319 // Track parameters in the global coordinate system
320 Double_t x0 = -fParams[0]*sin;
321 Double_t y0 = fParams[0]*cos;
322 if ((cos - fParams[1]*sin) == 0) return kFALSE;
323 Double_t dydx = (fParams[1]*cos + sin)/(cos - fParams[1]*sin);
324
325 // Define space-point refence plane
326 Double_t alphap = p.GetAngle();
327 Double_t sinp = TMath::Sin(alphap);
328 Double_t cosp = TMath::Cos(alphap);
329 Double_t x = p.GetX()*cosp + p.GetY()*sinp;
330 // Double_t y = p.GetY()*cosp - p.GetX()*sinp;
331 Double_t x0p= x0*cosp + y0*sinp;
332 Double_t y0p= y0*cosp - x0*sinp;
333 if ((cos + dydx*sin) == 0) return kFALSE;
334 Double_t dydxp = (dydx*cos - sin)/(cos + dydx*sin);
335 Double_t yprime = y0p + dydxp*(x-x0p);
336
337 // Back to the global coordinate system
338 Double_t xsecond = x*cosp - yprime*sinp;
339 Double_t ysecond = yprime*cosp + x*sinp;
340
341 // Now Z coordinate and track angles
342 Double_t x2 = xsecond*cos + ysecond*sin;
343 Double_t zsecond = GetZat(x2);
344 Double_t dydx2 = fParams[1];
345 Double_t dzdx = fParams[3];
346
347 // Fill the cov matrix of the track extrapolation point
348 Double_t cov[6] = {0,0,0,0,0,0};
349 Double_t sigmax = 100*100.;
350 cov[0] = sigmax; cov[1] = sigmax*dydx2; cov[2] = sigmax*dzdx;
351 cov[3] = sigmax*dydx2*dydx2; cov[4] = sigmax*dydx2*dzdx;
352 cov[5] = sigmax*dzdx*dzdx;
353
354 Float_t newcov[6];
355 newcov[0] = cov[0]*cos*cos-
356 2*cov[1]*sin*cos+
357 cov[3]*sin*sin;
358 newcov[1] = cov[1]*(cos*cos-sin*sin)-
359 (cov[3]-cov[0])*sin*cos;
360 newcov[2] = cov[2]*cos-
361 cov[4]*sin;
362 newcov[3] = cov[0]*sin*sin+
363 2*cov[1]*sin*cos+
364 cov[3]*cos*cos;
365 newcov[4] = cov[4]*cos+
366 cov[2]*sin;
367 newcov[5] = cov[5];
368
369 p2.SetXYZ(xsecond,ysecond,zsecond,newcov);
370
371 return kTRUE;
372}