Analysis Software
Documentation for sPHENIX simulation software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GFGbl.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file GFGbl.cc
1 //-*-mode: C++; c-basic-offset: 2; -*-
2 /* Copyright 2013
3  * Authors: Sergey Yashchenko and Tadeas Bilka
4  *
5  * This is an interface to General Broken Lines
6  *
7  * Version: 5 (Tadeas)
8  * - several bug-fixes:
9  * - Scatterers at bad points
10  * - Jacobians at a point before they should be (code reorganized)
11  * - Change of sign of residuals
12  * Version: 4 (Tadeas)
13  * Fixed calculation of equvivalent scatterers (solution by C. Kleinwort)
14  * Now a scatterer is inserted at each measurement (except last) and between each two measurements.
15  * TrueHits/Clusters. Ghost (1D) hits ignored. With or without magnetic field.
16  * Version: 3 (Tadeas)
17  * This version now supports both TrueHits and Clusters for VXD.
18  * It can be used for arbitrary material distribution between
19  * measurements. Moments of scattering distribution are computed
20  * and translated into two equivalent thin GBL scatterers placed
21  * at computed positions between measurement points.
22  * Version: 2 ... never published (Tadeas)
23  * Scatterer at each boundary (tooo many scatterers). TrueHits/Clusters. Without global der.&MP2 output.
24  * Version: 1 (Sergey & Tadeas)
25  * Scatterers at measurement planes. TrueHits
26  * Version 0: (Sergey)
27  * Without scatterers. Genfit 1.
28  *
29  * This file is part of GENFIT.
30  *
31  * GENFIT is free software: you can redistribute it and/or modify
32  * it under the terms of the GNU Lesser General Public License as published
33  * by the Free Software Foundation, either version 3 of the License, or
34  * (at your option) any later version.
35  *
36  * GENFIT is distributed in the hope that it will be useful,
37  * but WITHOUT ANY WARRANTY; without even the implied warranty of
38  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
39  * GNU Lesser General Public License for more details.
40  *
41  * You should have received a copy of the GNU Lesser General Public License
42  * along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
43  */
44 
45 #include "GFGbl.h"
46 #include "GblTrajectory.h"
47 #include "GblPoint.h"
48 #include "MyDebugTools.h"
49 
50 #include "AbsMeasurement.h"
51 #include "PlanarMeasurement.h"
52 #include "KalmanFitterInfo.h"
53 
54 #include "Track.h"
55 #include <TFile.h>
56 #include <TH1F.h>
57 #include <TTree.h>
58 #include <string>
59 #include <list>
60 #include <FieldManager.h>
61 #include <HMatrixU.h>
62 #include <HMatrixV.h>
63 #include <Math/SMatrix.h>
64 #include <TMatrixD.h>
65 #include <TVectorDfwd.h>
66 #include <TMatrixT.h>
67 
68 #include <TVector3.h>
69 
70 //#define DEBUG
71 //#define OUTPUT
72 
73 
74 #ifdef DEBUG
75 //ofstream debug("gbl.debug");
76 #endif
77 
78 #ifdef OUTPUT
79 
80 std::string rootFileName = "gbl.root";
81 
82 
83 TFile* diag;
84 TH1F* resHistosU[12];
85 TH1F* resHistosV[12];
86 TH1F* mhistosU[12];
87 TH1F* mhistosV[12];
88 TH1F* ghistosU[12];
89 TH1F* ghistosV[12];
90 TH1F* downWeightsHistosU[12];
91 TH1F* downWeightsHistosV[12];
92 TH1F* localPar1[12];
93 TH1F* localPar2[12];
94 TH1F* localPar3[12];
95 TH1F* localPar4[12];
96 TH1F* localPar5[12];
97 TH1F* localCov1[12];
98 TH1F* localCov2[12];
99 TH1F* localCov3[12];
100 TH1F* localCov4[12];
101 TH1F* localCov5[12];
102 TH1F* localCov12[12];
103 TH1F* localCov13[12];
104 TH1F* localCov14[12];
105 TH1F* localCov15[12];
106 TH1F* localCov23[12];
107 TH1F* localCov24[12];
108 TH1F* localCov25[12];
109 TH1F* localCov34[12];
110 TH1F* localCov35[12];
111 TH1F* localCov45[12];
112 TH1I* fitResHisto;
113 TH1I* ndfHisto;
114 TH1F* chi2OndfHisto;
115 TH1F* pValueHisto;
116 TH1I* stats;
117 
118 
119 
120 bool writeHistoDataForLabel(double label, TVectorD res, TVectorD measErr, TVectorD resErr, TVectorD downWeights, TVectorD localPar, TMatrixDSym localCov)
121 {
122  if (label < 1.) return false;
123 
124  unsigned int id = floor(label);
125  // skip segment (5 bits)
126  id = id >> 5;
127  unsigned int sensor = id & 7;
128  id = id >> 3;
129  unsigned int ladder = id & 31;
130  id = id >> 5;
131  unsigned int layer = id & 7;
132  if (layer == 7 && ladder == 2) {
133  label = sensor;
134  } else if (layer == 7 && ladder == 3) {
135  label = sensor + 9 - 3;
136  } else {
137  label = layer + 3;
138  }
139 
140  if (label > 12.) return false;
141 
142  int i = int(label);
143 
144  #ifdef OUTPUT
145  resHistosU[i - 1]->Fill(res[0]);
146  resHistosV[i - 1]->Fill(res[1]);
147  mhistosU[i - 1]->Fill(res[0] / measErr[0]);
148  mhistosV[i - 1]->Fill(res[1] / measErr[1]);
149  ghistosU[i - 1]->Fill(res[0] / resErr[0]);
150  ghistosV[i - 1]->Fill(res[1] / resErr[1]);
151  downWeightsHistosU[i - 1]->Fill(downWeights[0]);
152  downWeightsHistosV[i - 1]->Fill(downWeights[1]);
153  localPar1[i - 1]->Fill(localPar(0));
154  localPar2[i - 1]->Fill(localPar(1));
155  localPar3[i - 1]->Fill(localPar(2));
156  localPar4[i - 1]->Fill(localPar(3));
157  localPar5[i - 1]->Fill(localPar(4));
158  #endif
159 
160 
161  return true;
162 }
163 #endif
164 
165 // Millepede Binary File for output of GBL trajectories for alignment
167 // Minimum scattering sigma (will be squared and inverted...)
168 const double scatEpsilon = 1.e-8;
169 
170 
171 using namespace gbl;
172 using namespace std;
173 using namespace genfit;
174 
175 GFGbl::GFGbl() :
176 AbsFitter(), m_milleFileName("millefile.dat"), m_gblInternalIterations("THC"), m_pValueCut(0.), m_minNdf(1),
177 m_chi2Cut(0.),
178 m_enableScatterers(true),
179 m_enableIntermediateScatterer(true)
180 {
181 
182 }
183 
185 {
186  milleFile = new MilleBinary(m_milleFileName);
187 
188  #ifdef OUTPUT
189  diag = new TFile(rootFileName.c_str(), "RECREATE");
190  char name[20];
191 
192  for (int i = 0; i < 12; i++) {
193  sprintf(name, "res_u_%i", i + 1);
194  resHistosU[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
195  sprintf(name, "res_v_%i", i + 1);
196  resHistosV[i] = new TH1F(name, "Residual (V)", 1000, -0.1, 0.1);
197  sprintf(name, "meas_pull_u_%i", i + 1);
198  mhistosU[i] = new TH1F(name, "Res/Meas.Err. (U)", 1000, -20., 20.);
199  sprintf(name, "meas_pull_v_%i", i + 1);
200  mhistosV[i] = new TH1F(name, "Res/Meas.Err. (V)", 1000, -20., 20.);
201  sprintf(name, "pull_u_%i", i + 1);
202  ghistosU[i] = new TH1F(name, "Res/Res.Err. (U)", 1000, -20., 20.);
203  sprintf(name, "pull_v_%i", i + 1);
204  ghistosV[i] = new TH1F(name, "Res/Res.Err. (V)", 1000, -20., 20.);
205  sprintf(name, "downWeights_u_%i", i + 1);
206  downWeightsHistosU[i] = new TH1F(name, "Down-weights (U)", 1000, 0., 1.);
207  sprintf(name, "downWeights_v_%i", i + 1);
208  downWeightsHistosV[i] = new TH1F(name, "Down-weights (V)", 1000, 0., 1.);
209  sprintf(name, "localPar1_%i", i + 1);
210 
211  localPar1[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
212  sprintf(name, "localPar2_%i", i + 1);
213  localPar2[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
214  sprintf(name, "localPar3_%i", i + 1);
215  localPar3[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
216  sprintf(name, "localPar4_%i", i + 1);
217  localPar4[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
218  sprintf(name, "localPar5_%i", i + 1);
219  localPar5[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
220  }
221  fitResHisto = new TH1I("fit_result", "GBL Fit Result", 21, -1, 20);
222  ndfHisto = new TH1I("ndf", "GBL Track NDF", 41, -1, 40);
223  chi2OndfHisto = new TH1F("chi2_ndf", "Track Chi2/NDF", 100, 0., 10.);
224  pValueHisto = new TH1F("p_value", "Track P-value", 100, 0., 1.);
225 
226  stats = new TH1I("stats", "0: NDF>0 | 1: fTel&VXD | 2: all | 3: VXD | 4: SVD | 5: all - PXD | 6: fTel&SVD | 7: bTel", 10, 0, 10);
227 
228  #endif
229 }
230 
232 {
233  #ifdef OUTPUT
234  diag->cd();
235  diag->Write();
236  diag->Close();
237  #endif
238  // This is needed to close the file before alignment starts
239  if (milleFile)
240  delete milleFile;
241 }
242 
263 void getScattererFromMatList(double& length, double& theta, double& s, double& ds, const double p, const double mass, const double charge, const std::vector<MatStep>& steps)
264 {
265  theta = 0.; s = 0.; ds = 0.;
266  if (steps.empty()) return;
267 
268  // sum of step lengths
269  double len = 0.;
270  // normalization
271  double sumxx = 0.;
272  // first moment (non-normalized)
273  double sumx2x2 = 0.;
274  // (part of) second moment / variance (non-normalized)
275  double sumx3x3 = 0.;
276 
277  // cppcheck-suppress unreadVariable
278  double xmin = 0.;
279  double xmax = 0.;
280 
281  for (unsigned int i = 0; i < steps.size(); i++) {
282  const MatStep step = steps.at(i);
283  // inverse of material radiation length ... (in 1/cm) ... "density of scattering"
284  double rho = 1. / step.material_.radiationLength;
285  len += fabs(step.stepSize_);
286  xmin = xmax;
287  xmax = xmin + fabs(step.stepSize_);
288  // Compute integrals
289 
290  // integral of rho(x)
291  sumxx += rho * (xmax - xmin);
292  // integral of x*rho(x)
293  sumx2x2 += rho * (xmax * xmax - xmin * xmin) / 2.;
294  // integral of x*x*rho(x)
295  sumx3x3 += rho * (xmax * xmax * xmax - xmin * xmin * xmin) / 3.;
296  }
297  // This ensures PDG formula still gives positive results (but sumxx should be > 1e-4 for it to hold)
298  if (sumxx < 1.0e-10) return;
299  // Calculate theta from total sum of radiation length
300  // instead of summimg squares of individual deflection angle variances
301  // PDG formula:
302  double beta = p / sqrt(p * p + mass * mass);
303  theta = (0.0136 / p / beta) * fabs(charge) * sqrt(sumxx) * (1. + 0.038 * log(sumxx));
304  //theta = (0.015 / p / beta) * fabs(charge) * sqrt(sumxx);
305 
306  // track length
307  length = len;
308  // Normalization factor
309  double N = 1. / sumxx;
310  // First moment
311  s = N * sumx2x2;
312  // Square of second moment (variance)
313  // integral of (x - s)*(x - s)*rho(x)
314  double ds_2 = N * (sumx3x3 - 2. * sumx2x2 * s + sumxx * s * s);
315  ds = sqrt(ds_2);
316 
317  #ifdef DEBUG
318  //cout << "Thick scatterer parameters:" << endl;
319  //cout << "Variance of theta: " << theta << endl;
320  //cout << "Mean s : " << s << endl;
321  //cout << "Variance of s : " << ds << endl;
322 
323  #endif
324 }
325 
326 
327 void GFGbl::processTrackWithRep(Track* trk, const AbsTrackRep* rep, bool /*resortHits*/)
328 {
329  // Flag used to mark error in raw measurement combination
330  // measurement won't be considered, but scattering yes
331  bool skipMeasurement = false;
332  // Chi2 of Reference Track
333  double trkChi2 = 0.;
334  // This flag enables/disables fitting of q/p parameter in GBL
335  // It is switched off automatically if no B-field at (0,0,0) is detected.
336  bool fitQoverP = true;
337  //TODO: Use clever way to determine zero B-field
338  double Bfield = genfit::FieldManager::getInstance()->getFieldVal(TVector3(0., 0., 0.)).Mag();
339  if (!(Bfield > 0.))
340  fitQoverP = false;
341 
342  // Dimesion of repository/state
343  int dim = rep->getDim();
344  // current measurement point
345  TrackPoint* point_meas;
346  // current raw measurement
347  AbsMeasurement* raw_meas;
348 
349  // We assume no initial kinks, this will be reused several times
350  TVectorD scatResidual(2);
351  scatResidual.Zero();
352 
353  // All measurement points in ref. track
354  int npoints_meas = trk->getNumPointsWithMeasurement();
355 
356  #ifdef DEBUG
357  int npoints_all = trk->getNumPoints();
358 
359  if (resortHits)
360  cout << "WARNING: Hits resorting in GBL interface not supported." << endl;
361 
362  cout << "-------------------------------------------------------" << endl;
363  cout << " GBL processing genfit::Track " << endl;
364  cout << "-------------------------------------------------------" << endl;
365  cout << " # Ref. Track Points : " << npoints_all << endl;
366  cout << " # Meas. Points : " << npoints_meas << endl;
367 
368  #endif
369  // List of prepared GBL points for GBL trajectory construction
370  std::vector<GblPoint> listOfPoints;
371 
372  std::vector<double> listOfLayers;
373  //TODO: Add internal/external seed (from CDC) option in the future
374  // index of point with seed information (0 for none)
375  unsigned int seedLabel = 0;
376  // Seed covariance
377  // TMatrixDSym clCov(dim);
378  // Seed state
379  TMatrixDSym clSeed(dim);
380 
381  // propagation Jacobian to next point from current measurement point
382  TMatrixD jacPointToPoint(dim, dim);
383  jacPointToPoint.UnitMatrix();
384 
385  int n_gbl_points = 0;
386  int n_gbl_meas_points = 0;
387  int Ndf = 0;
388  double Chi2 = 0.;
389  double lostWeight = 0.;
390 
391  // Momentum of track at current plane
392  double trackMomMag = 0.;
393  // Charge of particle at current plane :-)
394  double particleCharge = 1.;
395 
396  for (int ipoint_meas = 0; ipoint_meas < npoints_meas; ipoint_meas++) {
397  point_meas = trk->getPointWithMeasurement(ipoint_meas);
398 
399  if (!point_meas->hasFitterInfo(rep)) {
400  cout << " ERROR: Measurement point does not have a fitter info. Track will be skipped." << endl;
401  return;
402  }
403  // Fitter info which contains Reference state and plane
404  KalmanFitterInfo* fi = dynamic_cast<KalmanFitterInfo*>(point_meas->getFitterInfo(rep));
405  if (!fi) {
406  cout << " ERROR: KalmanFitterInfo (with reference state) for measurement does not exist. Track will be skipped." << endl;
407  return;
408  }
409  // Current detector plane
410  SharedPlanePtr plane = fi->getPlane();
411  if (!fi->hasReferenceState()) {
412  cout << " ERROR: Fitter info does not contain reference state. Track will be skipped." << endl;
413  return;
414  }
415  // Reference StateOnPlane for extrapolation
416  ReferenceStateOnPlane* reference = new ReferenceStateOnPlane(*fi->getReferenceState());//(dynamic_cast<const genfit::ReferenceStateOnPlane&>(*fi->getReferenceState()));
417  // Representation state at plane
418  TVectorD state = reference->getState();
419  // track direction at plane (in global coords)
420  TVector3 trackDir = rep->getDir(*reference);
421  // track momentum vector at plane (in global coords)
422  trackMomMag = rep->getMomMag(*reference);
423  // charge of particle
424  particleCharge = rep->getCharge(*reference);
425  // mass of particle
426  double particleMass = rep->getMass(*reference);
427 
428  // Parameters of a thick scatterer between measurements
429  double trackLen = 0.;
430  double scatTheta = 0.;
431  double scatSMean = 0.;
432  double scatDeltaS = 0.;
433  // Parameters of two equivalent thin scatterers
434  double theta1 = 0.;
435  double theta2 = 0.;
436  double s1 = 0.;
437  double s2 = 0.;
438 
439  TMatrixDSym noise;
440  TVectorD deltaState;
441  // jacobian from s2 to M2
442  TMatrixD jacMeas2Scat(dim, dim);
443  jacMeas2Scat.UnitMatrix();
444 
445 
446  // Now get measurement. First have a look if we need to combine SVD clusters...
447  // Load Jacobian from previous extrapolation
448  // rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
449  // Try to get VxdId of current plane
450  int sensorId = 0;
451  PlanarMeasurement* measPlanar = dynamic_cast<PlanarMeasurement*>(point_meas->getRawMeasurement(0));
452  if (measPlanar) sensorId = measPlanar->getPlaneId();
453 
454  //WARNING: Now we only support 2D measurements. If 2 raw measurements are stored at the track
455  // point, these are checked if they correspond to "u" and "v" measurement (for SVD clusters) and these
456  // measurements are combined. SLANTED SENSORS NOT YET SUPPORTED!!!
457  if (point_meas->getRawMeasurement(0)->getDim() != 2
458  && trk->getPointWithMeasurement(ipoint_meas)->getNumRawMeasurements() == 2
459  && point_meas->getRawMeasurement(0)->getDim() == 1
460  && point_meas->getRawMeasurement(1)->getDim() == 1) {
461  AbsMeasurement* raw_measU = 0;
462  AbsMeasurement* raw_measV = 0;
463 
464  // cout << " Two 1D Measurements encountered. " << endl;
465 
466  int sensorId2 = -1;
467  PlanarMeasurement* measPlanar2 = dynamic_cast<PlanarMeasurement*>(point_meas->getRawMeasurement(0));
468  if (measPlanar2) sensorId2 = measPlanar->getPlaneId();
469 
470  // We only try to combine if at same sensor id (should be always, but who knows)
471  // otherwise ignore this point
472  if (sensorId != sensorId2) {
473  skipMeasurement = true;
474  cout << " ERROR: Incompatible sensorIDs at measurement point " << ipoint_meas << ". Measurement will be skipped." << endl;
475  }
476 
477  // We have to combine two SVD 1D Clusters at the same plane into one 2D recohit
478  AbsMeasurement* raw_meas1 = point_meas->getRawMeasurement(0);
479  AbsMeasurement* raw_meas2 = point_meas->getRawMeasurement(1);
480  // Decide which cluster is u and which v based on H-matrix
481  if (raw_meas1->constructHMatrix(rep)->isEqual(genfit::HMatrixU())
482  && raw_meas2->constructHMatrix(rep)->isEqual(genfit::HMatrixV())) {
483  // right order U, V
484  raw_measU = raw_meas1;
485  raw_measV = raw_meas2;
486  } else if (raw_meas2->constructHMatrix(rep)->isEqual(genfit::HMatrixU())
487  && raw_meas1->constructHMatrix(rep)->isEqual(genfit::HMatrixV())) {
488  // inversed order V, U
489  raw_measU = raw_meas2;
490  raw_measV = raw_meas1;
491  } else {
492  // Incompatible measurements ... skip track ... I saw this once and just before a segfault ...
493  cout << " ERROR: Incompatible 1D measurements at meas. point " << ipoint_meas << ". Track will be skipped." << endl;
494  return;
495  }
496  // Combine raw measurements
497  TVectorD _raw_coor(2);
498  _raw_coor(0) = raw_measU->getRawHitCoords()(0);
499  _raw_coor(1) = raw_measV->getRawHitCoords()(0);
500  // Combine covariance matrix
501  TMatrixDSym _raw_cov(2);
502  _raw_cov.Zero();
503  _raw_cov(0, 0) = raw_measU->getRawHitCov()(0, 0);
504  _raw_cov(1, 1) = raw_measV->getRawHitCov()(0, 0);
505  // Create new combined measurement
506  raw_meas = new PlanarMeasurement(_raw_coor, _raw_cov, raw_measU->getDetId(), raw_measU->getHitId(), point_meas);
507  } else {
508  // Default behavior
509  raw_meas = point_meas->getRawMeasurement(0);
510  }
511  //TODO: We only support 2D measurements in GBL (ot two 1D combined above)
512  if (raw_meas->getRawHitCoords().GetNoElements() != 2) {
513  skipMeasurement = true;
514  #ifdef DEBUG
515  cout << " WARNING: Measurement " << (ipoint_meas + 1) << " is not 2D. Measurement Will be skipped. " << endl;
516  #endif
517  }
518 
519  // Now we have all necessary information, so lets insert current measurement point
520  // if we don't want to skip it (e.g. ghost SVD hit ... just 1D information)
521  if (!skipMeasurement) {
522  // 2D hit coordinates
523  TVectorD raw_coor = raw_meas->getRawHitCoords();
524  // Covariance matrix of measurement
525  TMatrixDSym raw_cov = raw_meas->getRawHitCov();
526  // Projection matrix from repository state to measurement coords
527  std::unique_ptr<const AbsHMatrix> HitHMatrix(raw_meas->constructHMatrix(rep));
528  // Residual between measured position and reference track position
529  TVectorD residual = -1. * (raw_coor - HitHMatrix->Hv(state));
530 
531  trkChi2 += residual(0) * residual(0) / raw_cov(0, 0) + residual(1) * residual(1) / raw_cov(1, 1);
532 
533  // Measurement point
534  GblPoint measPoint(jacPointToPoint);
535  // Projection from local (state) coordinates to measurement coordinates (inverted)
536  // 2x2 matrix ... last block of H matrix (2 rows x 5 columns)
537  //TMatrixD proL2m = HitHMatrix->getMatrix().GetSub(0, 1, 3, 4);
538  TMatrixD proL2m(2, 2);
539  proL2m.Zero();
540  proL2m(0, 0) = HitHMatrix->getMatrix()(0, 3);
541  proL2m(0, 1) = HitHMatrix->getMatrix()(0, 4);
542  proL2m(1, 1) = HitHMatrix->getMatrix()(1, 4);
543  proL2m(1, 0) = HitHMatrix->getMatrix()(1, 3);
544  proL2m.Invert();
545  //raw_cov *= 100.;
546  //proL2m.Print();
547  measPoint.addMeasurement(proL2m, residual, raw_cov.Invert());
548 
549  //Add global derivatives to the point
550 
551  // sensor label = sensorID * 10, then last digit is label for global derivative for the sensor
552  int label = sensorId * 10;
553  // values for global derivatives
554  //TMatrixD derGlobal(2, 6);
555  TMatrixD derGlobal(2, 12);
556 
557  // labels for global derivatives
558  std::vector<int> labGlobal;
559 
560  // track direction in global coords
561  TVector3 tDir = trackDir;
562  // sensor u direction in global coords
563  TVector3 uDir = plane->getU();
564  // sensor v direction in global coords
565  TVector3 vDir = plane->getV();
566  // sensor normal direction in global coords
567  TVector3 nDir = plane->getNormal();
568  //file << sensorId << endl;
569  //outputVector(uDir, "U");
570  //outputVector(vDir, "V");
571  //outputVector(nDir, "Normal");
572  // track direction in local sensor system
573  TVector3 tLoc = TVector3(uDir.Dot(tDir), vDir.Dot(tDir), nDir.Dot(tDir));
574 
575  // track u-slope in local sensor system
576  double uSlope = tLoc[0] / tLoc[2];
577  // track v-slope in local sensor system
578  double vSlope = tLoc[1] / tLoc[2];
579 
580  // Measured track u-position in local sensor system
581  double uPos = raw_coor[0];
582  // Measured track v-position in local sensor system
583  double vPos = raw_coor[1];
584 
585  //Global derivatives for alignment in sensor local coordinates
586  derGlobal(0, 0) = 1.0;
587  derGlobal(0, 1) = 0.0;
588  derGlobal(0, 2) = - uSlope;
589  derGlobal(0, 3) = vPos * uSlope;
590  derGlobal(0, 4) = -uPos * uSlope;
591  derGlobal(0, 5) = vPos;
592 
593  derGlobal(1, 0) = 0.0;
594  derGlobal(1, 1) = 1.0;
595  derGlobal(1, 2) = - vSlope;
596  derGlobal(1, 3) = vPos * vSlope;
597  derGlobal(1, 4) = -uPos * vSlope;
598  derGlobal(1, 5) = -uPos;
599 
600  labGlobal.push_back(label + 1); // u
601  labGlobal.push_back(label + 2); // v
602  labGlobal.push_back(label + 3); // w
603  labGlobal.push_back(label + 4); // alpha
604  labGlobal.push_back(label + 5); // beta
605  labGlobal.push_back(label + 6); // gamma
606 
607  // Global derivatives for movement of whole detector system in global coordinates
608  //TODO: Usage of this requires Hierarchy Constraints to be provided to MP2
609 
610  // sensor centre position in global system
611  TVector3 detPos = plane->getO();
612  //cout << "detPos" << endl;
613  //detPos.Print();
614 
615  // global prediction from raw measurement
616  TVector3 pred = detPos + uPos * uDir + vPos * vDir;
617  //cout << "pred" << endl;
618  //pred.Print();
619 
620  double xPred = pred[0];
621  double yPred = pred[1];
622  double zPred = pred[2];
623 
624  // scalar product of sensor normal and track direction
625  double tn = tDir.Dot(nDir);
626  //cout << "tn" << endl;
627  //cout << tn << endl;
628 
629  // derivatives of local residuals versus measurements
630  TMatrixD drdm(3, 3);
631  drdm.UnitMatrix();
632  for (int row = 0; row < 3; row++)
633  for (int col = 0; col < 3; col++)
634  drdm(row, col) -= tDir[row] * nDir[col] / tn;
635 
636  //cout << "drdm" << endl;
637  //drdm.Print();
638 
639  // derivatives of measurements versus global alignment parameters
640  TMatrixD dmdg(3, 6);
641  dmdg.Zero();
642  dmdg(0, 0) = 1.; dmdg(0, 4) = -zPred; dmdg(0, 5) = yPred;
643  dmdg(1, 1) = 1.; dmdg(1, 3) = zPred; dmdg(1, 5) = -xPred;
644  dmdg(2, 2) = 1.; dmdg(2, 3) = -yPred; dmdg(2, 4) = xPred;
645 
646  //cout << "dmdg" << endl;
647  //dmdg.Print();
648 
649  // derivatives of local residuals versus global alignment parameters
650  TMatrixD drldrg(3, 3);
651  drldrg.Zero();
652  drldrg(0, 0) = uDir[0]; drldrg(0, 1) = uDir[1]; drldrg(0, 2) = uDir[2];
653  drldrg(1, 0) = vDir[0]; drldrg(1, 1) = vDir[1]; drldrg(1, 2) = vDir[2];
654 
655  //cout << "drldrg" << endl;
656  //drldrg.Print();
657 
658  //cout << "drdm * dmdg" << endl;
659  //(drdm * dmdg).Print();
660 
661  // derivatives of local residuals versus rigid body parameters
662  TMatrixD drldg(3, 6);
663  drldg = drldrg * (drdm * dmdg);
664 
665  //cout << "drldg" << endl;
666  //drldg.Print();
667 
668  // offset to determine labels for sensor sets or individual layers
669  // 0: PXD, TODO 1: SVD, or individual layers
670  // offset 0 is for alignment of whole setup
671  int offset = 0;
672  //if (sensorId > 16704) offset = 20; // svd, but needs to introduce new 6 constraints: sum rot&shifts of pxd&svd = 0
673 
674  derGlobal(0, 6) = drldg(0, 0); labGlobal.push_back(offset + 1);
675  derGlobal(0, 7) = drldg(0, 1); labGlobal.push_back(offset + 2);
676  derGlobal(0, 8) = drldg(0, 2); labGlobal.push_back(offset + 3);
677  derGlobal(0, 9) = drldg(0, 3); labGlobal.push_back(offset + 4);
678  derGlobal(0, 10) = drldg(0, 4); labGlobal.push_back(offset + 5);
679  derGlobal(0, 11) = drldg(0, 5); labGlobal.push_back(offset + 6);
680 
681  derGlobal(1, 6) = drldg(1, 0);
682  derGlobal(1, 7) = drldg(1, 1);
683  derGlobal(1, 8) = drldg(1, 2);
684  derGlobal(1, 9) = drldg(1, 3);
685  derGlobal(1, 10) = drldg(1, 4);
686  derGlobal(1, 11) = drldg(1, 5);
687 
688 
689 
690  measPoint.addGlobals(labGlobal, derGlobal);
691  listOfPoints.push_back(measPoint);
692  listOfLayers.push_back((unsigned int) sensorId);
693  n_gbl_points++;
694  n_gbl_meas_points++;
695  } else {
696  // Incompatible measurement, store point without measurement
697  GblPoint dummyPoint(jacPointToPoint);
698  listOfPoints.push_back(dummyPoint);
699  listOfLayers.push_back((unsigned int) sensorId);
700  n_gbl_points++;
701  skipMeasurement = false;
702  #ifdef DEBUG
703  cout << " Dummy point inserted. " << endl;
704  #endif
705  }
706 
707 
708  //cout << " Starting extrapolation..." << endl;
709  try {
710 
711  // Extrapolate to next measurement to get material distribution
712  if (ipoint_meas < npoints_meas - 1) {
713  // Check if fitter info is in place
714  if (!trk->getPoint(ipoint_meas + 1)->hasFitterInfo(rep)) {
715  cout << " ERROR: Measurement point does not have a fitter info. Track will be skipped." << endl;
716  return;
717  }
718  // Fitter of next point info which is only used now to get the plane
719  KalmanFitterInfo* fi_i_plus_1 = dynamic_cast<KalmanFitterInfo*>(trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep));
720  if (!fi_i_plus_1) {
721  cout << " ERROR: KalmanFitterInfo (with reference state) for measurement does not exist. Track will be skipped." << endl;
722  return;
723  }
724  StateOnPlane refCopy(*reference);
725  // Extrap to point + 1, do NOT stop at boundary
726  rep->extrapolateToPlane(refCopy, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, false);
727  getScattererFromMatList(trackLen,
728  scatTheta,
729  scatSMean,
730  scatDeltaS,
731  trackMomMag,
732  particleMass,
733  particleCharge,
734  rep->getSteps());
735  // Now calculate positions and scattering variance for equivalent scatterers
736  // (Solution from Claus Kleinwort (DESY))
737  s1 = 0.;
738  s2 = scatSMean + scatDeltaS * scatDeltaS / (scatSMean - s1);
739  theta1 = sqrt(scatTheta * scatTheta * scatDeltaS * scatDeltaS / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
740  theta2 = sqrt(scatTheta * scatTheta * (scatSMean - s1) * (scatSMean - s1) / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
741 
743  theta1 = scatTheta;
744  theta2 = 0;
745  } else if (!m_enableScatterers) {
746  theta1 = 0.;
747  theta2 = 0.;
748  }
749 
750  if (s2 < 1.e-4 || s2 >= trackLen - 1.e-4 || s2 <= 1.e-4) {
751  cout << " WARNING: GBL points will be too close. GBLTrajectory construction might fail. Let's try it..." << endl;
752  }
753 
754  }
755  // Return back to state on current plane
756  delete reference;
757  reference = new ReferenceStateOnPlane(*fi->getReferenceState());
758 
759  // If not last measurement, extrapolate and get jacobians for scattering points between this and next measurement
760  if (ipoint_meas < npoints_meas - 1) {
761  if (theta2 > scatEpsilon) {
762  // First scatterer will be placed at current measurement point (see bellow)
763 
764  // theta2 > 0 ... we want second scatterer:
765  // Extrapolate to s2 (remember we have s1 = 0)
766  rep->extrapolateBy(*reference, s2, false, true);
767  rep->getForwardJacobianAndNoise(jacMeas2Scat, noise, deltaState);
768  // Finish extrapolation to next measurement
769  double nextStep = rep->extrapolateToPlane(*reference, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, true);
770  if (0. > nextStep) {
771  cout << " ERROR: The extrapolation to measurement point " << (ipoint_meas + 2) << " stepped back by " << nextStep << "cm !!! Track will be skipped." << endl;
772  return;
773  }
774  rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
775 
776  } else {
777  // No scattering: extrapolate whole distance between measurements
778  rep->extrapolateToPlane(*reference, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, true);
779  //NOTE: we will load the jacobian from this extrapolation in next loop into measurement point
780  //jacPointToPoint.Print();
781  rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
782  //jacPointToPoint.Print();
783  }
784  }
785  } catch (...) {
786  cout << " ERROR: Extrapolation failed. Track will be skipped." << endl;
787  return;
788  }
789  //cout << " Extrapolation finished." << endl;
790 
791 
792  // Now store scatterers if not last measurement and if we decided
793  // there should be scatteres, otherwise the jacobian in measurement
794  // stored above is already correct
795  if (ipoint_meas < npoints_meas - 1) {
796 
797  if (theta1 > scatEpsilon) {
798  // We have to insert first scatterer at measurement point
799  // Therefore (because state is perpendicular to plane, NOT track)
800  // we have non-diaonal matrix of multiple scattering covariance
801  // We have to project scattering into plane coordinates
802  double c1 = trackDir.Dot(plane->getU());
803  double c2 = trackDir.Dot(plane->getV());
804  TMatrixDSym scatCov(2);
805  scatCov(0, 0) = 1. - c2 * c2;
806  scatCov(1, 1) = 1. - c1 * c1;
807  scatCov(0, 1) = c1 * c2;
808  scatCov(1, 0) = c1 * c2;
809  scatCov *= theta1 * theta1 / (1. - c1 * c1 - c2 * c2) / (1. - c1 * c1 - c2 * c2) ;
810 
811  // last point is the just inserted measurement (or dummy point)
812  GblPoint& lastPoint = listOfPoints.at(n_gbl_points - 1);
813  lastPoint.addScatterer(scatResidual, scatCov.Invert());
814 
815  }
816 
817  if (theta2 > scatEpsilon) {
818  // second scatterer is somewhere between measurements
819  // TrackRep state is perpendicular to track direction if using extrapolateBy (I asked Johannes Rauch),
820  // therefore scattering covariance is diagonal (and both elements are equal)
821  TMatrixDSym scatCov(2);
822  scatCov.Zero();
823  scatCov(0, 0) = theta2 * theta2;
824  scatCov(1, 1) = theta2 * theta2;
825 
826  GblPoint scatPoint(jacMeas2Scat);
827  scatPoint.addScatterer(scatResidual, scatCov.Invert());
828  listOfPoints.push_back(scatPoint);
829  listOfLayers.push_back(((unsigned int) sensorId) + 0.5);
830  n_gbl_points++;
831  }
832 
833 
834  }
835  // Free memory on the heap
836  delete reference;
837  }
838  // We should have at least two measurement points to fit anything
839  if (n_gbl_meas_points > 1) {
840  int fitRes = -1;
841  double pvalue = 0.;
842  GblTrajectory* traj = 0;
843  try {
844  // Construct the GBL trajectory, seed not used
845  traj = new GblTrajectory(listOfPoints, seedLabel, clSeed, fitQoverP);
846  // Fit the trajectory
847  fitRes = traj->fit(Chi2, Ndf, lostWeight, m_gblInternalIterations);
848  if (fitRes != 0) {
849  //#ifdef DEBUG
850  //traj->printTrajectory(100);
851  //traj->printData();
852  //traj->printPoints(100);
853  //#endif
854  }
855  } catch (...) {
856  // Gbl failed critically (usually GblPoint::getDerivatives ... singular matrix inversion)
857  return;
858  }
859 
860  pvalue = TMath::Prob(Chi2, Ndf);
861 
862  //traj->printTrajectory(100);
863  //traj->printData();
864  //traj->printPoints(100);
865 
866  #ifdef OUTPUT
867  // Fill histogram with fit result
868  fitResHisto->Fill(fitRes);
869  ndfHisto->Fill(Ndf);
870  #endif
871 
872  #ifdef DEBUG
873  cout << " Ref. Track Chi2 : " << trkChi2 << endl;
874  cout << " Ref. end momentum : " << trackMomMag << " GeV/c ";
875  if (abs(trk->getCardinalRep()->getPDG()) == 11) {
876  if (particleCharge < 0.)
877  cout << "(electron)";
878  else
879  cout << "(positron)";
880  }
881  cout << endl;
882  cout << "------------------ GBL Fit Results --------------------" << endl;
883  cout << " Fit q/p parameter : " << ((fitQoverP) ? ("True") : ("False")) << endl;
884  cout << " Valid trajectory : " << ((traj->isValid()) ? ("True") : ("False")) << endl;
885  cout << " Fit result : " << fitRes << " (0 for success)" << endl;
886  cout << " # GBL meas. points : " << n_gbl_meas_points << endl;
887  cout << " # GBL all points : " << n_gbl_points << endl;
888  cout << " GBL track NDF : " << Ndf << " (-1 for failure)" << endl;
889  cout << " GBL track Chi2 : " << Chi2 << endl;
890  cout << " GBL track P-value : " << pvalue;
891  if (pvalue < m_pValueCut)
892  cout << " < p-value cut " << m_pValueCut;
893  cout << endl;
894  cout << "-------------------------------------------------------" << endl;
895  #endif
896 
897  #ifdef OUTPUT
898  bool hittedLayers[12];
899  for (int hl = 0; hl < 12; hl++) {
900  hittedLayers[hl] = false;
901  }
902  #endif
903 
904  // GBL fit succeded if Ndf >= 0, but Ndf = 0 is useless
905  //TODO: Here should be some track quality check
906  // if (Ndf > 0 && fitRes == 0) {
907  if (traj->isValid() && pvalue >= m_pValueCut && Ndf >= m_minNdf) {
908 
909  // In case someone forgot to use beginRun and dind't reset mille file name to ""
910  if (!milleFile && m_milleFileName != "")
911  milleFile = new MilleBinary(m_milleFileName);
912 
913  // Loop over all GBL points
914  for (unsigned int p = 0; p < listOfPoints.size(); p++) {
915  unsigned int label = p + 1;
916  unsigned int numRes;
917  TVectorD residuals(2);
918  TVectorD measErrors(2);
919  TVectorD resErrors(2);
920  TVectorD downWeights(2);
921  //TODO: now we only provide info about measurements, not kinks
922  if (!listOfPoints.at(p).hasMeasurement())
923  continue;
924 
925  #ifdef OUTPUT
926  // Decode VxdId and get layer in TB setup
927  unsigned int l = 0;
928  unsigned int id = listOfLayers.at(p);
929  // skip segment (5 bits)
930  id = id >> 5;
931  unsigned int sensor = id & 7;
932  id = id >> 3;
933  unsigned int ladder = id & 31;
934  id = id >> 5;
935  unsigned int layer = id & 7;
936 
937  if (layer == 7 && ladder == 2) {
938  l = sensor;
939  } else if (layer == 7 && ladder == 3) {
940  l = sensor + 9 - 3;
941  } else {
942  l = layer + 3;
943  }
944 
945  hittedLayers[l - 1] = true;
946  #endif
947  TVectorD localPar(5);
948  TMatrixDSym localCov(5);
949  traj->getResults(label, localPar, localCov);
950  // Get GBL fit results
951  traj->getMeasResults(label, numRes, residuals, measErrors, resErrors, downWeights);
952  if (m_chi2Cut && (fabs(residuals[0] / resErrors[0]) > m_chi2Cut || fabs(residuals[1] / resErrors[1]) > m_chi2Cut))
953  return;
954  // Write layer-wise data
955  #ifdef OUTPUT
956  if (!writeHistoDataForLabel(listOfLayers.at(p), residuals, measErrors, resErrors, downWeights, localPar, localCov))
957  return;
958  #endif
959 
960  } // end for points
961 
962  // Write binary data to mille binary
963  if (milleFile && m_milleFileName != "" && pvalue >= m_pValueCut && Ndf >= m_minNdf) {
964  traj->milleOut(*milleFile);
965  #ifdef DEBUG
966  cout << " GBL Track written to Millepede II binary file." << endl;
967  cout << "-------------------------------------------------------" << endl;
968  #endif
969  }
970 
971  #ifdef OUTPUT
972  // Fill histograms
973  chi2OndfHisto->Fill(Chi2 / Ndf);
974  pValueHisto->Fill(TMath::Prob(Chi2, Ndf));
975  // track counting
976  stats->Fill(0);
977  // hitted sensors statistics
978  if (
979  hittedLayers[0] &&
980  hittedLayers[1] &&
981  hittedLayers[2] &&
982  hittedLayers[3] &&
983  hittedLayers[4] &&
984  hittedLayers[5] &&
985  hittedLayers[6] &&
986  hittedLayers[7] &&
987  hittedLayers[8]
988  ) {
989  // front tel + pxd + svd
990  stats->Fill(1);
991  }
992 
993  if (
994  hittedLayers[0] &&
995  hittedLayers[1] &&
996  hittedLayers[2] &&
997  hittedLayers[3] &&
998  hittedLayers[4] &&
999  hittedLayers[5] &&
1000  hittedLayers[6] &&
1001  hittedLayers[7] &&
1002  hittedLayers[8] &&
1003  hittedLayers[9] &&
1004  hittedLayers[10] &&
1005  hittedLayers[11]
1006  ) {
1007  // all layers
1008  stats->Fill(2);
1009  }
1010  if (
1011  hittedLayers[3] &&
1012  hittedLayers[4] &&
1013  hittedLayers[5] &&
1014  hittedLayers[6] &&
1015  hittedLayers[7] &&
1016  hittedLayers[8]
1017  ) {
1018  // vxd
1019  stats->Fill(3);
1020  }
1021  if (
1022  hittedLayers[5] &&
1023  hittedLayers[6] &&
1024  hittedLayers[7] &&
1025  hittedLayers[8]
1026  ) {
1027  // svd
1028  stats->Fill(4);
1029  }
1030  if (
1031  hittedLayers[0] &&
1032  hittedLayers[1] &&
1033  hittedLayers[2] &&
1034 
1035  hittedLayers[5] &&
1036  hittedLayers[6] &&
1037  hittedLayers[7] &&
1038  hittedLayers[8] &&
1039  hittedLayers[9] &&
1040  hittedLayers[10] &&
1041  hittedLayers[11]
1042  ) {
1043  // all except pxd
1044  stats->Fill(5);
1045  }
1046  if (
1047  hittedLayers[0] &&
1048  hittedLayers[1] &&
1049  hittedLayers[2] &&
1050 
1051  hittedLayers[5] &&
1052  hittedLayers[6] &&
1053  hittedLayers[7] &&
1054  hittedLayers[8]
1055  ) {
1056  // front tel + svd
1057  stats->Fill(6);
1058  }
1059  if (
1060  hittedLayers[9] &&
1061  hittedLayers[10] &&
1062  hittedLayers[11]
1063  ) {
1064  // backward tel
1065  stats->Fill(7);
1066  }
1067  #endif
1068  }
1069 
1070  // Free memory
1071  delete traj;
1072  }
1073 }
1074