]>
Commit | Line | Data |
---|---|---|
043badeb | 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 | // | |
18 | // Kalman filter based aligner: | |
19 | // Aligns two tracking volumes (by default ITS and TPC) | |
20 | // Determines the transformation of the second volume (TPC) wrt the first (ITS) | |
21 | // additionally calculates some callibration parameters for TPC | |
22 | // Fit parameters are: | |
23 | // - 3 shifts, x,y,z | |
24 | // - 3 Cardan angles, psi, theta, phi (see definition in alignment docs), | |
25 | // - TPC drift velocity correction (vel_true = vel*(1+correction)), | |
26 | // - TPC offset correction. | |
27 | // | |
28 | // Basic usage: | |
29 | // When aligning two volumes at any given time a single instance of | |
30 | // the class should be active. The fit of the parameters is updated | |
31 | // by adding new data using one of the Add.... methods. | |
32 | // | |
33 | // User methods: | |
34 | // In collision events add an ESD track to update the fit, | |
35 | // track will be automatically split in ITS and TPC tracklets | |
36 | // and the fit of the alignment constants will be updated: | |
37 | // | |
38 | // Bool_t AddESDTrack( AliESDtrack* pTrack ); | |
39 | // | |
40 | // You may also give it the AliTrackPointArray, | |
41 | // the fit will be updated if there are enough ITS and TPC | |
42 | // points in it. Again, all is done automatically: | |
43 | // | |
44 | // Bool_t AddTrackPointArray( AliTrackPointArray* pTrackPointArr ); | |
45 | // | |
46 | // For cosmic data, the assumption is that the tracking is done separately | |
47 | // for ITS and TPC and the tracklets are saved inside one AliESDEvent. The | |
48 | // method | |
49 | // | |
50 | // Bool_t AddCosmicEventSeparateTracking( AliESDEvent* pEvent ); | |
51 | // | |
52 | // then searches the event for matching tracklets and upon succes it updates | |
53 | // the fit. For cosmics the fitter switches to a 3 tracks mode, the 3 tracks | |
54 | // being: 1 ITS and 2 TPC tracklets (upper and lower). | |
55 | // | |
56 | // Experts only: | |
57 | // | |
58 | // | |
59 | // Origin: Mikolaj Krzewicki, Nikhef, Mikolaj.Krzewicki@cern.ch | |
60 | // | |
61 | ////////////////////////////////////////////////////////////////////////////// | |
62 | ||
248e9dfd | 63 | #include <Riostream.h> |
64 | ||
043badeb | 65 | #include "AliRelAlignerKalman.h" |
66 | ||
67 | ClassImp(AliRelAlignerKalman) | |
68 | ||
69 | //######################################################## | |
70 | // | |
71 | // Control section | |
72 | // | |
73 | //######################################################## | |
74 | ||
75 | AliRelAlignerKalman::AliRelAlignerKalman(): | |
76 | fPTrackPointArray1(new AliTrackPointArray(1)), | |
77 | fPTrackPointArray2(new AliTrackPointArray(1)), | |
78 | fPTrackPointArray2b(new AliTrackPointArray(1)), | |
79 | fPVolIDArray1(new TArrayI(1)), | |
80 | fPVolIDArray2(new TArrayI(1)), | |
81 | fPVolIDArray2b(new TArrayI(1)), | |
82 | fQ(1e-10), | |
83 | fNMeasurementParams(fgkNMeasurementParams2TrackMode), | |
84 | fNSystemParams(fgkNSystemParams), | |
85 | fOutRejSigmas(2.), | |
86 | f3TracksMode(kFALSE), | |
87 | fSortTrackPointsWithY(kTRUE), | |
88 | fFixedMeasurementCovariance(kTRUE), | |
89 | fCalibrationPass(kFALSE), | |
90 | fCalibrationUpdated(kFALSE), | |
91 | fFitted(kFALSE), | |
92 | fCuts(kFALSE), | |
93 | fNTracks(0), | |
94 | fNUpdates(0), | |
95 | fNOutliers(0), | |
96 | fNUnfitted(0), | |
97 | fNMatchedCosmics(0), | |
98 | fMinPointsVol1(2), | |
99 | fMinPointsVol2(10), | |
100 | fMinMom(0.), | |
101 | fMaxMom(1e100), | |
102 | fMinAbsSinPhi(0.), | |
103 | fMaxAbsSinPhi(1.), | |
104 | fMinSinTheta(-1.), | |
105 | fMaxSinTheta(1.), | |
106 | fMaxMatchingAngle(0.1), | |
107 | fMaxMatchingDistance(1.), //in cm | |
108 | fFirstLayerVol1(1), | |
109 | fLastLayerVol1(6), | |
110 | fFirstLayerVol2(7), | |
111 | fLastLayerVol2(8) | |
112 | { | |
113 | //Default constructor | |
114 | ||
115 | fPDir1 = new TVector3; //Direction | |
116 | fPDir2 = new TVector3; | |
117 | fPDir2b = new TVector3; | |
118 | fPDir1Cov = new TMatrixDSym(3); | |
119 | fPDir2Cov = new TMatrixDSym(3); | |
120 | fPDir2bCov = new TMatrixDSym(3); | |
121 | fPDir1ThetaPhiCov = new TMatrixDSym(2); //Direction vectors are unit vectors, therefore 2 independent components! | |
122 | fPDir2ThetaPhiCov = new TMatrixDSym(2); | |
123 | fPDir2bThetaPhiCov = new TMatrixDSym(2); | |
124 | fPPoint1 = new TVector3; | |
125 | fPPoint2 = new TVector3; | |
126 | fPPoint2b = new TVector3; | |
127 | fPPoint1Cov = new TMatrixDSym(3); | |
128 | fPPoint2Cov = new TMatrixDSym(3); | |
129 | fPPoint2bCov = new TMatrixDSym(3); | |
130 | fPRefPoint = new TVector3(0.,0.,0.); | |
131 | fPRefPointDir = new TVector3(0.,1.,0.); | |
132 | Float_t refpointcov[6] = { 1., 0., 0., | |
133 | 0., 0., | |
134 | 1. }; | |
135 | fPRefAliTrackPoint = new AliTrackPoint( 0.,0.,0.,refpointcov,0 ); | |
136 | fPMeasurement = new TVectorD( fNMeasurementParams ); | |
137 | fPMeasurementCov = new TMatrixDSym( fNMeasurementParams ); | |
138 | fPX = new TVectorD( fNSystemParams ); | |
139 | fPXcov = new TMatrixDSym( fNSystemParams ); | |
140 | fPH = new TMatrixD( fNMeasurementParams, fNSystemParams ); | |
141 | fPRotMat = new TMatrixD(3,3); | |
142 | fPVec010 = new TVector3(0,1,0); | |
143 | ||
144 | //default seed: zero, unit(large) errors | |
145 | fPXcov->UnitMatrix(); | |
146 | (*fPXcov) *= 1.; | |
147 | ||
148 | fPPhiMesHist = new TH1D("phi","phi", 50, 0, 0); | |
149 | fPThetaMesHist = new TH1D("theta","theta", 50, 0, 0); | |
150 | fPXMesHist = new TH1D("x","x", 50, 0, 0); | |
151 | fPZMesHist = new TH1D("z","z", 50, 0, 0); | |
152 | fPPhiMesHist2 = new TH1D("phi_","phi_", 50, 0, 0); | |
153 | fPThetaMesHist2 = new TH1D("theta_","theta_", 50, 0, 0); | |
154 | fPXMesHist2 = new TH1D("x_","x_", 50, 0, 0); | |
155 | fPZMesHist2 = new TH1D("z_","z_", 50, 0, 0); | |
156 | } | |
157 | ||
158 | Bool_t AliRelAlignerKalman::AddESDTrack( AliESDtrack* pTrack ) | |
159 | { | |
160 | //Add an ESD track from a central event | |
161 | //from the ESDtrack extract the pointarray | |
162 | ||
163 | SetCosmicEvent(kFALSE); | |
164 | ||
165 | const AliTrackPointArray *pTrackPointArrayConst = pTrack->GetTrackPointArray(); | |
166 | AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayConst); | |
167 | if (pTrackPointArray == 0) return kFALSE; | |
168 | ||
169 | if (!AddTrackPointArray( pTrackPointArray )) return kFALSE; | |
170 | return kTRUE; | |
171 | } | |
172 | ||
173 | Bool_t AliRelAlignerKalman::AddTrackPointArray( AliTrackPointArray* pTrackPointArray ) | |
174 | { | |
175 | //Add a trackpointarray from a central event | |
176 | ||
177 | SetCosmicEvent(kFALSE); | |
178 | fNTracks++; | |
179 | ||
180 | if (!ExtractSubTrackPointArray( fPTrackPointArray1, fPVolIDArray1, pTrackPointArray, 1,6 )) | |
181 | return kFALSE; | |
182 | if (!ExtractSubTrackPointArray( fPTrackPointArray2, fPVolIDArray2, pTrackPointArray, 7,8 )) | |
183 | return kFALSE; | |
184 | ||
185 | if (!ProcessTrackPointArrays()) return kFALSE; | |
186 | if (!PrepareUpdate()) return kFALSE; | |
187 | if (!Update()) return kFALSE; | |
188 | ||
189 | return kTRUE; | |
190 | } | |
191 | ||
192 | Bool_t AliRelAlignerKalman::AddCosmicEventSeparateTracking( AliESDEvent* pEvent ) | |
193 | { | |
194 | //Add an cosmic with separately tracked ITS and TPC parts, do trackmatching | |
195 | ||
196 | SetCosmicEvent(kTRUE); //set the cosmic flag | |
197 | fNTracks++; | |
198 | ||
199 | if (!FindCosmicInEvent( pEvent )) return kFALSE; | |
200 | if (!ProcessTrackPointArrays()) return kFALSE; | |
201 | //PrintDebugInfo(); | |
202 | if (!PrepareUpdate()) return kFALSE; | |
203 | if (!Update()) return kFALSE; | |
204 | printf("fpMeasurementCov:\n"); | |
205 | fPMeasurementCov->Print(); | |
206 | ||
207 | return kTRUE; | |
208 | } | |
209 | ||
210 | void AliRelAlignerKalman::Print() | |
211 | { | |
212 | //Print some useful info | |
213 | printf("\nAliRelAlignerKalman:\n"); | |
214 | printf(" %i tracks, %i updates, %i outliers, ", fNTracks, fNUpdates, fNOutliers ); | |
215 | printf("%i not fitted, %i matched cosmic tracklets\n", fNUnfitted, fNMatchedCosmics ); | |
216 | printf(" psi(x): %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(0),1e3*TMath::Sqrt((*fPXcov)(0,0))); | |
217 | printf(" theta(y): %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(1),1e3*TMath::Sqrt((*fPXcov)(1,1))); | |
218 | printf(" phi(z): %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(2),1e3*TMath::Sqrt((*fPXcov)(2,2))); | |
219 | printf(" x: %.1f ± (%.1f) micron\n", 1e4*(*fPX)(3),1e4*TMath::Sqrt((*fPXcov)(3,3))); | |
220 | printf(" y: %.1f ± (%.1f) micron\n", 1e4*(*fPX)(4),1e4*TMath::Sqrt((*fPXcov)(4,4))); | |
221 | printf(" z: %.1f ± (%.1f) micron\n", 1e4*(*fPX)(5),1e4*TMath::Sqrt((*fPXcov)(5,5))); | |
222 | printf(" TPC(drift corr) %.1f ± (%.1f) percent\n", 1e2*(*fPX)(6),1e2*TMath::Sqrt((*fPXcov)(6,6))); | |
223 | printf(" TPC(offset) %.1f ± (%.1f) micron\n", 1e4*(*fPX)(7),1e4*TMath::Sqrt((*fPXcov)(7,7))); | |
224 | return; | |
225 | } | |
226 | ||
227 | void AliRelAlignerKalman::SetTrackParams1( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov ) | |
228 | { | |
229 | //Set the trackparameters for track in the first volume at reference | |
230 | *fPPoint1 = point; | |
231 | *fPPoint1Cov = pointcov; | |
232 | *fPDir1 = dir; | |
233 | *fPDir1Cov = dircov; | |
234 | } | |
235 | ||
236 | void AliRelAlignerKalman::SetTrackParams2( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov ) | |
237 | { | |
238 | //Set the trackparameters for track in the second volume at reference | |
239 | *fPPoint2 = point; | |
240 | *fPPoint2Cov = pointcov; | |
241 | *fPDir2 = dir; | |
242 | *fPDir2Cov = dircov; | |
243 | } | |
244 | ||
245 | void AliRelAlignerKalman::SetTrackParams2b( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov ) | |
246 | { | |
247 | //Set the trackparameters for second track in the second volume at reference | |
248 | *fPPoint2b = point; | |
249 | *fPPoint2bCov = pointcov; | |
250 | *fPDir2b = dir; | |
251 | *fPDir2bCov = dircov; | |
252 | } | |
253 | ||
254 | void AliRelAlignerKalman::SetRefSurface( const Double_t yref ) | |
255 | { | |
256 | //set the reference surface | |
257 | fPRefPoint->SetXYZ( 0., 1.*yref, 0. ); | |
258 | fPRefPointDir->SetXYZ( 0., 1., 0. ); | |
259 | Float_t refpointcov[6] = { 1., 0., 0., | |
260 | 0., 0., | |
261 | 1. }; | |
262 | fPRefAliTrackPoint->SetXYZ( 0., 1.*yref, 0., refpointcov ); | |
263 | } | |
264 | ||
265 | void AliRelAlignerKalman::SetRefSurface( const TVector3& point, const TVector3& dir ) | |
266 | { | |
267 | //set the reference surface | |
268 | *fPRefPoint = point; | |
269 | *fPRefPointDir = dir; | |
270 | } | |
271 | ||
272 | void AliRelAlignerKalman::SetRefSurface( const AliTrackPoint& point, const Bool_t horizontal ) | |
273 | { | |
274 | //set the reference surface | |
275 | (*fPRefAliTrackPoint) = point; | |
276 | ||
277 | (*fPRefPoint)(0) = point.GetX(); | |
278 | (*fPRefPoint)(1) = point.GetY(); | |
279 | (*fPRefPoint)(2) = point.GetZ(); | |
280 | ||
281 | if (horizontal) | |
282 | { | |
283 | (*fPRefPointDir)(0) = 0.; | |
284 | (*fPRefPointDir)(1) = 1.; | |
285 | (*fPRefPointDir)(2) = 0.; | |
286 | ||
287 | Float_t refpointcov[6] = { 1., 0., 0., | |
288 | 0., 0., | |
289 | 1. }; | |
290 | fPRefAliTrackPoint->SetXYZ( point.GetX(), point.GetY() , point.GetZ(), refpointcov ); | |
291 | ||
292 | } | |
293 | else | |
294 | { | |
295 | Double_t angle = point.GetAngle(); | |
296 | (*fPRefPointDir)(0) = TMath::Cos(angle); | |
297 | (*fPRefPointDir)(1) = TMath::Sin(angle); | |
298 | (*fPRefPointDir)(2) = 0.; | |
299 | *fPRefPointDir = fPRefPointDir->Unit(); | |
300 | } | |
301 | } | |
302 | ||
303 | void AliRelAlignerKalman::Set3TracksMode( Bool_t mode ) | |
304 | { | |
305 | //set the mode where 2 tracklets are allowed in volume 2 (default:TPC) | |
306 | //used for alignment with cosmics | |
307 | ||
308 | f3TracksMode = mode; | |
309 | if (mode) | |
310 | { | |
311 | if (fNMeasurementParams!=fgkNMeasurementParams3TrackMode) //do something only if necessary | |
312 | { | |
313 | fNMeasurementParams = fgkNMeasurementParams3TrackMode; | |
314 | delete fPMeasurement; | |
315 | delete fPMeasurementCov; | |
316 | delete fPH; | |
317 | fPMeasurement = new TVectorD( fNMeasurementParams ); | |
318 | fPMeasurementCov = new TMatrixDSym( fNMeasurementParams ); | |
319 | fPH = new TMatrixD( fNMeasurementParams, fNSystemParams ); | |
320 | } | |
321 | } | |
322 | else | |
323 | { | |
324 | if (fNMeasurementParams!=fgkNMeasurementParams2TrackMode) | |
325 | { | |
326 | fNMeasurementParams = fgkNMeasurementParams2TrackMode; | |
327 | delete fPMeasurement; | |
328 | delete fPMeasurementCov; | |
329 | delete fPH; | |
330 | fPMeasurement = new TVectorD( fNMeasurementParams ); | |
331 | fPMeasurementCov = new TMatrixDSym( fNMeasurementParams ); | |
332 | fPH = new TMatrixD( fNMeasurementParams, fNSystemParams ); | |
333 | } | |
334 | } | |
335 | } | |
336 | ||
337 | Bool_t AliRelAlignerKalman::PrepareUpdate() | |
338 | { | |
339 | //Cast the extrapolated data (points and directions) into | |
340 | //the internal Kalman filter data representation. | |
341 | //takes the 3d coordinates of the points of intersection with | |
342 | //the reference surface and projects them onto a 2D plane. | |
343 | //does the same for angles, combines the result in one vector | |
344 | ||
345 | if (!FillMeasurement()) return kFALSE; | |
346 | if (!FillMeasurementMatrix()) return kFALSE; | |
347 | return kTRUE; | |
348 | } | |
349 | ||
350 | Bool_t AliRelAlignerKalman::Update() | |
351 | { | |
352 | //perform the update - either kalman or calibration | |
353 | if (fCalibrationPass) return UpdateCalibration(); | |
354 | else return UpdateEstimateKalman(); | |
355 | } | |
356 | ||
357 | void AliRelAlignerKalman::RotMat( TMatrixD &R, const TVectorD& angles ) | |
358 | { | |
359 | //Get Rotation matrix R given the Cardan angles psi, theta, phi (around x, y, z). | |
360 | Double_t sinpsi = TMath::Sin(angles(0)); | |
361 | Double_t sintheta = TMath::Sin(angles(1)); | |
362 | Double_t sinphi = TMath::Sin(angles(2)); | |
363 | Double_t cospsi = TMath::Cos(angles(0)); | |
364 | Double_t costheta = TMath::Cos(angles(1)); | |
365 | Double_t cosphi = TMath::Cos(angles(2)); | |
366 | ||
367 | R(0,0) = costheta*cosphi; | |
368 | R(0,1) = -costheta*sinphi; | |
369 | R(0,2) = sintheta; | |
370 | R(1,0) = sinpsi*sintheta*cosphi + cospsi*sinphi; | |
371 | R(1,1) = -sinpsi*sintheta*sinphi + cospsi*cosphi; | |
372 | R(1,2) = -costheta*sinpsi; | |
373 | R(2,0) = -cospsi*sintheta*cosphi + sinpsi*sinphi; | |
374 | R(2,1) = cospsi*sintheta*sinphi + sinpsi*cosphi; | |
375 | R(2,2) = costheta*cospsi; | |
376 | return; | |
377 | } | |
378 | ||
379 | Bool_t AliRelAlignerKalman::FillMeasurement() | |
380 | { | |
381 | //Take the track parameters and calculate the input to the Kalman filter | |
382 | ||
383 | //Direction vectors | |
384 | //Measured is the difference between the polar angles - convert to that | |
385 | (*fPMeasurement)(0) = fPDir2->Theta() - fPDir1->Theta(); | |
386 | (*fPMeasurement)(1) = fPDir2->Phi() - fPDir1->Phi(); | |
387 | ||
388 | //Points | |
389 | //Measured is the distance in XZ plane at Y of the reference point | |
390 | (*fPMeasurement)(2) = fPPoint2->X() - fPPoint1->X(); | |
391 | (*fPMeasurement)(3) = fPPoint2->Z() - fPPoint1->Z(); | |
392 | ||
393 | //if 3 track mode set, set also the stuff for 2nd TPC track | |
394 | if (f3TracksMode) | |
395 | { | |
396 | (*fPMeasurement)(4) = fPDir2b->Theta() - fPDir1->Theta(); | |
397 | (*fPMeasurement)(5) = fPDir2b->Phi() - fPDir1->Phi(); | |
398 | (*fPMeasurement)(6) = fPPoint2b->X() - fPPoint1->X(); | |
399 | (*fPMeasurement)(7) = fPPoint2b->Z() - fPPoint1->Z(); | |
400 | } | |
401 | ||
402 | //Fill the covariance | |
403 | if (fFixedMeasurementCovariance) return kTRUE; | |
404 | //the dirs | |
405 | GetThetaPhiCov( *fPDir1ThetaPhiCov, *fPDir1, *fPDir1Cov ); | |
406 | GetThetaPhiCov( *fPDir2ThetaPhiCov, *fPDir2, *fPDir2Cov ); | |
407 | fPMeasurementCov->SetSub( 0, (*fPDir1ThetaPhiCov) + (*fPDir2ThetaPhiCov) ); | |
408 | ||
409 | //the points | |
410 | (*fPMeasurementCov)(2,2) = (*fPPoint1Cov)(0,0)+(*fPPoint2Cov)(0,0); | |
411 | (*fPMeasurementCov)(2,3) = (*fPPoint1Cov)(0,2)+(*fPPoint2Cov)(0,2); | |
412 | (*fPMeasurementCov)(3,2) = (*fPPoint1Cov)(2,0)+(*fPPoint2Cov)(2,0); | |
413 | (*fPMeasurementCov)(3,3) = (*fPPoint1Cov)(2,2)+(*fPPoint2Cov)(2,2); | |
414 | ||
415 | //if 3 track mode set, set also the stuff for 2nd TPC track | |
416 | if (f3TracksMode) | |
417 | { | |
418 | GetThetaPhiCov( *fPDir2bThetaPhiCov, *fPDir2b, *fPDir2bCov ); | |
419 | fPMeasurementCov->SetSub( 4, (*fPDir1ThetaPhiCov) + (*fPDir2bThetaPhiCov) ); | |
420 | (*fPMeasurementCov)(6,6) = (*fPPoint1Cov)(0,0)+(*fPPoint2bCov)(0,0); | |
421 | (*fPMeasurementCov)(6,7) = (*fPPoint1Cov)(0,2)+(*fPPoint2bCov)(0,2); | |
422 | (*fPMeasurementCov)(7,6) = (*fPPoint1Cov)(2,0)+(*fPPoint2bCov)(2,0); | |
423 | (*fPMeasurementCov)(7,7) = (*fPPoint1Cov)(2,2)+(*fPPoint2bCov)(2,2); | |
424 | } | |
425 | return kTRUE; | |
426 | } | |
427 | ||
428 | Bool_t AliRelAlignerKalman::FillMeasurementMatrix() | |
429 | { | |
430 | //Calculate the system matrix for the Kalman filter | |
431 | //approximate the system using as reference the track in the first volume | |
432 | ||
433 | Double_t delta = 1.e-8; | |
434 | TVectorD z1( fNMeasurementParams ); | |
435 | TVectorD z2( fNMeasurementParams ); | |
436 | TVectorD x1( *fPX ); | |
437 | TVectorD x2( *fPX ); | |
438 | TMatrixD D( fNMeasurementParams, 1 ); | |
439 | for ( Int_t i=0; i<fNSystemParams; i++ ) | |
440 | { | |
441 | x1 = *fPX; | |
442 | x2 = *fPX; | |
443 | x1(i) -= delta; | |
444 | x2(i) += delta; | |
445 | if (!PredictMeasurement( z1, x1 )) return kFALSE; | |
446 | if (!PredictMeasurement( z2, x2 )) return kFALSE; | |
447 | for (Int_t j=0; j<fNMeasurementParams; j++ ) | |
448 | D.GetMatrixArray()[j] = (z2.GetMatrixArray()[j]-z1.GetMatrixArray()[j])/(2.*delta); | |
449 | fPH->SetSub( 0, i, D ); | |
450 | } | |
451 | return kTRUE; | |
452 | } | |
453 | ||
454 | Bool_t AliRelAlignerKalman::PredictMeasurement( TVectorD& pred, const TVectorD& state ) | |
455 | { | |
456 | // Implements a system model for the Kalman fit | |
457 | // pred is [dtheta, dphi, dx, dz, [dtheta', dphi', dx', dz'] ] - second part is for 2nd TPC track in cosmics | |
458 | // state is [psi,theta,phi,x,y,z,driftTPC,offsetTPC] | |
459 | ||
460 | TVector3 newPoint = *fPPoint1; | |
461 | TVector3 newDir = *fPDir1; | |
462 | TVector3 shift; | |
463 | //TPC drift correction | |
464 | newDir(2) *= (1.+state(6)); | |
465 | newPoint(2) *= (1.+state(6)); | |
466 | //TPC offset | |
467 | if (newPoint(2)>0) newPoint(2) += state(7); | |
468 | else newPoint(2) -= state(7); | |
469 | //rotate | |
470 | TMatrixD rotmat(3,3); | |
471 | RotMat( rotmat, state ); | |
472 | newDir = rotmat * newDir; | |
473 | newPoint = rotmat * newPoint; | |
474 | //shift | |
475 | shift(0) = state(3); | |
476 | shift(1) = state(4); | |
477 | shift(2) = state(5); | |
478 | newPoint += shift; | |
479 | //Predicted measurement | |
480 | if (!IntersectionLinePlane( newPoint, newPoint, newDir, *fPRefPoint, *fPVec010 )) return kFALSE; | |
481 | pred(0) = newDir.Theta() - fPDir1->Theta(); | |
482 | pred(1) = newDir.Phi() - fPDir1->Phi(); | |
483 | pred(2) = newPoint(0) - fPPoint1->X(); | |
484 | pred(3) = newPoint(2) - fPPoint1->Z(); | |
485 | ||
486 | if (f3TracksMode) | |
487 | { | |
488 | //Do the same for second TPC track | |
489 | //should be the same as the first TPC track | |
490 | pred(4) = pred(0); | |
491 | pred(5) = pred(1); | |
492 | pred(6) = pred(2); | |
493 | pred(7) = pred(3); | |
494 | } | |
495 | return kTRUE; | |
496 | } | |
497 | ||
498 | Bool_t AliRelAlignerKalman::UpdateEstimateKalman() | |
499 | { | |
500 | //Kalman estimation of noisy constants: in the model A=1 | |
501 | //The arguments are (following the usual convention): | |
502 | // x - the state vector (parameters) | |
503 | // P - the state covariance matrix (parameter errors) | |
504 | // z - measurement vector | |
505 | // R - measurement covariance matrix | |
506 | // H - measurement model matrix ( z = Hx + v ) v being measurement noise (error fR) | |
507 | TVectorD *x = fPX; | |
508 | TMatrixDSym *P = fPXcov; | |
509 | TVectorD *z = fPMeasurement; | |
510 | TMatrixDSym *R = fPMeasurementCov; | |
511 | TMatrixD *H = fPH; | |
512 | ||
513 | TMatrixDSym I(TMatrixDSym::kUnit, *P); //unit matrix | |
514 | ||
515 | //predict the state | |
516 | *P = *P + fQ*I; //add some process noise (diagonal) | |
517 | ||
518 | // update prediction with measurement | |
519 | // calculate Kalman gain | |
520 | // K = PH/(HPH+R) | |
521 | TMatrixD PHT( *P, TMatrixD::kMultTranspose, *H ); //common factor (used twice) | |
522 | TMatrixD HPHT( *H, TMatrixD::kMult, PHT ); | |
523 | HPHT += *R; | |
524 | TMatrixD K(PHT, TMatrixD::kMult, HPHT.Invert()); //compute K | |
525 | ||
526 | // update the state and its covariance matrix | |
527 | TVectorD xupdate(*x); | |
528 | TVectorD Hx(*z); | |
529 | PredictMeasurement( Hx, *x ); | |
530 | xupdate = K*((*z)-Hx); | |
531 | ||
532 | //SIMPLE OUTLIER REJECTION | |
533 | if ( | |
534 | (TMath::Abs(xupdate(0)) > fOutRejSigmas*TMath::Sqrt((*P)(0,0))) || | |
535 | (TMath::Abs(xupdate(1)) > fOutRejSigmas*TMath::Sqrt((*P)(1,1))) || | |
536 | (TMath::Abs(xupdate(2)) > fOutRejSigmas*TMath::Sqrt((*P)(2,2))) || | |
537 | (TMath::Abs(xupdate(3)) > fOutRejSigmas*TMath::Sqrt((*P)(3,3))) || | |
538 | (TMath::Abs(xupdate(4)) > fOutRejSigmas*TMath::Sqrt((*P)(4,4))) || | |
539 | (TMath::Abs(xupdate(5)) > fOutRejSigmas*TMath::Sqrt((*P)(5,5))) || | |
540 | (TMath::Abs(xupdate(6)) > fOutRejSigmas*TMath::Sqrt((*P)(6,6))) || | |
541 | (TMath::Abs(xupdate(7)) > fOutRejSigmas*TMath::Sqrt((*P)(7,7))) | |
542 | ) | |
543 | { | |
544 | fNOutliers++; | |
545 | return kFALSE; | |
546 | } | |
547 | ||
548 | *x += xupdate; | |
549 | TMatrixD KH( K, TMatrixD::kMult, *H ); | |
550 | TMatrixD IKH(I); | |
551 | IKH = I - KH; | |
552 | TMatrixD IKHP( IKH, TMatrixD::kMult, *P ); // (I-KH)P | |
553 | TMatrixDSym_from_TMatrixD( *P, IKHP ); | |
554 | fNUpdates++; | |
555 | return kTRUE; | |
556 | } | |
557 | ||
558 | void AliRelAlignerKalman::TMatrixDSym_from_TMatrixD( TMatrixDSym& matsym, const TMatrixD& mat ) | |
559 | { | |
560 | //Produce a valid symmetric matrix out of a TMatrixD | |
561 | ||
562 | //not very efficient, diagonals are computer twice, but who cares | |
563 | for (Int_t i=0; i<mat.GetNcols(); i++) | |
564 | { | |
565 | for (Int_t j=i; j<mat.GetNcols(); j++) | |
566 | { | |
567 | Double_t average = (mat(i,j)+mat(j,i))/2.; | |
568 | matsym(i,j)=average; | |
569 | matsym(j,i)=average; | |
570 | } | |
571 | } | |
572 | return; | |
573 | } | |
574 | ||
575 | void AliRelAlignerKalman::GetThetaPhiCov( TMatrixDSym& cov, const TVector3& vec, const TMatrixDSym& vecCov ) | |
576 | { | |
577 | //Given the covariance matrix of a vector in euclid.space | |
578 | //give cov matrix of the 2 spherical angles | |
579 | const Double_t delta = 1e-8; | |
580 | TVector3 vec1; | |
581 | TVector3 vec2; | |
582 | TMatrixD T(2,3); | |
583 | for (Int_t i=0; i<3; i++) | |
584 | { | |
585 | vec1 = vec; | |
586 | vec2 = vec; | |
587 | vec1(i) -= delta/2.; | |
588 | vec2(i) += delta/2.; | |
589 | T(0,i) = (vec2.Theta() - vec1.Theta())/delta; | |
590 | T(1,i) = (vec2.Phi() - vec1.Phi())/delta; | |
591 | } | |
592 | TMatrixD tmp( T, TMatrixD::kMult, vecCov ); | |
593 | TMatrixD nscov( tmp, TMatrixD::kMultTranspose, T ); | |
594 | cov(0,0) = nscov(0,0); cov(0,1) = nscov(0,1); | |
595 | cov(1,0) = nscov(0,1); cov(1,1) = nscov(1,1); | |
596 | return; | |
597 | } | |
598 | ||
599 | Bool_t AliRelAlignerKalman::IntersectionLinePlane( TVector3& intersection, const TVector3& linebase, const TVector3& linedir, const TVector3& planepoint, const TVector3& planenormal ) | |
600 | { | |
601 | //calculate the intersection point between a straight line and a plane | |
602 | //plane is defined with a point in it and a normal, line has a point and direction | |
603 | if (planenormal==TVector3(0,1,0)) | |
604 | { | |
605 | if (linedir(1)==0) return kFALSE; | |
606 | Double_t t = ( planepoint(1) - linebase(1) ) / linedir(1); | |
607 | intersection = linebase + t*linedir; | |
608 | return kTRUE; | |
609 | } | |
610 | else | |
611 | { | |
612 | Double_t dirdotn = linedir.Dot(planenormal); | |
613 | if (dirdotn==0) return kFALSE; | |
614 | Double_t t = ( planepoint.Dot(planenormal) - linebase.Dot(planenormal) ) / dirdotn; | |
615 | intersection = linebase + t*linedir; | |
616 | return kTRUE; | |
617 | } | |
618 | } | |
619 | ||
620 | void AliRelAlignerKalman::Angles( TVectorD &angles, const TMatrixD &rotmat ) | |
621 | { | |
622 | //Calculate the Cardan angles (psi,theta,phi) from rotation matrix | |
623 | //b = R*a | |
624 | // | |
625 | angles(0) = TMath::ATan2( -rotmat(1,2), rotmat(2,2) ); | |
626 | angles(1) = TMath::ASin( rotmat(0,2) ); | |
627 | angles(2) = TMath::ATan2( -rotmat(0,1), rotmat(0,0) ); | |
628 | return; | |
629 | } | |
630 | ||
631 | void AliRelAlignerKalman::PrintCorrelationMatrix() | |
632 | { | |
633 | //Print the correlation matrix for the fitted parameters | |
634 | ||
635 | for ( Int_t j=0; j<fNSystemParams; j++ ) | |
636 | { | |
637 | for ( Int_t i=0; i<fNSystemParams; i++ ) | |
638 | { | |
639 | printf("%1.2f ", (*fPXcov)(i,j)/TMath::Sqrt( (*fPXcov)(i,i) * (*fPXcov)(j,j) ) ); | |
640 | }//for i | |
641 | printf("\n"); | |
642 | }//for j | |
643 | printf("\n"); | |
644 | return; | |
645 | } | |
646 | ||
647 | Bool_t AliRelAlignerKalman::FindCosmicInEvent( AliESDEvent* pEvent ) | |
648 | { | |
649 | //Find track point arrays belonging to one cosmic in a separately tracked ESD | |
650 | //and put them in the apropriate data members | |
651 | ||
652 | //Sanity cuts on tracks + check which tracks are ITS which are TPC | |
653 | Int_t ntracks = pEvent->GetNumberOfTracks(); //printf("number of tracks in event: %i\n", ntracks); | |
654 | if(ntracks<2) return kFALSE; | |
655 | Float_t* phiArr = new Float_t[ntracks]; | |
656 | Float_t* thetaArr = new Float_t[ntracks]; | |
657 | Double_t* distanceFromVertexArr = new Double_t[ntracks]; | |
658 | Int_t* goodtracksArr = new Int_t[ntracks]; | |
659 | Int_t* ITStracksArr = new Int_t[ntracks]; | |
660 | Int_t* TPCtracksArr = new Int_t[ntracks]; | |
661 | Int_t* nPointsArr = new Int_t[ntracks]; | |
662 | Int_t nITStracks = 0; | |
663 | Int_t nTPCtracks = 0; | |
664 | Int_t nGoodTracks = 0; | |
665 | AliESDtrack* pTrack = NULL; | |
666 | ||
667 | const AliESDVertex* pVertex = pEvent->GetVertex(); | |
668 | Double_t vertexposition[3]; | |
669 | pVertex->GetXYZ(vertexposition); | |
670 | ||
671 | Double_t magfield = pEvent->GetMagneticField(); | |
672 | ||
673 | //select sane tracks | |
674 | for (Int_t itrack=0; itrack < ntracks; itrack++) | |
675 | { | |
676 | pTrack = pEvent->GetTrack(itrack); | |
677 | if (!pTrack) {cout<<"no track!"<<endl;continue;} | |
678 | if(pTrack->GetNcls(0)+pTrack->GetNcls(1) < fMinPointsVol1) continue; | |
679 | Float_t phi = pTrack->GetAlpha()+TMath::ASin(pTrack->GetSnp()); | |
680 | Float_t theta = 0.5*TMath::Pi()-TMath::ATan(pTrack->GetTgl()); | |
681 | //printf("phi: %4.2f theta: %4.2f\n", phi, theta); | |
682 | if(fCuts) | |
683 | { | |
684 | if(pTrack->GetP()<fMinMom || pTrack->GetP()>fMaxMom) continue; | |
685 | Float_t abssinphi = TMath::Abs(TMath::Sin(phi)); | |
686 | if(abssinphi<fMinAbsSinPhi || abssinphi>fMaxAbsSinPhi) continue; | |
687 | Float_t sintheta = TMath::Sin(theta); | |
688 | if(sintheta<fMinSinTheta || sintheta>fMaxSinTheta) continue; | |
689 | } | |
690 | goodtracksArr[nGoodTracks]=itrack; | |
691 | phiArr[nGoodTracks]=phi; | |
692 | thetaArr[nGoodTracks]=theta; | |
693 | ||
694 | pTrack->RelateToVertex( pVertex, magfield, 10000. ); | |
695 | Double_t trackposition[3]; | |
696 | pTrack->GetXYZ( trackposition ); | |
697 | distanceFromVertexArr[nGoodTracks] = | |
698 | TMath::Sqrt((trackposition[0]-vertexposition[0])*(trackposition[0]-vertexposition[0]) | |
699 | + (trackposition[1]-vertexposition[1])*(trackposition[1]-vertexposition[1]) | |
700 | + (trackposition[2]-vertexposition[2])*(trackposition[2]-vertexposition[2])); | |
701 | ||
702 | //check if track is ITS or TPC | |
703 | if ( ((pTrack->GetStatus()&AliESDtrack::kITSin)>0)&& | |
704 | !((pTrack->GetStatus()&AliESDtrack::kTPCin)>0) ) | |
705 | { | |
706 | ITStracksArr[nITStracks] = nGoodTracks; | |
707 | nITStracks++; | |
708 | } | |
709 | ||
710 | if ( ((pTrack->GetStatus()&AliESDtrack::kTPCin)>0)&& | |
711 | !((pTrack->GetStatus()&AliESDtrack::kITSin)>0) ) | |
712 | { | |
713 | TPCtracksArr[nTPCtracks] = nGoodTracks; | |
714 | nTPCtracks++; | |
715 | } | |
716 | ||
717 | nGoodTracks++; | |
718 | }//for itrack - sanity cuts | |
719 | ||
720 | //printf("there are good tracks, %d in ITS and %d TPC.\n", nITStracks, nTPCtracks); | |
721 | ||
722 | if( nITStracks < 2 || nTPCtracks < 2 ) | |
723 | { | |
724 | delete [] goodtracksArr; goodtracksArr=0; | |
725 | delete [] ITStracksArr; ITStracksArr=0; | |
726 | delete [] TPCtracksArr; TPCtracksArr=0; | |
727 | delete [] nPointsArr; nPointsArr=0; | |
728 | delete [] phiArr; phiArr=0; | |
729 | delete [] thetaArr; thetaArr=0; | |
730 | delete [] distanceFromVertexArr; distanceFromVertexArr=0; | |
731 | return kFALSE; | |
732 | } | |
733 | ||
734 | //find matching in TPC | |
735 | Float_t min = 10000000.; | |
736 | Int_t TPCgood1 = -1, TPCgood2 = -1; | |
737 | for(Int_t itr1=0; itr1<nTPCtracks; itr1++) | |
738 | { | |
739 | for(Int_t itr2=itr1+1; itr2<nTPCtracks; itr2++) | |
740 | { | |
741 | Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[TPCtracksArr[itr1]]-thetaArr[TPCtracksArr[itr2]]); | |
742 | if(deltatheta > fMaxMatchingAngle) continue; | |
743 | Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[TPCtracksArr[itr1]]-phiArr[TPCtracksArr[itr2]])-TMath::Pi()); | |
744 | if(deltaphi > fMaxMatchingAngle) continue; | |
745 | //printf("TPC: %f %f %f %f\n",deltaphi,deltatheta,thetaArr[TPCtracksArr[itr1]],thetaArr[TPCtracksArr[itr2]]); | |
746 | if(deltatheta+deltaphi<min) //only the best matching pair | |
747 | { | |
748 | min=deltatheta+deltaphi; | |
749 | TPCgood1 = TPCtracksArr[itr1]; //store the index of track in goodtracksArr[] | |
750 | TPCgood2 = TPCtracksArr[itr2]; | |
751 | } | |
752 | } | |
753 | } | |
754 | if (TPCgood1 < 0) //no dubble cosmic track | |
755 | { | |
756 | delete [] goodtracksArr; goodtracksArr=0; | |
757 | delete [] ITStracksArr; ITStracksArr=0; | |
758 | delete [] TPCtracksArr; TPCtracksArr=0; | |
759 | delete [] nPointsArr; nPointsArr=0; | |
760 | delete [] phiArr; phiArr=0; | |
761 | delete [] thetaArr; thetaArr=0; | |
762 | delete [] distanceFromVertexArr; distanceFromVertexArr=0; | |
763 | return kFALSE; | |
764 | } | |
765 | ||
766 | //in ITS find 2 tracks that best match the found TPC cosmic | |
767 | Double_t min1 = 10000000.; | |
768 | Double_t min2 = 10000000.; | |
769 | Int_t ITSgood1 = -1, ITSgood2 = -1; | |
770 | for(Int_t itr1=0; itr1<nITStracks; itr1++) | |
771 | { | |
772 | if(distanceFromVertexArr[ITStracksArr[itr1]]>fMaxMatchingDistance) continue; | |
773 | Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[ITStracksArr[itr1]]-thetaArr[TPCgood1]); | |
774 | if(deltatheta > fMaxMatchingAngle) deltatheta = TMath::Abs( deltatheta - TMath::Pi() ); | |
775 | if(deltatheta > fMaxMatchingAngle) continue; | |
776 | Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[ITStracksArr[itr1]]-phiArr[TPCgood1])-TMath::Pi()); | |
777 | if(deltaphi > fMaxMatchingAngle) deltaphi = TMath::Abs( deltaphi - TMath::Pi() ); | |
778 | if(deltaphi > fMaxMatchingAngle) continue; | |
779 | //printf("ITS %i dtheta, dphi, vrtx: %.4f, %.4f, %.4f\n",ITStracksArr[itr1], deltatheta, deltaphi,distanceFromVertexArr[ITStracksArr[itr1]]); | |
780 | if(deltatheta+deltaphi<min1) //only the best one | |
781 | { | |
782 | min1=deltatheta+deltaphi; | |
783 | //ITSgood2 = ITSgood1; | |
784 | ITSgood1 = ITStracksArr[itr1]; | |
785 | continue; | |
786 | } | |
787 | if(deltatheta+deltaphi<min2) //the second best | |
788 | { | |
789 | min2=deltatheta+deltaphi; | |
790 | ITSgood2 = ITStracksArr[itr1]; | |
791 | continue; | |
792 | } | |
793 | } | |
794 | if (ITSgood2 < 0) //no ITS cosmic track | |
795 | { | |
796 | delete [] goodtracksArr; goodtracksArr=0; | |
797 | delete [] ITStracksArr; ITStracksArr=0; | |
798 | delete [] TPCtracksArr; TPCtracksArr=0; | |
799 | delete [] nPointsArr; nPointsArr=0; | |
800 | delete [] phiArr; phiArr=0; | |
801 | delete [] thetaArr; thetaArr=0; | |
802 | delete [] distanceFromVertexArr; distanceFromVertexArr=0; | |
803 | return kFALSE; | |
804 | } | |
805 | ||
806 | //we found a cosmic | |
807 | fNMatchedCosmics++; | |
808 | ||
809 | ////////////////////////////////////////////////////////////////////////////////////// | |
810 | // convert indexes from local goodtrackarrays to global track index | |
811 | TPCgood1 = goodtracksArr[TPCgood1]; | |
812 | TPCgood2 = goodtracksArr[TPCgood2]; | |
813 | ITSgood1 = goodtracksArr[ITSgood1]; | |
814 | ITSgood2 = goodtracksArr[ITSgood2]; | |
815 | ///////////////////////////////////////////////////////////////////////////////////// | |
816 | ||
817 | AliESDtrack * pTPCtrack1 = pEvent->GetTrack(TPCgood1); | |
818 | AliESDtrack * pTPCtrack2 = pEvent->GetTrack(TPCgood2); | |
819 | AliESDtrack * pITStrack1 = pEvent->GetTrack(ITSgood1); | |
820 | AliESDtrack * pITStrack2 = pEvent->GetTrack(ITSgood2); | |
821 | const AliTrackPointArray* pTPCArray1 = pTPCtrack1->GetTrackPointArray(); | |
822 | const AliTrackPointArray* pTPCArray2 = pTPCtrack2->GetTrackPointArray(); | |
823 | const AliTrackPointArray* pITSArray1 = pITStrack1->GetTrackPointArray(); | |
824 | const AliTrackPointArray* pITSArray2 = pITStrack2->GetTrackPointArray(); | |
825 | ||
826 | AliTrackPointArray* pfullITStrackArray = new AliTrackPointArray(1); | |
827 | JoinTrackArrays( pfullITStrackArray, pITSArray1, pITSArray2 ); | |
828 | ||
829 | //cout<<"selected tracks: TPC: "<<TPCgood1<<" "<<TPCgood2<<" ITS: "<<ITSgood1<<" "<<ITSgood2<<endl; | |
830 | ||
831 | //Fill the final trackpointarrays | |
832 | if (!ExtractSubTrackPointArray( fPTrackPointArray1, fPVolIDArray1, pfullITStrackArray, 1, 6 )) return kFALSE; | |
833 | if (!ExtractSubTrackPointArray( fPTrackPointArray2, fPVolIDArray2, pTPCArray1, 7, 8 )) return kFALSE; | |
834 | if (!ExtractSubTrackPointArray( fPTrackPointArray2b, fPVolIDArray2b, pTPCArray2, 7, 8 )) return kFALSE; | |
835 | ||
836 | delete pfullITStrackArray; pfullITStrackArray=NULL; | |
837 | delete [] goodtracksArr; goodtracksArr=NULL; | |
838 | delete [] ITStracksArr; ITStracksArr=NULL; | |
839 | delete [] TPCtracksArr; TPCtracksArr=NULL; | |
840 | delete [] nPointsArr; nPointsArr=NULL; | |
841 | delete [] phiArr; phiArr=NULL; | |
842 | delete [] thetaArr; thetaArr=NULL; | |
843 | delete [] distanceFromVertexArr; distanceFromVertexArr=NULL; | |
844 | //printf("FindCosmicInEvent END\n"); | |
845 | return kTRUE; | |
846 | } | |
847 | ||
848 | Bool_t AliRelAlignerKalman::ExtractSubTrackPointArray( AliTrackPointArray* pTrackOut, TArrayI* pVolIDArrayOut, const AliTrackPointArray* pTrackIn, const Int_t firstlayer, const Int_t lastlayer ) | |
849 | { | |
850 | //printf("ExtractSubTrackPointArray\n"); | |
851 | //From pTrackIn select points between firstlayer and lastlayer and put the result in pTrackOut. | |
852 | //VolID's of selected points go in pVolIDArrayOut | |
853 | //only good points selected. | |
854 | //points can be ordered with z by setting fSortTrackPointsWithY | |
855 | ||
856 | AliTrackPoint point; | |
857 | Int_t nPointsIn = pTrackIn->GetNPoints(); | |
858 | if (nPointsIn<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE; | |
859 | Int_t iPointArr[1000]; | |
860 | Int_t VolIDArr[1000]; | |
861 | Float_t yArr[1000]; | |
862 | Int_t iOuter=-1; | |
863 | Int_t OuterLayerID=0; | |
864 | Int_t nGood=0; | |
865 | ||
866 | //loop over trackpoints and record the good ones | |
867 | for (Int_t i=0; i<nPointsIn; i++) | |
868 | { | |
869 | UShort_t VolIDshort = pTrackIn->GetVolumeID()[i]; | |
870 | Int_t layerID = AliGeomManager::VolUIDToLayer(VolIDshort); | |
871 | //some points are very dirty: have nan's in coordinates or are otherwise sick | |
872 | { | |
873 | Float_t x = pTrackIn->GetX()[i]; | |
874 | Float_t y = pTrackIn->GetY()[i]; | |
875 | Float_t z = pTrackIn->GetZ()[i]; | |
876 | if ( ((TMath::Abs(x)<1.)||(TMath::Abs(x)>1000.))&& | |
877 | ((TMath::Abs(y)<1.)||(TMath::Abs(y)>1000.))|| | |
878 | ((TMath::Abs(z)>1000.)) | |
879 | ) | |
880 | continue; | |
881 | } | |
882 | if ( (layerID >= firstlayer) && (layerID <= lastlayer) ) | |
883 | { | |
884 | ||
885 | iPointArr[nGood] = i; | |
886 | VolIDArr[nGood] = VolIDshort; | |
887 | yArr[nGood] = pTrackIn->GetY()[i]; | |
888 | if (layerID>OuterLayerID) | |
889 | { | |
890 | OuterLayerID=layerID; | |
891 | iOuter = nGood; | |
892 | } | |
893 | nGood++; | |
894 | } | |
895 | else continue; | |
896 | }//for i=0..nPointsIn | |
897 | if (nGood<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE; | |
898 | //Fill the output array of VolID's | |
899 | delete pVolIDArrayOut; | |
900 | pVolIDArrayOut = new TArrayI( nGood ); | |
901 | ||
902 | //fill the output trackarray | |
903 | Int_t* iSortedYArr = new Int_t[nGood]; | |
904 | if (fSortTrackPointsWithY) | |
905 | { | |
906 | TMath::Sort( nGood, yArr, iSortedYArr, kFALSE ); | |
907 | } | |
908 | delete pTrackOut; | |
909 | pTrackOut = new AliTrackPointArray( nGood ); | |
910 | ||
911 | for ( Int_t i=0; i<nGood; i++) | |
912 | { | |
913 | pTrackIn->GetPoint( point, iPointArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i] ); | |
914 | pTrackOut->AddPoint( i, &point ); | |
915 | pVolIDArrayOut->AddAt( VolIDArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i], i ); | |
916 | } | |
917 | //printf("ExtractSubTrackPointArray END\n"); | |
918 | return kTRUE; | |
919 | } | |
920 | ||
921 | void AliRelAlignerKalman::SortTrackPointArrayWithY( AliTrackPointArray* pout, const AliTrackPointArray* pinn, const Bool_t descending ) | |
922 | { | |
923 | //sorts a given trackpointarray by y coordinate | |
924 | ||
925 | Int_t npoints = pinn->GetNPoints(); | |
926 | const AliTrackPointArray* pin; | |
927 | if (pinn==pout) | |
928 | pin = new AliTrackPointArray(*pinn); | |
929 | else | |
930 | { | |
931 | pin = pinn; | |
932 | if (pout!=NULL) delete pout; | |
933 | pout = new AliTrackPointArray(npoints); | |
934 | } | |
935 | ||
936 | Int_t* iarr = new Int_t[npoints]; | |
937 | ||
938 | TMath::Sort( npoints, pin->GetY(), iarr, descending ); | |
939 | ||
940 | AliTrackPoint p; | |
941 | for (Int_t i=0; i<npoints; i++) | |
942 | { | |
943 | pin->GetPoint( p, iarr[i] ); | |
944 | pout->AddPoint( i, &p ); | |
945 | } | |
946 | delete [] iarr; | |
947 | } | |
948 | ||
949 | Bool_t AliRelAlignerKalman::FitTrackPointArrayRieman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint ) | |
950 | { | |
951 | //printf("FitTrackPointArrayRieman\n"); | |
952 | //Use AliTrackFitterRieman to fit a track through given point array | |
953 | ||
954 | AliTrackFitterRieman fitter; | |
955 | AliTrackPoint trackPoint; | |
956 | const Float_t* pointcov; | |
957 | ////here's an ugly hack:///////// | |
958 | //AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayIn); | |
959 | ///////////////////////////////// | |
960 | fitter.SetTrackPointArray( pTrackPointArray, kFALSE ); | |
961 | if ( !fitter.Fit( pVolIDs ) ) return kFALSE; | |
962 | if ( !fitter.GetPCA( *fPRefAliTrackPoint, trackPoint ) ) return kFALSE; | |
963 | Double_t xPoint = trackPoint.GetX(); | |
964 | pPoint->SetXYZ( xPoint, trackPoint.GetY(), trackPoint.GetZ() ); | |
965 | pDir->SetXYZ( 1., fitter.GetDYat(xPoint), fitter.GetDZat(xPoint) ); | |
966 | *pDir = pDir->Unit(); | |
967 | //TODO cov of dir! | |
968 | ||
969 | pointcov = trackPoint.GetCov(); | |
970 | (*pPointCov)(0,0) = pointcov[0];(*pPointCov)(0,1) = pointcov[1];(*pPointCov)(0,2) = pointcov[2]; | |
971 | (*pPointCov)(1,0) = pointcov[1];(*pPointCov)(1,1) = pointcov[3];(*pPointCov)(1,2) = pointcov[4]; | |
972 | (*pPointCov)(2,0) = pointcov[2];(*pPointCov)(2,1) = pointcov[4];(*pPointCov)(2,2) = pointcov[5]; | |
973 | ||
974 | //printf("FitTrackPointArrayRieman END\n"); | |
975 | return kTRUE; | |
976 | } | |
977 | ||
978 | Bool_t AliRelAlignerKalman::FitTrackPointArrayKalman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint ) | |
979 | { | |
980 | //printf("FitTrackPointArrayKalman\n"); | |
981 | //Use AliTrackFitterKalman to fit a track through given point array | |
982 | //still needs work | |
983 | ||
984 | AliTrackFitterKalman fitter; | |
985 | AliTrackPoint trackPoint, trackPoint2; | |
986 | ||
987 | pTrackPointArray->GetPoint( trackPoint, 0 ); | |
988 | pTrackPointArray->GetPoint( trackPoint2, 1 ); | |
989 | //Make sure all points count | |
990 | fitter.SetMaxChi2(100000000.); | |
991 | if (!fitter.MakeSeed( &trackPoint, &trackPoint2 )) return kFALSE; | |
992 | Int_t npoints = pTrackPointArray->GetNPoints(); | |
993 | for (Int_t i=2; i<npoints; i++) | |
994 | { | |
995 | pTrackPointArray->GetPoint( trackPoint, i ); | |
996 | if (!fitter.AddPoint( &trackPoint )) continue; | |
997 | }//for i | |
998 | TMatrixDSym fullcov = fitter.GetCovariance(); | |
999 | printf("FitTrackPointArrayKalman: the covariance matrix of the fit"); | |
1000 | fullcov.Print(); | |
1001 | ||
1002 | //now propagate to ref plane - its a hack that depends on the implementation of AddPoint()! | |
1003 | // - first set the maxchi2 to 0 so addpoint returns false, but | |
1004 | //actually the trackparameters have already been been propagated to the new planei, it's safe | |
1005 | //because AliTrackFitterKalman::Propagate() is always kTRUE. | |
1006 | fitter.SetMaxChi2(0.); | |
1007 | fitter.AddPoint( fPRefAliTrackPoint ); | |
1008 | ||
1009 | const Double_t* pparams = fitter.GetParam(); | |
1010 | fullcov = fitter.GetCovariance(); | |
1011 | ||
1012 | pDir->SetXYZ( pparams[3], 1., pparams[4] ); | |
1013 | pPoint->SetXYZ( pparams[0], pparams[1], pparams[2] ); | |
1014 | ||
1015 | (*pPointCov)(0,0) = fullcov(0,0);(*pPointCov)(0,1) = fullcov(0,1);(*pPointCov)(0,2) = fullcov(0,2); | |
1016 | (*pPointCov)(1,0) = fullcov(1,0);(*pPointCov)(1,1) = fullcov(1,1);(*pPointCov)(1,2) = fullcov(1,2); | |
1017 | (*pPointCov)(2,0) = fullcov(2,0);(*pPointCov)(2,1) = fullcov(2,1);(*pPointCov)(2,2) = fullcov(2,2); | |
1018 | ||
1019 | (*pDirCov)(0,0)=fullcov(3,3); (*pDirCov)(0,1)=0.; (*pDirCov)(0,2)=fullcov(3,4); | |
1020 | (*pDirCov)(1,0)=0.; (*pDirCov)(1,1)=0.; (*pDirCov)(1,2)=0.; | |
1021 | (*pDirCov)(2,0)=fullcov(4,3); (*pDirCov)(2,1)=0.; (*pDirCov)(2,2)=fullcov(4,4); | |
1022 | return kTRUE; | |
1023 | } | |
1024 | ||
1025 | void AliRelAlignerKalman::SanitizeExtendedCovMatrix( TMatrixDSym& covout, const TMatrixDSym& pointcov, const TVector3& dir ) | |
1026 | { | |
1027 | //reverse the effect of extending of the covariance matrix along the track | |
1028 | //direction as done by the AliTrackFitters | |
1029 | ||
1030 | //idea is to rotate to a system where track direction is y axis, | |
1031 | //in that refsystem setting the y related components of covariance | |
1032 | //then rotate back. | |
1033 | TVector3 yaxis(0,1,0); | |
1034 | Double_t angle = dir.Angle(yaxis); | |
1035 | TVector3 rotaxisvec = dir.Cross(yaxis); | |
1036 | TVector3 rotaxis = rotaxisvec.Unit(); | |
1037 | TMatrixD R(3,3); | |
1038 | ||
1039 | //Fill the rotation matrix. | |
1040 | Double_t ex=rotaxis(0); | |
1041 | Double_t ey=rotaxis(1); | |
1042 | Double_t ez=rotaxis(2); | |
1043 | Double_t sin = TMath::Sin(angle); | |
1044 | Double_t cos = TMath::Cos(angle); | |
1045 | Double_t icos = 1 - cos; | |
1046 | R(0,0) = cos + (ex*ex)*icos; | |
1047 | R(0,1) = icos*ex*ey - ez*sin; | |
1048 | R(0,2) = icos*ex*ez + ey*sin; | |
1049 | R(1,0) = icos*ey*ex + ez*sin; | |
1050 | R(1,1) = cos + (ey*ey)*icos; | |
1051 | R(1,2) = icos*ey*ez - ex*sin; | |
1052 | R(2,0) = icos*ez*ex - ey*sin; | |
1053 | R(2,1) = icos*ez*ey + ex*sin; | |
1054 | R(2,2) = cos + (ez*ez)*icos; | |
1055 | ||
1056 | //Transform covariance: | |
1057 | TMatrixD covR( pointcov, TMatrixD::kMultTranspose, R); | |
1058 | TMatrixD RcovR( R, TMatrixD::kMult, covR ); | |
1059 | TMatrixD newcov(3,3); | |
1060 | newcov(0,0)=RcovR(0,0); | |
1061 | newcov(0,2)=RcovR(0,2); | |
1062 | newcov(2,0)=RcovR(2,0); | |
1063 | newcov(2,2)=RcovR(2,2); | |
1064 | newcov(1,1)=(newcov(0,0)+newcov(2,2))/2.; //this is a bit insane!! | |
1065 | covR = newcov *R; | |
1066 | RcovR = R.T() * covR; | |
1067 | TMatrixDSym_from_TMatrixD( covout, RcovR ); | |
1068 | } | |
1069 | ||
1070 | void AliRelAlignerKalman::JoinTrackArrays( AliTrackPointArray* pout, const AliTrackPointArray* pin1, const AliTrackPointArray* pin2 ) | |
1071 | { | |
1072 | //Join two AliTrackPointArrays | |
1073 | ||
1074 | Int_t np1 = pin1->GetNPoints(); | |
1075 | Int_t np2 = pin2->GetNPoints(); | |
1076 | if (pout!=NULL) delete pout; | |
1077 | pout = new AliTrackPointArray(np1+np2); | |
1078 | AliTrackPoint p; | |
1079 | for (Int_t i=0;i<np1;i++) | |
1080 | { | |
1081 | pin1->GetPoint( p, i ); | |
1082 | pout->AddPoint( i, &p ); | |
1083 | } | |
1084 | for (Int_t i=0;i<np2;i++) | |
1085 | { | |
1086 | pin2->GetPoint( p, i ); | |
1087 | pout->AddPoint( i+np1, &p ); | |
1088 | } | |
1089 | } | |
1090 | ||
1091 | Bool_t AliRelAlignerKalman::ProcessTrackPointArrays() | |
1092 | { | |
1093 | //Fit the track point arrays and update some household info | |
1094 | ||
1095 | fFitted=kFALSE; //not fitted yet | |
1096 | if ( !FitTrackPointArrayKalman( fPPoint2, fPPoint2Cov, fPDir2, fPDir2Cov, | |
1097 | fPTrackPointArray2, fPVolIDArray2, fPRefAliTrackPoint ) ) | |
1098 | { | |
1099 | fNUnfitted++; | |
1100 | return kFALSE; | |
1101 | } | |
1102 | ||
1103 | if ( f3TracksMode ) | |
1104 | { | |
1105 | if ( !FitTrackPointArrayKalman( fPPoint2b, fPPoint2bCov, fPDir2b, fPDir2bCov, | |
1106 | fPTrackPointArray2b, fPVolIDArray2b, fPRefAliTrackPoint ) ) | |
1107 | { | |
1108 | fNUnfitted++; | |
1109 | return kFALSE; | |
1110 | } | |
1111 | } | |
1112 | ||
1113 | if ( !FitTrackPointArrayKalman( fPPoint1, fPPoint1Cov, fPDir1, fPDir1Cov, | |
1114 | fPTrackPointArray1, fPVolIDArray1, fPRefAliTrackPoint ) ) | |
1115 | { | |
1116 | fNUnfitted++; | |
1117 | return kFALSE; | |
1118 | } | |
1119 | ||
1120 | fFitted=kTRUE; | |
1121 | //printf("ProcessTrackPointArrays: fitted!\n"); | |
1122 | return kTRUE; | |
1123 | } | |
1124 | ||
1125 | Bool_t AliRelAlignerKalman::UpdateCalibration() | |
1126 | { | |
1127 | //Update the calibration with new data, used in calibration pass | |
1128 | ||
1129 | fPThetaMesHist->Fill( (*fPMeasurement)(0) ); | |
1130 | fPPhiMesHist->Fill( (*fPMeasurement)(1) ); | |
1131 | fPXMesHist->Fill( (*fPMeasurement)(2) ); | |
1132 | fPZMesHist->Fill( (*fPMeasurement)(3) ); | |
1133 | if (f3TracksMode) | |
1134 | { | |
1135 | fPThetaMesHist2->Fill( (*fPMeasurement)(4) ); | |
1136 | fPPhiMesHist2->Fill( (*fPMeasurement)(5) ); | |
1137 | fPXMesHist2->Fill( (*fPMeasurement)(6) ); | |
1138 | fPZMesHist2->Fill( (*fPMeasurement)(7) ); | |
1139 | } | |
1140 | fCalibrationUpdated=kTRUE; | |
1141 | return kTRUE; | |
1142 | } | |
1143 | ||
1144 | Bool_t AliRelAlignerKalman::SetCalibrationPass( Bool_t cp ) | |
1145 | { | |
1146 | //sets the calibration mode | |
1147 | if (cp) | |
1148 | { | |
1149 | fCalibrationPass=kTRUE; | |
1150 | return kTRUE; | |
1151 | }//if (cp) | |
1152 | else | |
1153 | { | |
1154 | if (!fCalibrationUpdated) return kFALSE; | |
1155 | if (fCalibrationPass) // do it only after the calibration pass | |
1156 | { | |
1157 | Double_t tmp; | |
1158 | fPMeasurementCov->Zero(); //reset the covariance | |
1159 | ||
1160 | fPThetaMesHist->Fit("gaus"); | |
1161 | TF1* fitformula = fPThetaMesHist->GetFunction("gaus"); | |
1162 | tmp = fitformula->GetParameter(2); | |
1163 | (*fPMeasurementCov)(0,0) = tmp*tmp; | |
1164 | ||
1165 | fPPhiMesHist->Fit("gaus"); | |
1166 | fitformula = fPPhiMesHist->GetFunction("gaus"); | |
1167 | tmp = fitformula->GetParameter(2); | |
1168 | (*fPMeasurementCov)(1,1) = tmp*tmp; | |
1169 | ||
1170 | fPXMesHist->Fit("gaus"); | |
1171 | fitformula = fPXMesHist->GetFunction("gaus"); | |
1172 | tmp = fitformula->GetParameter(2); | |
1173 | (*fPMeasurementCov)(2,2) = tmp*tmp; | |
1174 | ||
1175 | fPZMesHist->Fit("gaus"); | |
1176 | fitformula = fPZMesHist->GetFunction("gaus"); | |
1177 | tmp = fitformula->GetParameter(2); | |
1178 | (*fPMeasurementCov)(3,3) = tmp*tmp; | |
1179 | ||
1180 | if (f3TracksMode) | |
1181 | { | |
1182 | fPThetaMesHist2->Fit("gaus"); | |
1183 | fitformula = fPThetaMesHist2->GetFunction("gaus"); | |
1184 | tmp = fitformula->GetParameter(2); | |
1185 | (*fPMeasurementCov)(4,4) = tmp*tmp; | |
1186 | ||
1187 | fPPhiMesHist2->Fit("gaus"); | |
1188 | fitformula = fPPhiMesHist2->GetFunction("gaus"); | |
1189 | tmp = fitformula->GetParameter(2); | |
1190 | (*fPMeasurementCov)(5,5) = tmp*tmp; | |
1191 | ||
1192 | fPXMesHist2->Fit("gaus"); | |
1193 | fitformula = fPXMesHist2->GetFunction("gaus"); | |
1194 | tmp = fitformula->GetParameter(2); | |
1195 | (*fPMeasurementCov)(6,6) = tmp*tmp; | |
1196 | ||
1197 | fPZMesHist2->Fit("gaus"); | |
1198 | fitformula = fPZMesHist2->GetFunction("gaus"); | |
1199 | tmp = fitformula->GetParameter(2); | |
1200 | (*fPMeasurementCov)(7,7) = tmp*tmp; | |
1201 | } | |
1202 | ||
1203 | fCalibrationPass=kFALSE; | |
1204 | fFixedMeasurementCovariance=kTRUE; | |
1205 | fPMeasurementCov->Print(); | |
1206 | return kTRUE; | |
1207 | }//if (fCalibrationPass) | |
1208 | return kFALSE; | |
1209 | }//else (cp) | |
1210 | return kFALSE; | |
1211 | } | |
1212 | ||
1213 | void AliRelAlignerKalman::PrintDebugInfo() | |
1214 | { | |
1215 | //prints debug info | |
1216 | cout<<"AliRelAlignerKalman debug info"<<endl; | |
1217 | printf("fPTrackPointArray1 y: "); | |
1218 | for (Int_t i=0; i<fPTrackPointArray1->GetNPoints();i++) | |
1219 | { | |
1220 | printf("%.2f ",fPTrackPointArray1->GetY()[i]); | |
1221 | } | |
1222 | printf("\n"); | |
1223 | printf("fPTrackPointArray2 y: "); | |
1224 | for (Int_t i=0; i<fPTrackPointArray2->GetNPoints();i++) | |
1225 | { | |
1226 | printf("%.2f ",fPTrackPointArray2->GetY()[i]); | |
1227 | } | |
1228 | printf("\n"); | |
1229 | printf("fPTrackPointArray2b y: "); | |
1230 | for (Int_t i=0; i<fPTrackPointArray2b->GetNPoints();i++) | |
1231 | { | |
1232 | printf("%.2f ",fPTrackPointArray2b->GetY()[i]); | |
1233 | } | |
1234 | printf("\n"); | |
1235 | ||
1236 | printf("Measurement covariance"); | |
1237 | fPMeasurementCov->Print(); | |
1238 | } | |
1239 |