GBL/src/GblFitter.cc

0001 //-*-mode: C++; c-basic-offset: 2; -*-
0002 /* Copyright 2013-2014
0003  *  Authors: Sergey Yashchenko and Tadeas Bilka
0004  *
0005  *  This is an interface to General Broken Lines
0006  *
0007  *  Version: 6 --------------------------------------------------------------
0008  *  - complete rewrite to GblFitter using GblFitterInfo and GblFitStatus
0009  *  - mathematics should be the same except for additional iterations
0010  *    (not possible before)
0011  *  - track is populated with scatterers + additional points with
0012  *    scatterers and no measurement (optional)
0013  *  - final track contains GblFitStatus and fitted states from GBL prediction
0014  *  - 1D/2D hits supported (pixel, single strip, combined strips(2D), wire)
0015  *  - At point: Only the very first raw measurement is used and from
0016  *    that, constructed MeasurementOnPlane with heighest weight
0017  *  Version: 5 --------------------------------------------------------------
0018  *  - several bug-fixes:
0019  *    - Scatterers at bad points
0020  *    - Jacobians at a point before they should be (code reorganized)
0021  *    - Change of sign of residuals
0022  *  Version: 4 --------------------------------------------------------------
0023  *    Fixed calculation of equvivalent scatterers (solution by C. Kleinwort)
0024  *    Now a scatterer is inserted at each measurement (except last) and
0025  *    between each two measurements.
0026  *    TrueHits/Clusters. Ghost (1D) hits ignored. With or
0027  *    without magnetic field.
0028  *  Version: 3 --------------------------------------------------------------
0029  *    This version now supports both TrueHits and Clusters for VXD.
0030  *    It can be used for arbitrary material distribution between
0031  *    measurements. Moments of scattering distribution are computed
0032  *    and translated into two equivalent thin GBL scatterers placed
0033  *    at computed positions between measurement points.
0034  *  Version: 2 ... never published -----------------------------------------
0035  *    Scatterer at each boundary (tooo many scatterers). TrueHits/Clusters.
0036  *    Without global der.&MP2 output.
0037  *  Version: 1 --------------------------------------------------------------
0038  *    Scatterers at measurement planes. TrueHits
0039  *  Version: 0 --------------------------------------------------------------
0040  *    Without scatterers. Genfit 1.
0041  *  -------------------------------------------------------------------------
0042  *
0043  *  This file is part of GENFIT.
0044  *
0045  *  GENFIT is free software: you can redistribute it and/or modify
0046  *  it under the terms of the GNU Lesser General Public License as published
0047  *  by the Free Software Foundation, either version 3 of the License, or
0048  *  (at your option) any later version.
0049  *
0050  *  GENFIT is distributed in the hope that it will be useful,
0051  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
0052  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0053  *  GNU Lesser General Public License for more details.
0054  *
0055  *  You should have received a copy of the GNU Lesser General Public License
0056  *  along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
0057  */
0058
0059 #include "GblFitter.h"
0060 #include "../include/GblFitStatus.h"
0061 #include "GblFitterInfo.h"
0062 #include "GblTrajectory.h"
0063 #include "GblPoint.h"
0064 #include "ICalibrationParametersDerivatives.h"
0065
0066 #include "Track.h"
0067 #include <TFile.h>
0068 #include <TH1F.h>
0069 #include <TTree.h>
0070 #include <string>
0071 #include <list>
0072 #include <FieldManager.h>
0073 #include <HMatrixU.h>
0074 #include <HMatrixV.h>
0075 #include <Math/SMatrix.h>
0076 #include <TMatrixD.h>
0077 #include <TVectorDfwd.h>
0078 #include <TMatrixT.h>
0079 #include <TVector3.h>
0080
0081 //#define DEBUG
0082
0083 using namespace gbl;
0084 using namespace std;
0085 using namespace genfit;
0086
0087 /**
0088  * Destructor
0089  */
0090 GblFitter::~GblFitter() {
0091   if (m_segmentController) {
0092     delete m_segmentController;
0093     m_segmentController = nullptr;
0094   }
0095 }
0096
0097 void GblFitter::setTrackSegmentController(GblTrackSegmentController* controler)
0098 {
0099   if (m_segmentController) {
0100     delete m_segmentController;
0101     m_segmentController = nullptr;
0102   }
0103   m_segmentController = controler;
0104 }
0105
0106 void GblFitter::processTrackWithRep(Track* trk, const AbsTrackRep* rep, bool resortHits)
0107 {
0108   cleanGblInfo(trk, rep);
0109
0110   if (resortHits)
0111     sortHits(trk, rep);
0112
0113   // This flag enables/disables fitting of q/p parameter in GBL
0114   // It is switched off automatically if no B-field at (0,0,0) is detected.
0115   bool fitQoverP = true;
0116   //TODO: Use clever way to determine zero B-field
0117   double Bfield = genfit::FieldManager::getInstance()->getFieldVal(TVector3(0., 0., 0.)).Mag();
0118   if (!(Bfield > 1.e-16))
0119     fitQoverP = false;
0120   // degrees of freedom after fit
0121   int Ndf = 0;
0122   // Chi2 after fit
0123   double Chi2 = 0.;
0124   //FIXME: d-w's not used so far...
0125   double lostWeight = 0.;
0126
0127   // Preparation of points (+add reference states) for GBL fit
0128   // -----------------------------------------------------------------
0129   genfit::GblFitStatus* gblfs = new genfit::GblFitStatus();
0130   trk->setFitStatus(gblfs, rep);
0131   gblfs->setCurvature(fitQoverP);
0132   //
0133   // Propagate reference seed, create scattering points, calc Jacobians
0134   // and store everything in fitter infos. (ready to collect points and fit)
0135   //
0136   //
0137   gblfs->setTrackLen(
0138   //
0139     constructGblInfo(trk, rep)
0140   //
0141   );
0142   //
0143   //
0144   gblfs->setIsFittedWithReferenceTrack(true);
0145   gblfs->setNumIterations(0); //default value, still valid, No GBL iteration
0146   if (m_externalIterations < 1)
0147     return;
0148   // -----------------------------------------------------------------
0149
0150   // cppcheck-suppress unreadVariable
0151   unsigned int nFailed = 0;
0152   // cppcheck-suppress unreadVariable
0153   int fitRes = 0;
0154   std::vector<std::string> gblIterations;
0155   gblIterations.push_back(m_gblInternalIterations);
0156
0157   // Iterations and updates of fitter infos and fit status
0158   // -------------------------------------------------------------------
0159   for (unsigned int iIter = 0; iIter < m_externalIterations; iIter++) {
0160     // GBL refit (1st of reference, then refit of GBL trajectory itself)
0161     int nscat = 0, nmeas = 0, ndummy = 0;
0162     std::vector<gbl::GblPoint> points = collectGblPoints(trk, rep);
0163     for(unsigned int ip = 0;ip<points.size(); ip++) {
0164       GblPoint & p = points.at(ip);
0165       if (p.hasScatterer())
0166         nscat++;
0167       if (p.hasMeasurement())
0168         nmeas++;
0169       if(!p.hasMeasurement()&&!p.hasScatterer())
0170         ndummy++;
0171     }
0172     gbl::GblTrajectory traj(points, gblfs->hasCurvature());
0173
0174     fitRes = traj.fit(Chi2, Ndf, lostWeight, (iIter == m_externalIterations - 1) ? m_gblInternalIterations : "");
0175
0176     // Update fit results in fitterinfos
0177     updateGblInfo(traj, trk, rep);
0178
0179     // This repropagates to get new Jacobians,
0180     // if planes changed, predictions are extrapolated to new planes
0181     if (m_recalcJacobians > iIter) {
0182       GblFitterInfo* prevFitterInfo = 0;
0183       GblFitterInfo* currFitterInfo = 0;
0184       for (unsigned int ip = 0; ip < trk->getNumPoints(); ip++) {
0185         if (trk->getPoint(ip)->hasFitterInfo(rep) && (currFitterInfo = dynamic_cast<GblFitterInfo*>(trk->getPoint(ip)->getFitterInfo(rep)))) {
0186
0187           currFitterInfo->recalculateJacobian(prevFitterInfo);
0188           prevFitterInfo = currFitterInfo;
0189         }
0190       }
0191     }
0192
0193     gblfs->setIsFitted(true);
0194     gblfs->setIsFitConvergedPartially(fitRes == 0);
0195     nFailed = trk->getNumPointsWithMeasurement() - nmeas;
0196     gblfs->setNFailedPoints(nFailed);
0197     gblfs->setIsFitConvergedFully(fitRes == 0 && nFailed == 0);
0198     gblfs->setNumIterations(iIter + 1);
0199     gblfs->setChi2(Chi2);
0200     gblfs->setNdf(Ndf);
0201     gblfs->setCharge(trk->getFittedState().getCharge());
0202
0203     #ifdef DEBUG
0204     int npoints_meas = trk->getNumPointsWithMeasurement();
0205     int npoints_all = trk->getNumPoints();
0206
0207     cout << "-------------------------------------------------------" << endl;
0208     cout << "               GBL processed genfit::Track            " << endl;
0209     cout << "-------------------------------------------------------" << endl;
0210     cout << " # Track Points       :  " << npoints_all  << endl;
0211     cout << " # Meas. Points       :  " << npoints_meas << endl;
0212     cout << " # GBL points all     :  " << traj.getNumPoints();
0213     if (ndummy)
0214       cout << " (" << ndummy << " dummy) ";
0215     cout << endl;
0216     cout << " # GBL points meas    :  " << nmeas << endl;
0217     cout << " # GBL points scat    :  " << nscat << endl;
0218     cout << "-------------- GBL Fit Results ----------- Iteration  " << iIter+1 << " " << ((iIter < gblIterations.size()) ? gblIterations[iIter] : "") << endl;
0219     cout << " Fit q/p parameter    :  " << (gblfs->hasCurvature() ? ("True") : ("False")) << endl;
0220     cout << " Valid trajectory     :  " << ((traj.isValid()) ? ("True") : ("False")) << endl;
0221     cout << " Fit result           :  " << fitRes << "    (0 for success)" << endl;
0222     cout << " GBL track NDF        :  " << Ndf << "    (-1 for failure)" << endl;
0223     cout << " GBL track Chi2       :  " << Chi2 << endl;
0224     cout << " GBL track P-value    :  " << TMath::Prob(Chi2, Ndf) << endl;
0225     cout << "-------------------------------------------------------" << endl;
0226     #endif
0227
0228   }
0229   // -------------------------------------------------------------------
0230
0231 }
0232
0233 void GblFitter::cleanGblInfo(Track* trk, const AbsTrackRep* rep) const {
0234
0235   for (int ip = trk->getNumPoints() - 1; ip >=0; ip--) {
0236     trk->getPoint(ip)->setScatterer(nullptr);
0237     trk->getPoint(ip)->deleteFitterInfo(rep);
0238     //TODO
0239     if (!trk->getPoint(ip)->hasRawMeasurements())
0240       trk->deletePoint(ip);
0241   }
0242 }
0243
0244 void GblFitter::sortHits(Track* trk, const AbsTrackRep* rep) const {
0245   // All measurement points in ref. track
0246   int npoints_meas = trk->getNumPointsWithMeasurement();
0247   // Prepare state for extrapolation of track seed
0248   StateOnPlane reference(rep);
0249   rep->setTime(reference, trk->getTimeSeed());
0250   rep->setPosMom(reference, trk->getStateSeed());
0251   // Take the state to first plane
0252   SharedPlanePtr firstPlane(trk->getPointWithMeasurement(0)->getRawMeasurement(0)->constructPlane(reference));
0253   reference.extrapolateToPlane(firstPlane);
0254   //1st point is at arc-len=0
0255   double arcLenPos = 0;
0256
0257   // Loop only between meas. points
0258   for (int ipoint_meas = 0; ipoint_meas < npoints_meas - 1; ipoint_meas++) {
0259     // current measurement point
0260     TrackPoint* point_meas = trk->getPointWithMeasurement(ipoint_meas);
0261     // Current detector plane
0262     SharedPlanePtr plane = point_meas->getRawMeasurement(0)->constructPlane(reference);
0263     // Get the next plane
0264     SharedPlanePtr nextPlane(trk->getPointWithMeasurement(ipoint_meas + 1)->getRawMeasurement(0)->constructPlane(reference));
0265
0266     point_meas->setSortingParameter(arcLenPos);
0267     arcLenPos += reference.extrapolateToPlane(nextPlane);
0268
0269   } // end of loop over track points with measurement
0270   trk->getPointWithMeasurement(npoints_meas - 1)->setSortingParameter(arcLenPos);
0271   trk->sort();
0272 }
0273
0274 std::vector<gbl::GblPoint> GblFitter::collectGblPoints(genfit::Track* trk, const genfit::AbsTrackRep* rep) {
0275   //TODO store collected points in in fit status? need streamer for GblPoint (or something like that)
0276   std::vector<gbl::GblPoint> thePoints;
0277   thePoints.clear();
0278
0279   // Collect points from track and fitterInfo(rep)
0280   for (unsigned int ip = 0; ip < trk->getNumPoints(); ip++) {
0281     GblFitterInfo * gblfi = dynamic_cast<GblFitterInfo*>(trk->getPoint(ip)->getFitterInfo(rep));
0282     if (!gblfi)
0283       continue;
0284     thePoints.push_back(gblfi->constructGblPoint());
0285   }
0286   return thePoints;
0287 }
0288
0289 void GblFitter::updateGblInfo(gbl::GblTrajectory& traj, genfit::Track* trk, const genfit::AbsTrackRep* rep) {
0290   //FIXME
0291   if (!traj.isValid())
0292     return;
0293
0294   // Update points in track and fitterInfo(rep)
0295   int igblfi = -1;
0296   for (unsigned int ip = 0; ip < trk->getNumPoints(); ip++) {
0297     GblFitterInfo * gblfi = dynamic_cast<GblFitterInfo*>(trk->getPoint(ip)->getFitterInfo(rep));
0298     if (!gblfi)
0299       continue;
0300     igblfi++;
0301
0302     // The point will calculate its position on the track
0303     // (counting fitter infos) which hopefully
0304     gblfi->updateFitResults(traj);
0305
0306     // This is agains logic. User can do this if he wants and it is recommended usually
0307     // so that following fit could reuse the updated seed
0308     //if (igblfi == 0) {
0309     //  trk->setStateSeed( gblfi->getFittedState(true).getPos(), gblfi->getFittedState(true).getMom() );
0310     //  trk->setCovSeed( gblfi->getFittedState(true).get6DCov() );
0311     //}
0312   }
0313 }
0314
0315 void GblFitter::getScattererFromMatList(double& length,
0316                              double& theta, double& s, double& ds,
0317                              const double p, const double mass, const double charge,
0318                              const std::vector<genfit::MatStep>& steps) const {
0319   theta = 0.; s = 0.; ds = 0.; length = 0;
0320   if (steps.empty()) return;
0321
0322   // sum of step lengths
0323   double len = 0.;
0324   // normalization
0325   double sumxx = 0.;
0326   // first moment (non-normalized)
0327   double sumx2x2 = 0.;
0328   // (part of) second moment / variance (non-normalized)
0329   double sumx3x3 = 0.;
0330
0331   // cppcheck-suppress unreadVariable
0332   double xmin = 0.;
0333   double xmax = 0.;
0334
0335   for (unsigned int i = 0; i < steps.size(); i++) {
0336     const MatStep step = steps.at(i);
0337     // inverse of material radiation length ... (in 1/cm) ... "density of scattering"
0338     double rho = 1. / step.material_.radiationLength;
0339     len += fabs(step.stepSize_);
0340     xmin = xmax;
0341     xmax = xmin + fabs(step.stepSize_);
0342     // Compute integrals
0343
0344     // integral of rho(x)
0345     sumxx   += rho * (xmax - xmin);
0346     // integral of x*rho(x)
0347     sumx2x2 += rho * (xmax * xmax - xmin * xmin) / 2.;
0348     // integral of x*x*rho(x)
0349     sumx3x3 += rho * (xmax * xmax * xmax - xmin * xmin * xmin) / 3.;
0350   }
0351   // This ensures PDG formula still gives positive results (but sumxx should be > 1e-4 for it to hold)
0352   if (sumxx < 1.0e-10) return;
0353   // Calculate theta from total sum of radiation length
0354   // instead of summimg squares of individual deflection angle variances
0355   // PDG formula:
0356   double beta = p / sqrt(p * p + mass * mass);
0357   theta = (0.0136 / p / beta) * fabs(charge) * sqrt(sumxx) * (1. + 0.038 * log(sumxx));
0358   //theta = (0.015 / p / beta) * fabs(charge) * sqrt(sumxx);
0359
0360   // track length
0361   length = len;
0362   // Normalization factor
0363   double N = 1. / sumxx;
0364   // First moment
0365   s  = N * sumx2x2;
0366   // Square of second moment (variance)
0367   // integral of (x - s)*(x - s)*rho(x)
0368   double ds_2 = N * (sumx3x3 - 2. * sumx2x2 * s + sumxx * s * s);
0369   ds = sqrt(ds_2);
0370
0371   #ifdef DEBUG
0372   ////std::cout << "Thick scatterer parameters (dtheta, <s>, ds): " << "(" << theta << ", " << s << ", " << ds << ")" << endl;
0373   #endif
0374 }
0375
0376 double GblFitter::constructGblInfo(Track* trk, const AbsTrackRep* rep)
0377 {
0378   // All measurement points in ref. track
0379   int npoints_meas = trk->getNumPointsWithMeasurement();
0380   // Dimesion of representation/state
0381   int dim = rep->getDim();
0382   // Jacobian for point with measurement = how to propagate from previous point (scat/meas)
0383   TMatrixD jacPointToPoint(dim, dim);
0384   jacPointToPoint.UnitMatrix();
0385
0386   // Prepare state for extrapolation of track seed
0387   // Take the state to first plane
0388   StateOnPlane reference(rep);
0389   rep->setTime(reference, trk->getTimeSeed());
0390   rep->setPosMom(reference, trk->getStateSeed());
0391
0392   SharedPlanePtr firstPlane(trk->getPointWithMeasurement(0)->getRawMeasurement(0)->constructPlane(reference));
0393   reference.extrapolateToPlane(firstPlane);
0394
0395   double sumTrackLen = 0;
0396   // NOT used but useful
0397   TMatrixDSym noise; TVectorD deltaState;
0398
0399   // Loop only between meas. points
0400   for (int ipoint_meas = 0; ipoint_meas < npoints_meas; ipoint_meas++) {
0401     // current measurement point
0402     TrackPoint* point_meas = trk->getPointWithMeasurement(ipoint_meas);
0403     // Current detector plane
0404     SharedPlanePtr plane = point_meas->getRawMeasurement(0)->constructPlane(reference);
0405     // track direction at plane (in global coords)
0406     TVector3 trackDir = rep->getDir(reference);
0407     // track momentum direction vector at plane (in global coords)
0408     double trackMomMag = rep->getMomMag(reference);
0409     // charge of particle
0410     double particleCharge = rep->getCharge(reference);
0411     // mass of particle
0412     double particleMass = rep->getMass(reference);
0413     // Parameters of a thick scatterer between measurements
0414     double trackLen = 0., scatTheta = 0., scatSMean = 0., scatDeltaS = 0.;
0415     // Parameters of two equivalent thin scatterers
0416     double theta1 = 0., theta2 = 0., s1 = 0., s2 = 0.;
0417     // jacobian from s1=0 to s2
0418     TMatrixD jacMeas2Scat(dim, dim);
0419     jacMeas2Scat.UnitMatrix();
0420
0421     // Stop here if we are at last point (do not add scatterers to last point),
0422     // just the fitter info
0423     if (ipoint_meas >= npoints_meas - 1) {
0424
0425       // Construction last measurement (no scatterer)
0426       // --------------------------------------------
0427       // Just add the fitter info of last plane
0428       GblFitterInfo* gblfimeas(new GblFitterInfo(point_meas, rep, reference));
0429       gblfimeas->setJacobian(jacPointToPoint);
0430       point_meas->setFitterInfo(gblfimeas);
0431       // --------------------------------------------
0432
0433       break;
0434     }
0435     // Extrapolate to next measurement to get material distribution
0436     // Use a temp copy of the StateOnPlane to propage between measurements
0437     StateOnPlane refCopy(reference);
0438     // Get the next plane
0439     SharedPlanePtr nextPlane(trk->getPointWithMeasurement(ipoint_meas + 1)->getRawMeasurement(0)->constructPlane(reference));
0440
0441     // Extrapolation for multiple scattering calculation
0442     // Extrap to point + 1, do NOT stop at boundary
0443     TVector3 segmentEntry = refCopy.getPos();
0444     rep->extrapolateToPlane(refCopy, nextPlane, false, false);
0445     TVector3 segmentExit = refCopy.getPos();
0446
0447     getScattererFromMatList(trackLen,
0448                             scatTheta,
0449                             scatSMean,
0450                             scatDeltaS,
0451                             trackMomMag,
0452                             particleMass,
0453                             particleCharge,
0454                             rep->getSteps());
0455     // Now calculate positions and scattering variance for equivalent scatterers
0456     // (Solution from Claus Kleinwort (DESY))
0457     s1 = 0.; s2 = scatSMean + scatDeltaS * scatDeltaS / (scatSMean - s1);
0458     theta1 = sqrt(scatTheta * scatTheta * scatDeltaS * scatDeltaS / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
0459     theta2 = sqrt(scatTheta * scatTheta * (scatSMean - s1) * (scatSMean - s1) / (scatDeltaS * scatDeltaS + (scatSMean - s1) * (scatSMean - s1)));
0460
0461     // Call segment controller to set MS options:
0462     if (m_segmentController)
0463       m_segmentController->controlTrackSegment(segmentEntry, segmentExit, scatTheta, this);
0464
0465     // Scattering options: OFF / THIN / THICK
0466     if (m_enableScatterers && !m_enableIntermediateScatterer) {
0467       theta1 = scatTheta;
0468       theta2 = 0;
0469     } else if (!m_enableScatterers) {
0470       theta1 = 0.;
0471       theta2 = 0.;
0472     }
0473
0474     // Construction of measurement (with scatterer)
0475     // --------------------------------------------
0476
0477     if (theta1 > scatEpsilon)
0478       point_meas->setScatterer(new ThinScatterer(plane, Material(theta1, 0., 0., 0., 0.)));
0479
0480     GblFitterInfo* gblfimeas(new GblFitterInfo(point_meas, rep, reference));
0481     gblfimeas->setJacobian(jacPointToPoint);
0482     point_meas->setFitterInfo(gblfimeas);
0483     // --------------------------------------------
0484
0485
0486     // If not last measurement, extrapolate and get jacobians for scattering points between this and next measurement
0487     if (theta2 > scatEpsilon) {
0488       // First scatterer has been placed at current measurement point (see above)
0489       // theta2 > 0 ... we want second scatterer:
0490       // Extrapolate to s2 (we have s1 = 0)
0491       rep->extrapolateBy(reference, s2, false, true);
0492       rep->getForwardJacobianAndNoise(jacMeas2Scat, noise, deltaState);
0493
0494       // Construction of intermediate scatterer
0495       // --------------------------------------
0496       TrackPoint* scattp = new TrackPoint(trk);
0497       scattp->setSortingParameter(point_meas->getSortingParameter() + s2);
0498       scattp->setScatterer(new ThinScatterer(reference.getPlane(), Material(theta2, 0., 0., 0., 0.)));
0499       // Add point to track before next point
0500       int pointIndex = 0;
0501       //TODO Deduce this rather than looping over all points
0502       for (unsigned int itp = 0; itp < trk->getNumPoints(); itp++) {
0503         if (trk->getPoint(itp) == point_meas) {
0504           pointIndex = itp;
0505           break;
0506         }
0507       }
0508       trk->insertPoint(scattp, pointIndex + 1);
0509       // Create and store fitter info
0510       GblFitterInfo * gblfiscat(new GblFitterInfo(scattp, rep, reference));
0511       gblfiscat->setJacobian(jacMeas2Scat);
0512       scattp->setFitterInfo(gblfiscat);
0513       // ---------------------------------------
0514
0515       // Finish extrapolation to next measurement
0516       double nextStep = rep->extrapolateToPlane(reference, nextPlane, false, true);
0517       rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
0518
0519       if (0. > nextStep) {
0520         cout << " ERROR: The extrapolation to measurement point " << (ipoint_meas + 2) << " stepped back by " << nextStep << "cm !!! Track will be cut before this point." << endl;
0521         // stop trajectory construction here
0522         break;
0523       }
0524
0525     } else {
0526       // No scattering: extrapolate whole distance between measurements
0527       double nextStep = rep->extrapolateToPlane(reference, nextPlane, false, true);
0528       rep->getForwardJacobianAndNoise(jacPointToPoint, noise, deltaState);
0529
0530       if (0. > nextStep) {
0531         cout << " ERROR: The extrapolation to measurement point " << (ipoint_meas + 2) << " stepped back by " << nextStep << "cm !!! Track will be cut before this point." << endl;
0532         // stop trajectory construction here
0533         break;
0534       }
0535     }
0536     // Track length up to next point
0537     sumTrackLen += trackLen;
0538
0539   } // end of loop over track points with measurement
0540   return sumTrackLen;
0541 }