GFGbl.cc

//-*-mode: C++; c-basic-offset: 2; -*-
/* Copyright 2013
 *  Authors: Sergey Yashchenko and Tadeas Bilka
 *
 *  This is an interface to General Broken Lines
 *
 *  Version: 5 (Tadeas)
 *  - several bug-fixes:
 *    - Scatterers at bad points
 *    - Jacobians at a point before they should be (code reorganized)
 *    - Change of sign of residuals
 *  Version: 4 (Tadeas)
 *  Fixed calculation of equvivalent scatterers (solution by C. Kleinwort)
 *  Now a scatterer is inserted at each measurement (except last) and between each two measurements.
 *  TrueHits/Clusters. Ghost (1D) hits ignored. With or without magnetic field.
 *  Version: 3 (Tadeas)
 *  This version now supports both TrueHits and Clusters for VXD.
 *  It can be used for arbitrary material distribution between
 *  measurements. Moments of scattering distribution are computed
 *  and translated into two equivalent thin GBL scatterers placed
 *  at computed positions between measurement points.
 *  Version: 2 ... never published (Tadeas)
 *  Scatterer at each boundary (tooo many scatterers). TrueHits/Clusters. Without global der.&MP2 output.
 *  Version: 1 (Sergey & Tadeas)
 *  Scatterers at measurement planes. TrueHits
 *  Version 0: (Sergey)
 *  Without scatterers. Genfit 1.
 *
 *  This file is part of GENFIT.
 *
 *  GENFIT is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published
 *  by the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  GENFIT is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "GFGbl.h"
#include "GblTrajectory.h"
#include "GblPoint.h"
#include "MyDebugTools.h"
 
#include "AbsMeasurement.h"
#include "PlanarMeasurement.h"
#include "KalmanFitterInfo.h"
 
#include "Track.h"
#include <TFile.h>
#include <TH1F.h>
#include <TTree.h>
#include <string>
#include <list>
#include <FieldManager.h>
#include <HMatrixU.h>
#include <HMatrixV.h>
#include <Math/SMatrix.h>
#include <TMatrixD.h>
#include <TVectorDfwd.h>
#include <TMatrixT.h>
 
#include <TVector3.h>
 
//#define DEBUG
//#define OUTPUT
 
 
#ifdef DEBUG
//ofstream debug("gbl.debug");
#endif
 
#ifdef OUTPUT
 
std::string rootFileName = "gbl.root";
 
 
TFile* diag;
TH1F* resHistosU[12];
TH1F* resHistosV[12];
TH1F* mhistosU[12];
TH1F* mhistosV[12];
TH1F* ghistosU[12];
TH1F* ghistosV[12];
TH1F* downWeightsHistosU[12];
TH1F* downWeightsHistosV[12];
TH1F* localPar1[12];
TH1F* localPar2[12];
TH1F* localPar3[12];
TH1F* localPar4[12];
TH1F* localPar5[12];
TH1F* localCov1[12];
TH1F* localCov2[12];
TH1F* localCov3[12];
TH1F* localCov4[12];
TH1F* localCov5[12];
TH1F* localCov12[12];
TH1F* localCov13[12];
TH1F* localCov14[12];
TH1F* localCov15[12];
TH1F* localCov23[12];
TH1F* localCov24[12];
TH1F* localCov25[12];
TH1F* localCov34[12];
TH1F* localCov35[12];
TH1F* localCov45[12];
TH1I* fitResHisto;
TH1I* ndfHisto;
TH1F* chi2OndfHisto;
TH1F* pValueHisto;
TH1I* stats;
 
 
 
bool writeHistoDataForLabel(double label, TVectorD res, TVectorD measErr, TVectorD resErr, TVectorD downWeights, TVectorD localPar, TMatrixDSym localCov)
{
  if (label < 1.) return false;
  
  unsigned int id = floor(label);
  // skip segment (5 bits)
  id = id >> 5;
  unsigned int sensor = id & 7;
  id = id >> 3;
  unsigned int ladder = id & 31;
  id = id >> 5;
  unsigned int layer = id & 7;
  if (layer == 7 && ladder == 2) {
    label = sensor;
  } else if (layer == 7 && ladder == 3) {
    label = sensor + 9 - 3;
  } else {
    label = layer + 3;
  }
  
  if (label > 12.) return false;
  
  int i = int(label);
  
  #ifdef OUTPUT
  resHistosU[i - 1]->Fill(res[0]);
  resHistosV[i - 1]->Fill(res[1]);
  mhistosU[i - 1]->Fill(res[0] / measErr[0]);
  mhistosV[i - 1]->Fill(res[1] / measErr[1]);
  ghistosU[i - 1]->Fill(res[0] / resErr[0]);
  ghistosV[i - 1]->Fill(res[1] / resErr[1]);
  downWeightsHistosU[i - 1]->Fill(downWeights[0]);
  downWeightsHistosV[i - 1]->Fill(downWeights[1]);
  localPar1[i - 1]->Fill(localPar(0));
  localPar2[i - 1]->Fill(localPar(1));
  localPar3[i - 1]->Fill(localPar(2));
  localPar4[i - 1]->Fill(localPar(3));
  localPar5[i - 1]->Fill(localPar(4));
  #endif
  
  
  return true;
}
#endif
 
// Millepede Binary File for output of GBL trajectories for alignment
gbl::MilleBinary* milleFile;
// Minimum scattering sigma (will be squared and inverted...)
const double scatEpsilon = 1.e-8;
 
 
using namespace gbl;
using namespace std;
using namespace genfit;
 
GFGbl::GFGbl() :
AbsFitter(), m_milleFileName("millefile.dat"), m_gblInternalIterations("THC"), m_pValueCut(0.), m_minNdf(1),
m_chi2Cut(0.),
m_enableScatterers(true),
m_enableIntermediateScatterer(true)
{
  
}
 
void GFGbl::beginRun()
{
  milleFile = new MilleBinary(m_milleFileName);
  
  #ifdef OUTPUT
  diag = new TFile(rootFileName.c_str(), "RECREATE");
  char name[20];
  
  for (int i = 0; i < 12; i++) {
    sprintf(name, "res_u_%i", i + 1);
    resHistosU[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
    sprintf(name, "res_v_%i", i + 1);
    resHistosV[i] = new TH1F(name, "Residual (V)", 1000, -0.1, 0.1);
    sprintf(name, "meas_pull_u_%i", i + 1);
    mhistosU[i] = new TH1F(name, "Res/Meas.Err. (U)", 1000, -20., 20.);
    sprintf(name, "meas_pull_v_%i", i + 1);
    mhistosV[i] = new TH1F(name, "Res/Meas.Err. (V)", 1000, -20., 20.);
    sprintf(name, "pull_u_%i", i + 1);
    ghistosU[i] = new TH1F(name, "Res/Res.Err. (U)", 1000, -20., 20.);
    sprintf(name, "pull_v_%i", i + 1);
    ghistosV[i] = new TH1F(name, "Res/Res.Err. (V)", 1000, -20., 20.);
    sprintf(name, "downWeights_u_%i", i + 1);
    downWeightsHistosU[i] = new TH1F(name, "Down-weights (U)", 1000, 0., 1.);
    sprintf(name, "downWeights_v_%i", i + 1);
    downWeightsHistosV[i] = new TH1F(name, "Down-weights (V)", 1000, 0., 1.);
    sprintf(name, "localPar1_%i", i + 1);
    
    localPar1[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
    sprintf(name, "localPar2_%i", i + 1);
    localPar2[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
    sprintf(name, "localPar3_%i", i + 1);
    localPar3[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
    sprintf(name, "localPar4_%i", i + 1);
    localPar4[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
    sprintf(name, "localPar5_%i", i + 1);
    localPar5[i] = new TH1F(name, "Residual (U)", 1000, -0.1, 0.1);
  }
  fitResHisto = new TH1I("fit_result", "GBL Fit Result", 21, -1, 20);
  ndfHisto = new TH1I("ndf", "GBL Track NDF", 41, -1, 40);
  chi2OndfHisto = new TH1F("chi2_ndf", "Track Chi2/NDF", 100, 0., 10.);
  pValueHisto = new TH1F("p_value", "Track P-value", 100, 0., 1.);
  
  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);
  
  #endif
}
 
void GFGbl::endRun()
{
  #ifdef OUTPUT
  diag->cd();
  diag->Write();
  diag->Close();
  #endif
  // This is needed to close the file before alignment starts
  if (milleFile)
    delete milleFile;
}
 
void getScattererFromMatList(double& length, double& theta, double& s, double& ds, const double p, const double mass, const double charge, const std::vector<MatStep>& steps)
{
  theta = 0.; s = 0.; ds = 0.;
  if (steps.empty()) return;
  
  // sum of step lengths
  double len = 0.;
  // normalization
  double sumxx = 0.;
  // first moment (non-normalized)
  double sumx2x2 = 0.;
  // (part of) second moment / variance (non-normalized)
  double sumx3x3 = 0.;
  
  // cppcheck-suppress unreadVariable
  double xmin = 0.;
  double xmax = 0.;
  
  for (unsigned int i = 0; i < steps.size(); i++) {
    const MatStep step = steps.at(i);
    // inverse of material radiation length ... (in 1/cm) ... "density of scattering"
    double rho = 1. / step.material_.radiationLength;
    len += fabs(step.stepSize_);
    xmin = xmax;
    xmax = xmin + fabs(step.stepSize_);
    // Compute integrals
    
    // integral of rho(x)
    sumxx   += rho * (xmax - xmin);
    // integral of x*rho(x)
    sumx2x2 += rho * (xmax * xmax - xmin * xmin) / 2.;
    // integral of x*x*rho(x)
    sumx3x3 += rho * (xmax * xmax * xmax - xmin * xmin * xmin) / 3.;
  }
  // This ensures PDG formula still gives positive results (but sumxx should be > 1e-4 for it to hold)
  if (sumxx < 1.0e-10) return;
  // Calculate theta from total sum of radiation length
  // instead of summimg squares of individual deflection angle variances
  // PDG formula:
  double beta = p / sqrt(p * p + mass * mass);
  theta = (0.0136 / p / beta) * fabs(charge) * sqrt(sumxx) * (1. + 0.038 * log(sumxx));
  //theta = (0.015 / p / beta) * fabs(charge) * sqrt(sumxx);
  
  // track length
  length = len;
  // Normalization factor
  double N = 1. / sumxx;
  // First moment
  s  = N * sumx2x2;
  // Square of second moment (variance)
  // integral of (x - s)*(x - s)*rho(x)
  double ds_2 = N * (sumx3x3 - 2. * sumx2x2 * s + sumxx * s * s);
  ds = sqrt(ds_2);
  
  #ifdef DEBUG
  //cout << "Thick scatterer parameters:" << endl;
  //cout << "Variance of theta: " << theta << endl;
  //cout << "Mean s           : " << s << endl;
  //cout << "Variance of s    : " << ds << endl;
  
  #endif
}
 
 
void GFGbl::processTrackWithRep(Track* trk, const AbsTrackRep* rep, bool /*resortHits*/)
{
  // Flag used to mark error in raw measurement combination
  // measurement won't be considered, but scattering yes
  bool skipMeasurement = false;
  // Chi2 of Reference Track
  double trkChi2 = 0.;
  // This flag enables/disables fitting of q/p parameter in GBL
  // It is switched off automatically if no B-field at (0,0,0) is detected.
  bool fitQoverP = true;
  //TODO: Use clever way to determine zero B-field
  double Bfield = genfit::FieldManager::getInstance()->getFieldVal(TVector3(0., 0., 0.)).Mag();
  if (!(Bfield > 0.))
    fitQoverP = false;
  
  // Dimesion of repository/state
  int dim = rep->getDim();
  // current measurement point
  TrackPoint* point_meas;
  // current raw measurement
  AbsMeasurement* raw_meas;
  
  // We assume no initial kinks, this will be reused several times
  TVectorD scatResidual(2);
  scatResidual.Zero();
  
  // All measurement points in ref. track
  int npoints_meas = trk->getNumPointsWithMeasurement();
  
  #ifdef DEBUG
  int npoints_all = trk->getNumPoints();
  
  if (resortHits)
    cout << "WARNING: Hits resorting in GBL interface not supported." << endl;
  
  cout << "-------------------------------------------------------" << endl;
  cout << "               GBL processing genfit::Track            " << endl;
  cout << "-------------------------------------------------------" << endl;
  cout << " # Ref. Track Points  :  " << npoints_all  << endl;
  cout << " # Meas. Points       :  " << npoints_meas << endl;
  
  #endif
  // List of prepared GBL points for GBL trajectory construction
  std::vector<GblPoint> listOfPoints;
  
  std::vector<double> listOfLayers;
  //TODO: Add internal/external seed (from CDC) option in the future
  // index of point with seed information (0 for none)
  unsigned int seedLabel = 0;
  // Seed covariance
  // TMatrixDSym clCov(dim);
  // Seed state
  TMatrixDSym clSeed(dim);
  
  // propagation Jacobian to next point from current measurement point
  TMatrixD jacPointToPoint(dim, dim);
  jacPointToPoint.UnitMatrix();
  
  int n_gbl_points = 0;
  int n_gbl_meas_points = 0;
  int Ndf = 0;
  double Chi2 = 0.;
  double lostWeight = 0.;
  
  // Momentum of track at current plane
  double trackMomMag = 0.;
  // Charge of particle at current plane :-)
  double particleCharge = 1.;
  
  for (int ipoint_meas = 0; ipoint_meas < npoints_meas; ipoint_meas++) {
    point_meas = trk->getPointWithMeasurement(ipoint_meas);
    
    if (!point_meas->hasFitterInfo(rep)) {
      cout << " ERROR: Measurement point does not have a fitter info. Track will be skipped." << endl;
      return;
    }
    // Fitter info which contains Reference state and plane
    KalmanFitterInfo* fi = dynamic_cast<KalmanFitterInfo*>(point_meas->getFitterInfo(rep));
    if (!fi) {
      cout << " ERROR: KalmanFitterInfo (with reference state) for measurement does not exist. Track will be skipped." << endl;
      return;
    }
    // Current detector plane
    SharedPlanePtr plane = fi->getPlane();
    if (!fi->hasReferenceState()) {
      cout << " ERROR: Fitter info does not contain reference state. Track will be skipped." << endl;
      return;
    }
    // Reference StateOnPlane for extrapolation
    ReferenceStateOnPlane* reference = new ReferenceStateOnPlane(*fi->getReferenceState());//(dynamic_cast<const genfit::ReferenceStateOnPlane&>(*fi->getReferenceState()));
    // Representation state at plane
    TVectorD state = reference->getState();
    // track direction at plane (in global coords)
    TVector3 trackDir = rep->getDir(*reference);
    // track momentum vector at plane (in global coords)
    trackMomMag = rep->getMomMag(*reference);
    // charge of particle
    particleCharge = rep->getCharge(*reference);
    // mass of particle
    double particleMass = rep->getMass(*reference);
    
    // Parameters of a thick scatterer between measurements
    double trackLen = 0.;
    double scatTheta = 0.;
    double scatSMean = 0.;
    double scatDeltaS = 0.;
    // Parameters of two equivalent thin scatterers
    double theta1 = 0.;
    double theta2 = 0.;
    double s1 = 0.;
    double s2 = 0.;
    
    TMatrixDSym noise;
    TVectorD deltaState;
    // jacobian from s2 to M2
    TMatrixD jacMeas2Scat(dim, dim);
    jacMeas2Scat.UnitMatrix();
    
    
    // Now get measurement. First have a look if we need to combine SVD clusters...
    // Load Jacobian from previous extrapolation
    // rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
    // Try to get VxdId of current plane
    int sensorId = 0;
    PlanarMeasurement* measPlanar = dynamic_cast<PlanarMeasurement*>(point_meas->getRawMeasurement(0));
    if (measPlanar) sensorId = measPlanar->getPlaneId();
    
    //WARNING: Now we only support 2D measurements. If 2 raw measurements are stored at the track
    // point, these are checked if they correspond to "u" and "v" measurement (for SVD clusters) and these
    // measurements are combined. SLANTED SENSORS NOT YET SUPPORTED!!!
    if (point_meas->getRawMeasurement(0)->getDim() != 2
      && trk->getPointWithMeasurement(ipoint_meas)->getNumRawMeasurements() == 2
      && point_meas->getRawMeasurement(0)->getDim() == 1
      && point_meas->getRawMeasurement(1)->getDim() == 1) {
      AbsMeasurement* raw_measU = 0;
      AbsMeasurement* raw_measV = 0;
    
      // cout << " Two 1D Measurements encountered. " << endl;
    
      int sensorId2 = -1;
      PlanarMeasurement* measPlanar2 = dynamic_cast<PlanarMeasurement*>(point_meas->getRawMeasurement(0));
      if (measPlanar2) sensorId2 = measPlanar->getPlaneId();
    
      // We only try to combine if at same sensor id (should be always, but who knows)
      // otherwise ignore this point
      if (sensorId != sensorId2) {
  skipMeasurement = true;
  cout << " ERROR: Incompatible sensorIDs at measurement point " << ipoint_meas << ". Measurement will be skipped." << endl;
      }
    
      // We have to combine two SVD 1D Clusters at the same plane into one 2D recohit
      AbsMeasurement* raw_meas1 = point_meas->getRawMeasurement(0);
      AbsMeasurement* raw_meas2 = point_meas->getRawMeasurement(1);
      // Decide which cluster is u and which v based on H-matrix
      if (raw_meas1->constructHMatrix(rep)->isEqual(genfit::HMatrixU())
    && raw_meas2->constructHMatrix(rep)->isEqual(genfit::HMatrixV())) {
  // right order U, V
  raw_measU = raw_meas1;
  raw_measV = raw_meas2;
      } else if (raw_meas2->constructHMatrix(rep)->isEqual(genfit::HMatrixU())
     && raw_meas1->constructHMatrix(rep)->isEqual(genfit::HMatrixV())) {
        // inversed order V, U
        raw_measU = raw_meas2;
  raw_measV = raw_meas1;
      } else {
  // Incompatible measurements ... skip track ... I saw this once and just before a segfault ...
  cout << " ERROR: Incompatible 1D measurements at meas. point " << ipoint_meas << ". Track will be skipped." << endl;
  return;
      }
      // Combine raw measurements
      TVectorD _raw_coor(2);
      _raw_coor(0) = raw_measU->getRawHitCoords()(0);
      _raw_coor(1) = raw_measV->getRawHitCoords()(0);
      // Combine covariance matrix
      TMatrixDSym _raw_cov(2);
      _raw_cov.Zero();
      _raw_cov(0, 0) = raw_measU->getRawHitCov()(0, 0);
      _raw_cov(1, 1) = raw_measV->getRawHitCov()(0, 0);
      // Create new combined measurement
      raw_meas = new PlanarMeasurement(_raw_coor, _raw_cov, raw_measU->getDetId(), raw_measU->getHitId(), point_meas);
    } else {
      // Default behavior
      raw_meas = point_meas->getRawMeasurement(0);
    }
    //TODO: We only support 2D measurements in GBL (ot two 1D combined above)
    if (raw_meas->getRawHitCoords().GetNoElements() != 2) {
      skipMeasurement = true;
      #ifdef DEBUG
      cout << " WARNING: Measurement " << (ipoint_meas + 1) << " is not 2D. Measurement Will be skipped. " << endl;
      #endif
    }
      
    // Now we have all necessary information, so lets insert current measurement point
    // if we don't want to skip it (e.g. ghost SVD hit ... just 1D information)
    if (!skipMeasurement) {
      // 2D hit coordinates
      TVectorD raw_coor = raw_meas->getRawHitCoords();
      // Covariance matrix of measurement
      TMatrixDSym raw_cov = raw_meas->getRawHitCov();
      // Projection matrix from repository state to measurement coords
      std::unique_ptr<const AbsHMatrix> HitHMatrix(raw_meas->constructHMatrix(rep));
      // Residual between measured position and reference track position
      TVectorD residual = -1. * (raw_coor - HitHMatrix->Hv(state));
 
      trkChi2 += residual(0) * residual(0) / raw_cov(0, 0) + residual(1) * residual(1) / raw_cov(1, 1);
        
      // Measurement point
      GblPoint measPoint(jacPointToPoint);
      // Projection from local (state) coordinates to measurement coordinates (inverted)
      // 2x2 matrix ... last block of H matrix (2 rows x 5 columns)
      //TMatrixD proL2m = HitHMatrix->getMatrix().GetSub(0, 1, 3, 4);
      TMatrixD proL2m(2, 2);
      proL2m.Zero();
      proL2m(0, 0) = HitHMatrix->getMatrix()(0, 3);
      proL2m(0, 1) = HitHMatrix->getMatrix()(0, 4);
      proL2m(1, 1) = HitHMatrix->getMatrix()(1, 4);
      proL2m(1, 0) = HitHMatrix->getMatrix()(1, 3);
      proL2m.Invert();
      //raw_cov *= 100.;
      //proL2m.Print();
      measPoint.addMeasurement(proL2m, residual, raw_cov.Invert());
        
      //Add global derivatives to the point
        
      // sensor label = sensorID * 10, then last digit is label for global derivative for the sensor
      int label = sensorId * 10;
      // values for global derivatives
      //TMatrixD derGlobal(2, 6);
      TMatrixD derGlobal(2, 12);
        
      // labels for global derivatives
      std::vector<int> labGlobal;
        
      // track direction in global coords
      TVector3 tDir = trackDir;
      // sensor u direction in global coords
      TVector3 uDir = plane->getU();
      // sensor v direction in global coords
      TVector3 vDir = plane->getV();
      // sensor normal direction in global coords
      TVector3 nDir = plane->getNormal();
      //file << sensorId << endl;
      //outputVector(uDir, "U");
      //outputVector(vDir, "V");
      //outputVector(nDir, "Normal");
      // track direction in local sensor system
      TVector3 tLoc = TVector3(uDir.Dot(tDir), vDir.Dot(tDir), nDir.Dot(tDir));
        
      // track u-slope in local sensor system
      double uSlope = tLoc[0] / tLoc[2];
      // track v-slope in local sensor system
      double vSlope = tLoc[1] / tLoc[2];
        
      // Measured track u-position in local sensor system
      double uPos = raw_coor[0];
      // Measured track v-position in local sensor system
      double vPos = raw_coor[1];
        
      //Global derivatives for alignment in sensor local coordinates
      derGlobal(0, 0) = 1.0;
      derGlobal(0, 1) = 0.0;
      derGlobal(0, 2) = - uSlope;
      derGlobal(0, 3) = vPos * uSlope;
      derGlobal(0, 4) = -uPos * uSlope;
      derGlobal(0, 5) = vPos;
        
      derGlobal(1, 0) = 0.0;
      derGlobal(1, 1) = 1.0;
      derGlobal(1, 2) = - vSlope;
      derGlobal(1, 3) = vPos * vSlope;
      derGlobal(1, 4) = -uPos * vSlope;
      derGlobal(1, 5) = -uPos;
 
      labGlobal.push_back(label + 1); // u
      labGlobal.push_back(label + 2); // v
      labGlobal.push_back(label + 3); // w
      labGlobal.push_back(label + 4); // alpha
      labGlobal.push_back(label + 5); // beta
      labGlobal.push_back(label + 6); // gamma
 
      // Global derivatives for movement of whole detector system in global coordinates
      //TODO: Usage of this requires Hierarchy Constraints to be provided to MP2
  
      // sensor centre position in global system
      TVector3 detPos = plane->getO();
      //cout << "detPos" << endl;
      //detPos.Print();
 
      // global prediction from raw measurement
      TVector3 pred = detPos + uPos * uDir + vPos * vDir;
      //cout << "pred" << endl;
      //pred.Print();
 
      double xPred = pred[0];
      double yPred = pred[1];
      double zPred = pred[2];
 
      // scalar product of sensor normal and track direction
      double tn = tDir.Dot(nDir);
      //cout << "tn" << endl;
      //cout << tn << endl;
 
      // derivatives of local residuals versus measurements
      TMatrixD drdm(3, 3);
      drdm.UnitMatrix();
      for (int row = 0; row < 3; row++)
  for (int col = 0; col < 3; col++)
    drdm(row, col) -= tDir[row] * nDir[col] / tn;
 
      //cout << "drdm" << endl;
      //drdm.Print();
 
      // derivatives of measurements versus global alignment parameters
      TMatrixD dmdg(3, 6);
      dmdg.Zero();
      dmdg(0, 0) = 1.; dmdg(0, 4) = -zPred; dmdg(0, 5) = yPred;
      dmdg(1, 1) = 1.; dmdg(1, 3) = zPred;  dmdg(1, 5) = -xPred;
      dmdg(2, 2) = 1.; dmdg(2, 3) = -yPred; dmdg(2, 4) = xPred;
 
      //cout << "dmdg" << endl;
      //dmdg.Print();
 
      // derivatives of local residuals versus global alignment parameters
      TMatrixD drldrg(3, 3);
      drldrg.Zero();
      drldrg(0, 0) = uDir[0]; drldrg(0, 1) = uDir[1]; drldrg(0, 2) = uDir[2];
      drldrg(1, 0) = vDir[0]; drldrg(1, 1) = vDir[1]; drldrg(1, 2) = vDir[2];
 
      //cout << "drldrg" << endl;
      //drldrg.Print();
 
      //cout << "drdm * dmdg" << endl;
      //(drdm * dmdg).Print();
 
      // derivatives of local residuals versus rigid body parameters
      TMatrixD drldg(3, 6);
      drldg = drldrg * (drdm * dmdg);
 
      //cout << "drldg" << endl;
      //drldg.Print();
 
      // offset to determine labels for sensor sets or individual layers
      // 0: PXD, TODO 1: SVD, or individual layers
      // offset 0 is for alignment of whole setup
      int offset = 0;
      //if (sensorId > 16704) offset = 20; // svd, but needs to introduce new 6 constraints: sum rot&shifts of pxd&svd = 0
 
      derGlobal(0, 6) = drldg(0, 0); labGlobal.push_back(offset + 1);
      derGlobal(0, 7) = drldg(0, 1); labGlobal.push_back(offset + 2);
      derGlobal(0, 8) = drldg(0, 2); labGlobal.push_back(offset + 3);
      derGlobal(0, 9) = drldg(0, 3); labGlobal.push_back(offset + 4);
      derGlobal(0, 10) = drldg(0, 4); labGlobal.push_back(offset + 5);
      derGlobal(0, 11) = drldg(0, 5); labGlobal.push_back(offset + 6);
 
      derGlobal(1, 6) = drldg(1, 0);
      derGlobal(1, 7) = drldg(1, 1);
      derGlobal(1, 8) = drldg(1, 2);
      derGlobal(1, 9) = drldg(1, 3);
      derGlobal(1, 10) = drldg(1, 4);
      derGlobal(1, 11) = drldg(1, 5);
 
 
 
      measPoint.addGlobals(labGlobal, derGlobal);
      listOfPoints.push_back(measPoint);
      listOfLayers.push_back((unsigned int) sensorId);
      n_gbl_points++;
      n_gbl_meas_points++;
    } else {
      // Incompatible measurement, store point without measurement
      GblPoint dummyPoint(jacPointToPoint);
      listOfPoints.push_back(dummyPoint);
      listOfLayers.push_back((unsigned int) sensorId);
      n_gbl_points++;
      skipMeasurement = false;
      #ifdef DEBUG
      cout << " Dummy point inserted. " << endl;
      #endif
    }
 
 
    //cout << " Starting extrapolation..." << endl;
    try {
 
      // Extrapolate to next measurement to get material distribution
      if (ipoint_meas < npoints_meas - 1) {
  // Check if fitter info is in place
  if (!trk->getPoint(ipoint_meas + 1)->hasFitterInfo(rep)) {
    cout << " ERROR: Measurement point does not have a fitter info. Track will be skipped." << endl;
    return;
  }
  // Fitter of next point info which is only used now to get the plane
  KalmanFitterInfo* fi_i_plus_1 = dynamic_cast<KalmanFitterInfo*>(trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep));
  if (!fi_i_plus_1) {
    cout << " ERROR: KalmanFitterInfo (with reference state) for measurement does not exist. Track will be skipped." << endl;
    return;
  }
  StateOnPlane refCopy(*reference);
  // Extrap to point + 1, do NOT stop at boundary
  rep->extrapolateToPlane(refCopy, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, false);
  getScattererFromMatList(trackLen,
        scatTheta,
        scatSMean,
        scatDeltaS,
        trackMomMag,
        particleMass,
        particleCharge,
        rep->getSteps());
  // Now calculate positions and scattering variance for equivalent scatterers
  // (Solution from Claus Kleinwort (DESY))
  s1 = 0.;
  s2 = scatSMean + scatDeltaS * scatDeltaS / (scatSMean - s1);
  theta1 = sqrt(scatTheta * scatTheta * scatDeltaS * scatDeltaS / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
  theta2 = sqrt(scatTheta * scatTheta * (scatSMean - s1) * (scatSMean - s1) / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
 
  if (m_enableScatterers && !m_enableIntermediateScatterer) {
    theta1 = scatTheta;
    theta2 = 0;
  } else if (!m_enableScatterers) {
    theta1 = 0.;
    theta2 = 0.;
  }
 
  if (s2 < 1.e-4 || s2 >= trackLen - 1.e-4 || s2 <= 1.e-4) {
    cout << " WARNING: GBL points will be too close. GBLTrajectory construction might fail. Let's try it..." << endl;
  }
 
      }
      // Return back to state on current plane
      delete reference;
      reference = new ReferenceStateOnPlane(*fi->getReferenceState());
 
      // If not last measurement, extrapolate and get jacobians for scattering points between this and next measurement
      if (ipoint_meas < npoints_meas - 1) {
  if (theta2 > scatEpsilon) {
    // First scatterer will be placed at current measurement point (see bellow)
 
    // theta2 > 0 ... we want second scatterer:
    // Extrapolate to s2 (remember we have s1 = 0)
    rep->extrapolateBy(*reference, s2, false, true);
    rep->getForwardJacobianAndNoise(jacMeas2Scat, noise, deltaState);
    // Finish extrapolation to next measurement
    double nextStep = rep->extrapolateToPlane(*reference, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, true);
    if (0. > nextStep) {
      cout << " ERROR: The extrapolation to measurement point " << (ipoint_meas + 2) << " stepped back by " << nextStep << "cm !!! Track will be skipped." << endl;
      return;
    }
    rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
 
  } else {
    // No scattering: extrapolate whole distance between measurements
    rep->extrapolateToPlane(*reference, trk->getPoint(ipoint_meas + 1)->getFitterInfo(rep)->getPlane(), false, true);
    //NOTE: we will load the jacobian from this extrapolation in next loop into measurement point
    //jacPointToPoint.Print();
    rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
    //jacPointToPoint.Print();
  }
      }
    } catch (...) {
      cout << " ERROR: Extrapolation failed. Track will be skipped." << endl;
      return;
    }
    //cout << " Extrapolation finished." << endl;
 
 
    // Now store scatterers if not last measurement and if we decided
    // there should be scatteres, otherwise the jacobian in measurement
    // stored above is already correct
    if (ipoint_meas < npoints_meas - 1) {
 
      if (theta1 > scatEpsilon) {
  // We have to insert first scatterer at measurement point
  // Therefore (because state is perpendicular to plane, NOT track)
  // we have non-diaonal matrix of multiple scattering covariance
  // We have to project scattering into plane coordinates
  double c1 = trackDir.Dot(plane->getU());
  double c2 = trackDir.Dot(plane->getV());
  TMatrixDSym scatCov(2);
  scatCov(0, 0) = 1. - c2 * c2;
  scatCov(1, 1) = 1. - c1 * c1;
  scatCov(0, 1) = c1 * c2;
  scatCov(1, 0) = c1 * c2;
  scatCov *= theta1 * theta1 / (1. - c1 * c1 - c2 * c2) / (1. - c1 * c1 - c2 * c2) ;
 
  // last point is the just inserted measurement (or dummy point)
  GblPoint& lastPoint = listOfPoints.at(n_gbl_points - 1);
  lastPoint.addScatterer(scatResidual, scatCov.Invert());
 
      }
 
      if (theta2 > scatEpsilon) {
  // second scatterer is somewhere between measurements
  // TrackRep state is perpendicular to track direction if using extrapolateBy (I asked Johannes Rauch),
  // therefore scattering covariance is diagonal (and both elements are equal)
  TMatrixDSym scatCov(2);
  scatCov.Zero();
  scatCov(0, 0) = theta2 * theta2;
  scatCov(1, 1) = theta2 * theta2;
 
  GblPoint scatPoint(jacMeas2Scat);
  scatPoint.addScatterer(scatResidual, scatCov.Invert());
  listOfPoints.push_back(scatPoint);
  listOfLayers.push_back(((unsigned int) sensorId) + 0.5);
  n_gbl_points++;
      }
 
 
    }
    // Free memory on the heap
    delete reference;
  }
  // We should have at least two measurement points to fit anything
  if (n_gbl_meas_points > 1) {
    int fitRes = -1;
    double pvalue = 0.;
    GblTrajectory* traj = 0;
    try {
      // Construct the GBL trajectory, seed not used
      traj = new GblTrajectory(listOfPoints, seedLabel, clSeed, fitQoverP);
      // Fit the trajectory
      fitRes = traj->fit(Chi2, Ndf, lostWeight, m_gblInternalIterations);
      if (fitRes != 0) {
        //#ifdef DEBUG
        //traj->printTrajectory(100);
        //traj->printData();
        //traj->printPoints(100);
        //#endif
      }
    } catch (...) {
      // Gbl failed critically (usually GblPoint::getDerivatives ... singular matrix inversion)
      return;
    }
    
    pvalue = TMath::Prob(Chi2, Ndf);
    
    //traj->printTrajectory(100);
    //traj->printData();
    //traj->printPoints(100);
    
    #ifdef OUTPUT
    // Fill histogram with fit result
    fitResHisto->Fill(fitRes);
    ndfHisto->Fill(Ndf);
    #endif
    
    #ifdef DEBUG
    cout << " Ref. Track Chi2      :  " << trkChi2 << endl;
    cout << " Ref. end momentum    :  " << trackMomMag << " GeV/c ";
    if (abs(trk->getCardinalRep()->getPDG()) == 11) {
      if (particleCharge < 0.)
        cout << "(electron)";
      else
        cout << "(positron)";
    }
    cout << endl;
    cout << "------------------ GBL Fit Results --------------------" << endl;
    cout << " Fit q/p parameter    :  " << ((fitQoverP) ? ("True") : ("False")) << endl;
    cout << " Valid trajectory     :  " << ((traj->isValid()) ? ("True") : ("False")) << endl;
    cout << " Fit result           :  " << fitRes << "    (0 for success)" << endl;
    cout << " # GBL meas. points   :  " << n_gbl_meas_points << endl;
    cout << " # GBL all points     :  " << n_gbl_points << endl;
    cout << " GBL track NDF        :  " << Ndf << "    (-1 for failure)" << endl;
    cout << " GBL track Chi2       :  " << Chi2 << endl;
    cout << " GBL track P-value    :  " << pvalue;
    if (pvalue < m_pValueCut)
      cout << " < p-value cut " << m_pValueCut;
    cout << endl;
    cout << "-------------------------------------------------------" << endl;
    #endif
    
    #ifdef OUTPUT
    bool hittedLayers[12];
    for (int hl = 0; hl < 12; hl++) {
      hittedLayers[hl] = false;
    }
    #endif
    
    // GBL fit succeded if Ndf >= 0, but Ndf = 0 is useless
    //TODO: Here should be some track quality check
    //    if (Ndf > 0 && fitRes == 0) {
    if (traj->isValid() && pvalue >= m_pValueCut && Ndf >= m_minNdf) {
      
      // In case someone forgot to use beginRun and dind't reset mille file name to ""
      if (!milleFile && m_milleFileName != "")
        milleFile = new MilleBinary(m_milleFileName);
      
      // Loop over all GBL points
      for (unsigned int p = 0; p < listOfPoints.size(); p++) {
        unsigned int label = p + 1;
        unsigned int numRes;
        TVectorD residuals(2);
        TVectorD measErrors(2);
        TVectorD resErrors(2);
        TVectorD downWeights(2);
        //TODO: now we only provide info about measurements, not kinks
        if (!listOfPoints.at(p).hasMeasurement())
          continue;
        
        #ifdef OUTPUT
        // Decode VxdId and get layer in TB setup
        unsigned int l = 0;
        unsigned int id = listOfLayers.at(p);
        // skip segment (5 bits)
        id = id >> 5;
        unsigned int sensor = id & 7;
        id = id >> 3;
        unsigned int ladder = id & 31;
        id = id >> 5;
        unsigned int layer = id & 7;
        
        if (layer == 7 && ladder == 2) {
          l = sensor;
        } else if (layer == 7 && ladder == 3) {
          l = sensor + 9 - 3;
        } else {
          l = layer + 3;
        }
        
        hittedLayers[l - 1] = true;
        #endif
        TVectorD localPar(5);
        TMatrixDSym localCov(5);
        traj->getResults(label, localPar, localCov);
        // Get GBL fit results
        traj->getMeasResults(label, numRes, residuals, measErrors, resErrors, downWeights);
        if (m_chi2Cut && (fabs(residuals[0] / resErrors[0]) > m_chi2Cut || fabs(residuals[1] / resErrors[1]) > m_chi2Cut))
          return;
        // Write layer-wise data
        #ifdef OUTPUT
        if (!writeHistoDataForLabel(listOfLayers.at(p), residuals, measErrors, resErrors, downWeights, localPar, localCov))
          return;
        #endif
        
      } // end for points
      
      // Write binary data to mille binary
      if (milleFile && m_milleFileName != "" && pvalue >= m_pValueCut && Ndf >= m_minNdf) {
        traj->milleOut(*milleFile);
        #ifdef DEBUG
        cout << " GBL Track written to Millepede II binary file." << endl;
        cout << "-------------------------------------------------------" << endl;
        #endif
      }
      
      #ifdef OUTPUT
      // Fill histograms
      chi2OndfHisto->Fill(Chi2 / Ndf);
      pValueHisto->Fill(TMath::Prob(Chi2, Ndf));
      // track counting
      stats->Fill(0);
      // hitted sensors statistics
      if (
        hittedLayers[0] &&
        hittedLayers[1] &&
        hittedLayers[2] &&
        hittedLayers[3] &&
        hittedLayers[4] &&
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8]
      ) {
        // front tel + pxd + svd
        stats->Fill(1);
      }
      
      if (
        hittedLayers[0] &&
        hittedLayers[1] &&
        hittedLayers[2] &&
        hittedLayers[3] &&
        hittedLayers[4] &&
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8] &&
        hittedLayers[9] &&
        hittedLayers[10] &&
        hittedLayers[11]
      ) {
        // all layers
        stats->Fill(2);
      }
      if (
        hittedLayers[3] &&
        hittedLayers[4] &&
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8]
      ) {
        // vxd
        stats->Fill(3);
      }
      if (
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8]
      ) {
        // svd
        stats->Fill(4);
      }
      if (
        hittedLayers[0] &&
        hittedLayers[1] &&
        hittedLayers[2] &&
        
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8] &&
        hittedLayers[9] &&
        hittedLayers[10] &&
        hittedLayers[11]
      ) {
        // all except pxd
        stats->Fill(5);
      }
      if (
        hittedLayers[0] &&
        hittedLayers[1] &&
        hittedLayers[2] &&
        
        hittedLayers[5] &&
        hittedLayers[6] &&
        hittedLayers[7] &&
        hittedLayers[8]
      ) {
        // front tel + svd
        stats->Fill(6);
      }
      if (
        hittedLayers[9] &&
        hittedLayers[10] &&
        hittedLayers[11]
      ) {
        // backward tel
        stats->Fill(7);
      }
      #endif
    }
    
    // Free memory
    delete traj;
  }
}