GblTrajectory.cc

Go to the documentation of this file.

/*
 * GblTrajectory.cpp
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */

#include "Alignment/ReferenceTrajectories/interface/GblTrajectory.h"
using namespace Eigen;

namespace gbl {


  GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
                               bool flagCurv, bool flagU1dir, bool flagU2dir) :
    numAllPoints(aPointList.size()), numPoints(), numOffsets(0),
    numInnerTrans(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
    numLocals(0), numMeasurements(0), externalPoint(0), skippedMeasLabel(0),
    maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(),
    scatDataIndex(), externalSeed(), innerTransformations(),
    externalDerivatives(), externalMeasurements(), externalPrecisions()
  {

    if (flagU1dir)
      theDimension.push_back(0);
    if (flagU2dir)
      theDimension.push_back(1);
    // simple (single) trajectory
    thePoints.emplace_back(std::move(aPointList));
    numPoints.push_back(numAllPoints);
    construct(); // construct trajectory
  }


  GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, MatrixXd> > &aPointsAndTransList) :
    numAllPoints(), numPoints(), numOffsets(0),
    numInnerTrans(aPointsAndTransList.size()), numParameters(0), numLocals(0),
    numMeasurements(0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0),
    theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(),
    externalSeed(), innerTransformations(), externalDerivatives(),
    externalMeasurements(), externalPrecisions()
  {

    for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
      thePoints.push_back(aPointsAndTransList[iTraj].first);
      numPoints.push_back(thePoints.back().size());
      numAllPoints += numPoints.back();
      innerTransformations.push_back(aPointsAndTransList[iTraj].second);
    }
    theDimension.push_back(0);
    theDimension.push_back(1);
    numCurvature = innerTransformations[0].cols();
    construct(); // construct (composed) trajectory
  }

#ifdef GBL_EIGEN_SUPPORT_ROOT

  GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
                               unsigned int aLabel, const TMatrixDSym &aSeed,
                               bool flagCurv, bool flagU1dir, bool flagU2dir) :
    numAllPoints(aPointList.size()), numPoints(), numOffsets(0),
    numInnerTrans(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
    numLocals(0), numMeasurements(0), externalPoint(aLabel),
    skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(),
    theData(), measDataIndex(), scatDataIndex(), externalSeed(),
    innerTransformations(), externalDerivatives(), externalMeasurements(),
    externalPrecisions()
  {

    if (flagU1dir)
      theDimension.push_back(0);
    if (flagU2dir)
      theDimension.push_back(1);
    // convert from ROOT
    unsigned int nParSeed = aSeed.GetNrows();
    externalSeed.resize(nParSeed, nParSeed);
    for (unsigned int i = 0; i < nParSeed; ++i) {
      for (unsigned int j = 0; j < nParSeed; ++j) {
        externalSeed(i, j) = aSeed(i, j);
      }
    }
    // simple (single) trajectory
    thePoints.emplace_back(std::move(aPointList));
    numPoints.push_back(numAllPoints);
    construct(); // construct trajectory
  }


  GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList) :
    numAllPoints(), numPoints(), numOffsets(0),
    numInnerTrans(aPointsAndTransList.size()), numParameters(0), numLocals(0),
    numMeasurements(0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0),
    theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(),
    externalSeed(), innerTransformations(), externalDerivatives(),
    externalMeasurements(), externalPrecisions()
  {

    for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
      thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
      numPoints.push_back(thePoints.back().size());
      numAllPoints += numPoints.back();
      // convert from ROOT
      unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
      unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
      MatrixXd aTrans(nRowTrans, nColTrans);
      for (unsigned int i = 0; i < nRowTrans; ++i) {
        for (unsigned int j = 0; j < nColTrans; ++j) {
          aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
        }
      }
      innerTransformations.emplace_back(std::move(aTrans));
    }
    theDimension.push_back(0);
    theDimension.push_back(1);
    numCurvature = innerTransformations[0].cols();
    construct(); // construct (composed) trajectory
  }


  GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
                               const TMatrixD &extDerivatives,
                               const TVectorD &extMeasurements,
                               const TVectorD &extPrecisions) :
    numAllPoints(), numPoints(), numOffsets(0),
    numInnerTrans(aPointsAndTransList.size()), numParameters(0), numLocals(0),
    numMeasurements(0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0),
    theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(),
    externalSeed(), innerTransformations(), externalDerivatives(),
    externalMeasurements(), externalPrecisions()
  {

    // convert from ROOT
    unsigned int nExtMeas = extDerivatives.GetNrows();
    unsigned int nExtPar = extDerivatives.GetNcols();
    externalDerivatives.resize(nExtMeas, nExtPar);
    externalMeasurements.resize(nExtMeas);
    externalPrecisions.resize(nExtMeas);
    for (unsigned int i = 0; i < nExtMeas; ++i) {
      externalMeasurements(i) = extMeasurements[i];
      externalPrecisions(i) = extPrecisions[i];
      for (unsigned int j = 0; j < nExtPar; ++j) {
        externalDerivatives(i, j) = extDerivatives(i, j);
      }
    }
    for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
      thePoints.emplace_back(std::move(aPointsAndTransList[iTraj].first));
      numPoints.push_back(thePoints.back().size());
      numAllPoints += numPoints.back();
      // convert from ROOT
      unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
      unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
      MatrixXd aTrans(nRowTrans, nColTrans);
      for (unsigned int i = 0; i < nRowTrans; ++i) {
        for (unsigned int j = 0; j < nColTrans; ++j) {
          aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
        }
      }
      innerTransformations.emplace_back(std::move(aTrans));
    }
    theDimension.push_back(0);
    theDimension.push_back(1);
    numCurvature = innerTransformations[0].cols();
    construct(); // construct (composed) trajectory
  }


  GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
                               const TMatrixD &extDerivatives,
                               const TVectorD &extMeasurements,
                               const TMatrixDSym &extPrecisions) :
    numAllPoints(), numPoints(), numOffsets(0),
    numInnerTrans(aPointsAndTransList.size()), numParameters(0), numLocals(0),
    numMeasurements(0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0),
    theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(),
    externalSeed(), innerTransformations()
  {

    // diagonalize external measurement
    TMatrixDSymEigen extEigen(extPrecisions);
    TMatrixD extTransformation = extEigen.GetEigenVectors();
    extTransformation.T();
    TMatrixD aDerivatives = extTransformation * extDerivatives;
    TVectorD aMeasurements = extTransformation * extMeasurements;
    TVectorD aPrecisions = extEigen.GetEigenValues();
    // convert from ROOT
    unsigned int nExtMeas = aDerivatives.GetNrows();
    unsigned int nExtPar = aDerivatives.GetNcols();
    externalDerivatives.resize(nExtMeas, nExtPar);
    externalMeasurements.resize(nExtMeas);
    externalPrecisions.resize(nExtMeas);
    for (unsigned int i = 0; i < nExtMeas; ++i) {
      externalMeasurements(i) = aMeasurements[i];
      externalPrecisions(i) = aPrecisions[i];
      for (unsigned int j = 0; j < nExtPar; ++j) {
        externalDerivatives(i, j) = aDerivatives(i, j);
      }
    }
    for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
      thePoints.push_back(aPointsAndTransList[iTraj].first);
      numPoints.push_back(thePoints.back().size());
      numAllPoints += numPoints.back();
      // convert from ROOT
      unsigned int nRowTrans = aPointsAndTransList[iTraj].second.GetNrows();
      unsigned int nColTrans = aPointsAndTransList[iTraj].second.GetNcols();
      MatrixXd aTrans(nRowTrans, nColTrans);
      for (unsigned int i = 0; i < nRowTrans; ++i) {
        for (unsigned int j = 0; j < nColTrans; ++j) {
          aTrans(i, j) = aPointsAndTransList[iTraj].second(i, j);
        }
      }
      innerTransformations.push_back(aTrans);
    }
    theDimension.push_back(0);
    theDimension.push_back(1);
    numCurvature = innerTransformations[0].cols();
    construct(); // construct (composed) trajectory
  }
#endif

  GblTrajectory::~GblTrajectory() {
  }

  bool GblTrajectory::isValid() const {
    return constructOK;
  }

  unsigned int GblTrajectory::getNumPoints() const {
    return numAllPoints;
  }


  void GblTrajectory::construct() {

    constructOK = false;
    fitOK = false;
    unsigned int aLabel = 0;
    if (numAllPoints < 2) {
      std::cout << " GblTrajectory construction failed: too few GblPoints "
                << std::endl;
      return;
    }
    // loop over trajectories
    numTrajectories = thePoints.size();
    //std::cout << " numTrajectories: " << numTrajectories << ", " << innerTransformations.size() << std::endl;
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      std::vector<GblPoint>::iterator itPoint;
      for (itPoint = thePoints[iTraj].begin();
           itPoint < thePoints[iTraj].end(); ++itPoint) {
        numLocals = std::max(numLocals, itPoint->getNumLocals());
        numMeasurements += itPoint->hasMeasurement();
        itPoint->setLabel(++aLabel);
      }
    }
    defineOffsets();
    calcJacobians();
    try {
      prepare();
    } catch (std::overflow_error &e) {
      std::cout << " GblTrajectory construction failed: " << e.what()
                << std::endl;
      return;
    }

    constructOK = true;
    // number of fit parameters
    numParameters = (numOffsets - 2 * numInnerTrans) * theDimension.size()
      + numCurvature + numLocals;
  }


  void GblTrajectory::defineOffsets() {

    // loop over trajectories
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      // first point is offset
      thePoints[iTraj].front().setOffset(numOffsets++);
      // intermediate scatterers are offsets
      std::vector<GblPoint>::iterator itPoint;
      for (itPoint = thePoints[iTraj].begin() + 1;
           itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
        if (itPoint->hasScatterer()) {
          itPoint->setOffset(numOffsets++);
        } else {
          itPoint->setOffset(-numOffsets);
        }
      }
      // last point is offset
      thePoints[iTraj].back().setOffset(numOffsets++);
    }
  }

  void GblTrajectory::calcJacobians() {

    Matrix5d scatJacobian;
    // loop over trajectories
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      // forward propagation (all)
      GblPoint* previousPoint = &thePoints[iTraj].front();
      unsigned int numStep = 0;
      std::vector<GblPoint>::iterator itPoint;
      for (itPoint = thePoints[iTraj].begin() + 1;
           itPoint < thePoints[iTraj].end(); ++itPoint) {
        if (numStep == 0) {
          scatJacobian = itPoint->getP2pJacobian();
        } else {
          scatJacobian = itPoint->getP2pJacobian() * scatJacobian;
        }
        numStep++;
        itPoint->addPrevJacobian(scatJacobian); // iPoint -> previous scatterer
        if (itPoint->getOffset() >= 0) {
          previousPoint->addNextJacobian(scatJacobian); // lastPoint -> next scatterer
          numStep = 0;
          previousPoint = &(*itPoint);
        }
      }
      // backward propagation (without scatterers)
      for (itPoint = thePoints[iTraj].end() - 1;
           itPoint > thePoints[iTraj].begin(); --itPoint) {
        if (itPoint->getOffset() >= 0) {
          scatJacobian = itPoint->getP2pJacobian();
          continue; // skip offsets
        }
        itPoint->addNextJacobian(scatJacobian); // iPoint -> next scatterer
        scatJacobian = scatJacobian * itPoint->getP2pJacobian();
      }
    }
  }


  std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
                                                                            int aSignedLabel) const {

    unsigned int nDim = theDimension.size();
    unsigned int nCurv = numCurvature;
    unsigned int nLocals = numLocals;
    unsigned int nBorder = nCurv + nLocals;
    unsigned int nParBRL = nBorder + 2 * nDim;
    unsigned int nParLoc = nLocals + 5;
    std::vector<unsigned int> anIndex;
    anIndex.reserve(nParBRL);
    MatrixXd aJacobian(nParLoc, nParBRL);
    aJacobian.setZero();

    unsigned int aLabel = abs(aSignedLabel);
    unsigned int firstLabel = 1;
    unsigned int lastLabel = 0;
    unsigned int aTrajectory = 0;
    // loop over trajectories
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      aTrajectory = iTraj;
      lastLabel += numPoints[iTraj];
      if (aLabel <= lastLabel)
        break;
      if (iTraj < numTrajectories - 1)
        firstLabel += numPoints[iTraj];
    }
    int nJacobian; // 0: prev, 1: next
    // check consistency of (index, direction)
    if (aSignedLabel > 0) {
      nJacobian = 1;
      if (aLabel >= lastLabel) {
        aLabel = lastLabel;
        nJacobian = 0;
      }
    } else {
      nJacobian = 0;
      if (aLabel <= firstLabel) {
        aLabel = firstLabel;
        nJacobian = 1;
      }
    }
    const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
    std::array<unsigned int, 5> labDer;
    Matrix5d matDer;
    getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);

    // from local parameters
    for (unsigned int i = 0; i < nLocals; ++i) {
      aJacobian(i + 5, i) = 1.0;
      anIndex.push_back(i + 1);
    }
    // from trajectory parameters
    unsigned int iCol = nLocals;
    for (unsigned int i = 0; i < 5; ++i) {
      if (labDer[i] > 0) {
        anIndex.push_back(labDer[i]);
        for (unsigned int j = 0; j < 5; ++j) {
          aJacobian(j, iCol) = matDer(j, i);
        }
        ++iCol;
      }
    }
    return std::make_pair(anIndex, aJacobian);
  }


  void GblTrajectory::getFitToLocalJacobian(std::array<unsigned int, 5>& anIndex,
                                            Matrix5d &aJacobian, const GblPoint &aPoint, unsigned int measDim,
                                            unsigned int nJacobian) const {

    unsigned int nDim = theDimension.size();
    unsigned int nCurv = numCurvature;
    unsigned int nLocals = numLocals;

    int nOffset = aPoint.getOffset();

    anIndex = {};
    aJacobian.setZero();
    if (nOffset < 0) // need interpolation
      {
        Matrix2d prevW, prevWJ, nextW, nextWJ, matN;
        Vector2d prevWd, nextWd;
        aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
        aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W+, W+ * J+, W+ * d+
        const Matrix2d sumWJ(prevWJ + nextWJ);
        matN = sumWJ.inverse(); // N = (W- * J- + W+ * J+)^-1
        // derivatives for u_int
        const Matrix2d prevNW(matN * prevW); // N * W-
        const Matrix2d nextNW(matN * nextW); // N * W+
        const Vector2d prevNd(matN * prevWd); // N * W- * d-
        const Vector2d nextNd(matN * nextWd); // N * W+ * d+

        unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i)

        // local offset
        if (nCurv > 0) {
          aJacobian.block<2, 1>(3, 0) = -prevNd - nextNd; // from curvature
          anIndex[0] = nLocals + 1;
        }
        aJacobian.block<2, 2>(3, 1) = prevNW; // from 1st Offset
        aJacobian.block<2, 2>(3, 3) = nextNW; // from 2nd Offset
        for (unsigned int i = 0; i < nDim; ++i) {
          anIndex[1 + theDimension[i]] = iOff + i;
          anIndex[3 + theDimension[i]] = iOff + nDim + i;
        }

        // local slope and curvature
        if (measDim > 2) {
          // derivatives for u'_int
          const Matrix2d prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
          const Matrix2d nextWPN(prevWJ * nextNW); // W- * J- * N * W+
          const Vector2d prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
          const Vector2d nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
          if (nCurv > 0) {
            aJacobian(0, 0) = 1.0;
            aJacobian.block<2, 1>(1, 0) = prevWNd - nextWNd; // from curvature
          }
          aJacobian.block<2, 2>(1, 1) = -prevWPN; // from 1st Offset
          aJacobian.block<2, 2>(1, 3) = nextWPN; // from 2nd Offset
        }
      } else { // at point
      // anIndex must be sorted
      // forward : iOff2 = iOff1 + nDim, index1 = 1, index2 = 3
      // backward: iOff2 = iOff1 - nDim, index1 = 3, index2 = 1
      unsigned int iOff1 = nDim * nOffset + nCurv + nLocals + 1; // first offset ('i' in u_i)
      unsigned int index1 = 3 - 2 * nJacobian; // index of first offset
      unsigned int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1); // second offset ('i' in u_i)
      unsigned int index2 = 1 + 2 * nJacobian; // index of second offset
      // local offset
      aJacobian(3, index1) = 1.0; // from 1st Offset
      aJacobian(4, index1 + 1) = 1.0;
      for (unsigned int i = 0; i < nDim; ++i) {
        anIndex[index1 + theDimension[i]] = iOff1 + i;
      }

      // local slope and curvature
      if (measDim > 2) {
        Matrix2d matW, matWJ;
        Vector2d vecWd;
        aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d
        double sign = (nJacobian > 0) ? 1. : -1.;
        if (nCurv > 0) {
          aJacobian(0, 0) = 1.0;
          aJacobian.block<2, 1>(1, 0) = -sign * vecWd; // from curvature
          anIndex[0] = nLocals + 1;
        }
        aJacobian.block<2, 2>(1, index1) = -sign * matWJ; // from 1st Offset
        aJacobian.block<2, 2>(1, index2) = sign * matW; // from 2nd Offset
        for (unsigned int i = 0; i < nDim; ++i) {
          anIndex[index2 + theDimension[i]] = iOff2 + i;
        }
      }
    }
  }


  void GblTrajectory::getFitToKinkJacobian(std::array<unsigned int, 7>& anIndex,
                                           Matrix27d &aJacobian, const GblPoint &aPoint) const {

    unsigned int nDim = theDimension.size();
    unsigned int nCurv = numCurvature;
    unsigned int nLocals = numLocals;

    int nOffset = aPoint.getOffset();

    anIndex = {};
    aJacobian.setZero();

    Matrix2d prevW, prevWJ, nextW, nextWJ;
    Vector2d prevWd, nextWd;
    aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
    aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
    const Matrix2d sumWJ(prevWJ + nextWJ); // W- * J- + W+ * J+
    const Vector2d sumWd(prevWd + nextWd); // W+ * d+ + W- * d-

    unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)

    // local offset
    if (nCurv > 0) {
      aJacobian.block<2, 1>(0, 0) = -sumWd; // from curvature
      anIndex[0] = nLocals + 1;
    }
    aJacobian.block<2, 2>(0, 1) = prevW; // from 1st Offset
    aJacobian.block<2, 2>(0, 3) = -sumWJ; // from 2nd Offset
    aJacobian.block<2, 2>(0, 5) = nextW; // from 1st Offset
    for (unsigned int i = 0; i < nDim; ++i) {
      anIndex[1 + theDimension[i]] = iOff + i;
      anIndex[3 + theDimension[i]] = iOff + nDim + i;
      anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i;
    }
  }


  unsigned int GblTrajectory::getResults(int aSignedLabel, VectorXd &localPar,
                                         MatrixXd &localCov) const {
    if (not fitOK)
      return 1;
    std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
      getJacobian(aSignedLabel);
    unsigned int nParBrl = indexAndJacobian.first.size();
    VectorXd aVec(nParBrl); // compressed vector
    for (unsigned int i = 0; i < nParBrl; ++i) {
      aVec[i] = theVector(indexAndJacobian.first[i] - 1);
    }
    MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
    localPar = indexAndJacobian.second * aVec;
    localCov = indexAndJacobian.second * aMat
      * indexAndJacobian.second.adjoint();
    return 0;
  }


  unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
                                             unsigned int &numData, VectorXd &aResiduals, VectorXd &aMeasErrors,
                                             VectorXd &aResErrors, VectorXd &aDownWeights) {
    numData = 0;
    if (not fitOK)
      return 1;

    unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
    numData = measDataIndex[aLabel] - firstData; // number of data blocks
    for (unsigned int i = 0; i < numData; ++i) {
      getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals(i),
                   aMeasErrors(i), aResErrors(i), aDownWeights(i));
    }
    return 0;
  }


  unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
                                             unsigned int &numData, VectorXd &aResiduals, VectorXd &aMeasErrors,
                                             VectorXd &aResErrors, VectorXd &aDownWeights) {
    numData = 0;
    if (not fitOK)
      return 1;

    unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
    numData = scatDataIndex[aLabel] - firstData; // number of data blocks
    for (unsigned int i = 0; i < numData; ++i) {
      getResAndErr(firstData + i, true, aResiduals(i), aMeasErrors(i),
                   aResErrors(i), aDownWeights(i));
    }
    return 0;
  }

#ifdef GBL_EIGEN_SUPPORT_ROOT

  unsigned int GblTrajectory::getResults(int aSignedLabel, TVectorD &localPar,
                                         TMatrixDSym &localCov) const {
    if (not fitOK)
      return 1;
    std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
      getJacobian(aSignedLabel);
    unsigned int nParBrl = indexAndJacobian.first.size();
    VectorXd aVec(nParBrl); // compressed vector
    for (unsigned int i = 0; i < nParBrl; ++i) {
      aVec[i] = theVector(indexAndJacobian.first[i] - 1);
    }
    MatrixXd aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
    VectorXd aLocalPar = indexAndJacobian.second * aVec;
    MatrixXd aLocalCov = indexAndJacobian.second * aMat
      * indexAndJacobian.second.adjoint();
    // convert to ROOT
    unsigned int nParOut = localPar.GetNrows();
    for (unsigned int i = 0; i < nParOut; ++i) {
      localPar[i] = aLocalPar(i);
      for (unsigned int j = 0; j < nParOut; ++j) {
        localCov(i, j) = aLocalCov(i, j);
      }
    }
    return 0;
  }


  unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
                                             unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
                                             TVectorD &aResErrors, TVectorD &aDownWeights) {
    numData = 0;
    if (not fitOK)
      return 1;

    unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
    numData = measDataIndex[aLabel] - firstData; // number of data blocks
    for (unsigned int i = 0; i < numData; ++i) {
      getResAndErr(firstData + i, (aLabel != skippedMeasLabel), aResiduals[i],
                   aMeasErrors[i], aResErrors[i], aDownWeights[i]);
    }
    return 0;
  }


  unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
                                             unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
                                             TVectorD &aResErrors, TVectorD &aDownWeights) {
    numData = 0;
    if (not fitOK)
      return 1;

    unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
    numData = scatDataIndex[aLabel] - firstData; // number of data blocks
    for (unsigned int i = 0; i < numData; ++i) {
      getResAndErr(firstData + i, true, aResiduals[i], aMeasErrors[i],
                   aResErrors[i], aDownWeights[i]);
    }
    return 0;
  }

#endif


  unsigned int GblTrajectory::getLabels(std::vector<unsigned int> &aLabelList) {
    if (not constructOK)
      return 1;

    unsigned int aLabel = 0;
    unsigned int nPoint = thePoints[0].size();
    aLabelList.resize(nPoint);
    for (unsigned i = 0; i < nPoint; ++i) {
      aLabelList[i] = ++aLabel;
    }
    return 0;
  }


  unsigned int GblTrajectory::getLabels(
                                        std::vector<std::vector<unsigned int> > &aLabelList) {
    if (not constructOK)
      return 1;

    unsigned int aLabel = 0;
    aLabelList.resize(numTrajectories);
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      unsigned int nPoint = thePoints[iTraj].size();
      aLabelList[iTraj].resize(nPoint);
      for (unsigned i = 0; i < nPoint; ++i) {
        aLabelList[iTraj][i] = ++aLabel;
      }
    }
    return 0;
  }


  void GblTrajectory::getResAndErr(unsigned int aData, bool used,
                                   double &aResidual, double &aMeasError, double &aResError,
                                   double &aDownWeight) {

    double aMeasVar;
    unsigned int numLocal;
    unsigned int* indLocal;
    double* derLocal;
    theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, numLocal,
                               indLocal, derLocal);
    VectorXd aVec(numLocal); // compressed vector of derivatives
    for (unsigned int j = 0; j < numLocal; ++j) {
      aVec[j] = derLocal[j];
    }
    MatrixXd aMat = theMatrix.getBlockMatrix(numLocal, indLocal); // compressed (covariance) matrix
    double aFitVar = aVec.transpose() * aMat * aVec; // variance from track fit
    aMeasError = sqrt(aMeasVar); // error of measurement
    if (used)
      aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of (biased) residual
    else
      aResError = sqrt(aMeasVar + aFitVar); // error of (unbiased) residual
  }

  void GblTrajectory::buildLinearEquationSystem() {
    unsigned int nBorder = numCurvature + numLocals;
    theVector.resize(numParameters);
    theMatrix.resize(numParameters, nBorder);
    double aValue, aWeight;
    unsigned int* indLocal;
    double* derLocal;
    unsigned int numLocal;

    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData < theData.end(); ++itData) {
      // skipped (internal) measurement ?
      if (itData->getLabel() == skippedMeasLabel
          && itData->getType() == InternalMeasurement)
        continue;
      itData->getLocalData(aValue, aWeight, numLocal, indLocal, derLocal);
      for (unsigned int j = 0; j < numLocal; ++j) {
        theVector(indLocal[j] - 1) += derLocal[j] * aWeight * aValue;
      }
      theMatrix.addBlockMatrix(aWeight, numLocal, indLocal, derLocal);
    }
  }


  void GblTrajectory::prepare() {
    unsigned int nDim = theDimension.size();
    // upper limit
    unsigned int maxData = numMeasurements + nDim * (numOffsets - 2)
      + externalSeed.rows();
    theData.reserve(maxData);
    measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
    scatDataIndex.resize(numAllPoints + 1);
    unsigned int nData = 0;
    std::vector<MatrixXd> innerTransDer;
    std::vector<std::array<unsigned int, 5> > innerTransLab;
    // composed trajectory ?
    if (numInnerTrans > 0) {
      //std::cout << "composed trajectory" << std::endl;
      for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
        // innermost point
        GblPoint* innerPoint = &thePoints[iTraj].front();
        // transformation fit to local track parameters
        std::array<unsigned int, 5> firstLabels;
        Matrix5d matFitToLocal, matLocalToFit;
        getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
        // transformation local track to fit parameters
        matLocalToFit = matFitToLocal.inverse();
        // transformation external to fit parameters at inner (first) point
        innerTransDer.emplace_back(matLocalToFit * innerTransformations[iTraj]);
        innerTransLab.push_back(firstLabels);
      }
    }
    Matrix5d matP;              // measurements
    std::vector<GblPoint>::iterator itPoint;
    // limit the scope of proDer:
    {
      // transform for external parameters
      Eigen::Matrix<double, Eigen::Dynamic, 5,
                    Eigen::ColMajor /* default */, 5, 5> proDer;
      // loop over trajectories
      for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
        for (itPoint = thePoints[iTraj].begin();
             itPoint < thePoints[iTraj].end(); ++itPoint) {
          Vector5d aMeas, aPrec;
          unsigned int nLabel = itPoint->getLabel();
          unsigned int measDim = itPoint->hasMeasurement();
          if (measDim) {
            const MatrixXd localDer = itPoint->getLocalDerivatives();
            maxNumGlobals = std::max(maxNumGlobals,
                                     itPoint->getNumGlobals());
            MatrixXd transDer;
            itPoint->getMeasurement(matP, aMeas, aPrec);
            double minPrecision = itPoint->getMeasPrecMin();
            unsigned int iOff = 5 - measDim; // first active component
            std::array<unsigned int, 5> labDer;
            Matrix5d matDer, matPDer;
            unsigned int nJacobian =
              (itPoint < thePoints[iTraj].end() - 1) ? 1 : 0; // last point needs backward propagation
            getFitToLocalJacobian(labDer, matDer, *itPoint, measDim,
                                  nJacobian);
            if (measDim > 2) {
              matPDer = matP * matDer;
            } else { // 'shortcut' for position measurements
              matPDer.setZero();
              matPDer.block<2, 5>(3, 0) = matP.block<2, 2>(3, 3)
                * matDer.block<2, 5>(3, 0);
            }

            if (numInnerTrans > 0) {
              // transform for external parameters
              proDer.resize(measDim, Eigen::NoChange);
              proDer.setZero();
              // match parameters
              unsigned int ifirst = 0;
              unsigned int ilabel = 0;
              while (ilabel < 5) {
                if (labDer[ilabel] > 0) {
                  while (innerTransLab[iTraj][ifirst]
                         != labDer[ilabel] and ifirst < 5) {
                    ++ifirst;
                  }
                  if (ifirst >= 5) {
                    labDer[ilabel] -= 2 * nDim * (iTraj + 1); // adjust label
                  } else {
                    // match
                    labDer[ilabel] = 0; // mark as related to external parameters
                    for (unsigned int k = iOff; k < 5; ++k) {
                      proDer(k - iOff, ifirst) = matPDer(k,
                                                         ilabel);
                    }
                  }
                }
                ++ilabel;
              }
              transDer.resize(measDim, numCurvature);
              transDer = proDer * innerTransDer[iTraj];
            }
            for (unsigned int i = iOff; i < 5; ++i) {
              if (aPrec(i) > minPrecision) {
                GblData aData(nLabel, InternalMeasurement, aMeas(i),
                              aPrec(i), iTraj,
                              itPoint - thePoints[iTraj].begin());
                aData.addDerivatives(i, labDer, matPDer, iOff, localDer,
                                     numLocals, transDer);
                theData.emplace_back(aData);
                nData++;
              }
            }

          }
          measDataIndex[nLabel] = nData;
        }
      }
    } // end of scope for proDer

    Matrix2d matT;              // measurements
    // limit the scope of proDer:
    {
      // transform for external parameters
      Eigen::Matrix<double, Eigen::Dynamic, 5,
                    Eigen::ColMajor /* default */, 5, 5> proDer;
      scatDataIndex[0] = nData;
      scatDataIndex[1] = nData;
      // loop over trajectories
      for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
        for (itPoint = thePoints[iTraj].begin() + 1;
             itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
          Vector2d aMeas, aPrec;
          unsigned int nLabel = itPoint->getLabel();
          if (itPoint->hasScatterer()) {
            itPoint->getScatterer(matT, aMeas, aPrec);
            MatrixXd transDer;
            std::array<unsigned int, 7> labDer;
            Matrix27d matDer, matTDer;
            getFitToKinkJacobian(labDer, matDer, *itPoint);
            matTDer = matT * matDer;
            if (numInnerTrans > 0) {
              // transform for external parameters
              proDer.resize(nDim, Eigen::NoChange);
              proDer.setZero();
              // match parameters
              unsigned int ifirst = 0;
              unsigned int ilabel = 0;
              while (ilabel < 7) {
                if (labDer[ilabel] > 0) {
                  while (innerTransLab[iTraj][ifirst]
                         != labDer[ilabel] and ifirst < 5) {
                    ++ifirst;
                  }
                  if (ifirst >= 5) {
                    labDer[ilabel] -= 2 * nDim * (iTraj + 1); // adjust label
                  } else {
                    // match
                    labDer[ilabel] = 0; // mark as related to external parameters
                    for (unsigned int k = 0; k < nDim; ++k) {
                      proDer(k, ifirst) = matTDer(k, ilabel);
                    }
                  }
                }
                ++ilabel;
              }
              transDer.resize(nDim, numCurvature);
              transDer = proDer * innerTransDer[iTraj];
            }
            for (unsigned int i = 0; i < nDim; ++i) {
              unsigned int iDim = theDimension[i];
              if (aPrec(iDim) > 0.) {
                GblData aData(nLabel, InternalKink, aMeas(iDim),
                              aPrec(iDim), iTraj,
                              itPoint - thePoints[iTraj].begin());
                aData.addDerivatives(iDim, labDer, matTDer, numLocals,
                                     transDer);
                theData.emplace_back(aData);
                nData++;
              }
            }
          }
          scatDataIndex[nLabel] = nData;
        }
        scatDataIndex[thePoints[iTraj].back().getLabel()] = nData;
      }
    }

    // external seed
    if (externalPoint > 0) {
      std::pair<std::vector<unsigned int>, MatrixXd> indexAndJacobian =
        getJacobian(externalPoint);
      std::vector<unsigned int> externalSeedIndex = indexAndJacobian.first;
      std::vector<double> externalSeedDerivatives(externalSeedIndex.size());
      SelfAdjointEigenSolver<MatrixXd> externalSeedEigen(externalSeed);
      VectorXd valEigen = externalSeedEigen.eigenvalues();
      MatrixXd vecEigen = externalSeedEigen.eigenvectors();
      vecEigen = vecEigen.transpose() * indexAndJacobian.second;
      for (int i = 0; i < externalSeed.rows(); ++i) {
        if (valEigen(i) > 0.) {
          for (int j = 0; j < externalSeed.cols(); ++j) {
            externalSeedDerivatives[j] = vecEigen(i, j);
          }
          GblData aData(externalPoint, ExternalSeed, 0., valEigen(i));
          aData.addDerivatives(externalSeedIndex,
                               externalSeedDerivatives);
          theData.emplace_back(std::move(aData));
          nData++;
        }
      }
    }
    measDataIndex[numAllPoints + 1] = nData;
    // external measurements
    unsigned int nExt = externalMeasurements.rows();
    if (nExt > 0) {
      std::vector<unsigned int> index(numCurvature);
      std::vector<double> derivatives(numCurvature);
      for (unsigned int iExt = 0; iExt < nExt; ++iExt) {
        for (unsigned int iCol = 0; iCol < numCurvature; ++iCol) {
          index[iCol] = iCol + 1;
          derivatives[iCol] = externalDerivatives(iExt, iCol);
        }
        GblData aData(1U, ExternalMeasurement, externalMeasurements(iExt),
                      externalPrecisions(iExt));
        aData.addDerivatives(index, derivatives);
        theData.emplace_back(std::move(aData));
        nData++;
      }
    }
    measDataIndex[numAllPoints + 2] = nData;
  }

  void GblTrajectory::predict() {
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData < theData.end(); ++itData) {
      itData->setPrediction(theVector);
    }
  }


  double GblTrajectory::downWeight(unsigned int aMethod) {
    double aLoss = 0.;
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData < theData.end(); ++itData) {
      aLoss += (1. - itData->setDownWeighting(aMethod));
    }
    return aLoss;
  }


  unsigned int GblTrajectory::fit(double &Chi2, int &Ndf, double &lostWeight,
                                  const std::string& optionList, unsigned int aLabel) {
    const double normChi2[4] = { 1.0, 0.8737, 0.9326, 0.8228 };
    const std::string methodList = "TtHhCc";

    Chi2 = 0.;
    Ndf = -1;
    lostWeight = 0.;
    if (not constructOK)
      return 10;

    unsigned int aMethod = 0;
    skippedMeasLabel = aLabel;

    buildLinearEquationSystem();
    lostWeight = 0.;
    unsigned int ierr = 0;
    try {

      theMatrix.solveAndInvertBorderedBand(theVector, theVector);
      predict();

      for (unsigned int i = 0; i < optionList.size(); ++i) // down weighting iterations
        {
          size_t aPosition = methodList.find(optionList[i]);
          if (aPosition != std::string::npos) {
            aMethod = aPosition / 2 + 1;
            lostWeight = downWeight(aMethod);
            buildLinearEquationSystem();
            theMatrix.solveAndInvertBorderedBand(theVector, theVector);
            predict();
          }
        }
      Ndf = -numParameters;
      Chi2 = 0.;
      for (unsigned int i = 0; i < theData.size(); ++i) {
        // skipped (internal) measurement ?
        if (theData[i].getLabel() == skippedMeasLabel
            && theData[i].getType() == InternalMeasurement)
          continue;
        Chi2 += theData[i].getChi2();
        Ndf++;
      }
      Chi2 /= normChi2[aMethod];
      fitOK = true;

    } catch (int e) {
      std::cout << " GblTrajectory::fit exception " << e << std::endl;
      ierr = e;
    }
    return ierr;
  }

  void GblTrajectory::milleOut(MilleBinary &aMille) {
    double aValue;
    double aErr;
    unsigned int aTraj;
    unsigned int aPoint;
    unsigned int aRow;
    unsigned int numLocal;
    unsigned int* labLocal;
    double* derLocal;
    std::vector<int> labGlobal;
    std::vector<double> derGlobal;

    if (not constructOK)
      return;

    //   data: measurements, kinks and external seed
    labGlobal.reserve(maxNumGlobals);
    derGlobal.reserve(maxNumGlobals);
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData != theData.end(); ++itData) {
      itData->getAllData(aValue, aErr, numLocal, labLocal, derLocal, aTraj,
                         aPoint, aRow);
      if (itData->getType() == InternalMeasurement)
        thePoints[aTraj][aPoint].getGlobalLabelsAndDerivatives(aRow,
                                                               labGlobal, derGlobal);
      else
        labGlobal.resize(0);
      aMille.addData(aValue, aErr, numLocal, labLocal, derLocal, labGlobal,
                     derGlobal);
    }
    aMille.writeRecord();
  }


  void GblTrajectory::printTrajectory(unsigned int level) {
    if (numInnerTrans) {
      std::cout << "Composed GblTrajectory, " << numInnerTrans
                << " subtrajectories" << std::endl;
    } else {
      std::cout << "Simple GblTrajectory" << std::endl;
    }
    if (theDimension.size() < 2) {
      std::cout << " 2D-trajectory" << std::endl;
    }
    std::cout << " Number of GblPoints          : " << numAllPoints
              << std::endl;
    std::cout << " Number of points with offsets: " << numOffsets << std::endl;
    std::cout << " Number of fit parameters     : " << numParameters
              << std::endl;
    std::cout << " Number of measurements       : " << numMeasurements
              << std::endl;
    if (externalMeasurements.rows()) {
      std::cout << " Number of ext. measurements  : "
                << externalMeasurements.rows() << std::endl;
    }
    if (externalPoint) {
      std::cout << " Label of point with ext. seed: " << externalPoint
                << std::endl;
    }
    if (constructOK) {
      std::cout << " Constructed OK " << std::endl;
    }
    if (fitOK) {
      std::cout << " Fitted OK " << std::endl;
    }
    if (level > 0) {
      IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
      if (numInnerTrans) {
        std::cout << " Inner transformations" << std::endl;
        for (unsigned int i = 0; i < numInnerTrans; ++i) {
          std::cout << innerTransformations[i].format(CleanFmt)
                    << std::endl;
        }
      }
      if (externalMeasurements.rows()) {
        std::cout << " External measurements" << std::endl;
        std::cout << "  Measurements:" << std::endl;
        std::cout << externalMeasurements.format(CleanFmt) << std::endl;
        std::cout << "  Precisions:" << std::endl;
        std::cout << externalPrecisions.format(CleanFmt) << std::endl;
        std::cout << "  Derivatives:" << std::endl;
        std::cout << externalDerivatives.format(CleanFmt) << std::endl;
      }
      if (externalPoint) {
        std::cout << " External seed:" << std::endl;
        std::cout << externalSeed.format(CleanFmt) << std::endl;
      }
      if (fitOK) {
        std::cout << " Fit results" << std::endl;
        std::cout << "  Parameters:" << std::endl;
        theVector.print();
        std::cout << "  Covariance matrix (bordered band part):"
                  << std::endl;
        theMatrix.printMatrix();
      }
    }
  }


  void GblTrajectory::printPoints(unsigned int level) {
    std::cout << "GblPoints " << std::endl;
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
      std::vector<GblPoint>::iterator itPoint;
      for (itPoint = thePoints[iTraj].begin();
           itPoint < thePoints[iTraj].end(); ++itPoint) {
        itPoint->printPoint(level);
      }
    }
  }

  void GblTrajectory::printData() {
    std::cout << "GblData blocks " << std::endl;
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData < theData.end(); ++itData) {
      itData->printData();
    }
  }

}