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