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