GblTrajectory.cc

Go to the documentation of this file.

/*
 * GblTrajectory.cpp
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */
 
#include "GblTrajectory.h"
 
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), 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.push_back(aPointList);
    numPoints.push_back(numAllPoints);
    construct(); // construct trajectory
}
 
 
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), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(
                aSeed), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
 
    if (flagU1dir)
        theDimension.push_back(0);
    if (flagU2dir)
        theDimension.push_back(1);
    // simple (single) trajectory
    thePoints.push_back(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), 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].GetNcols();
    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), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(
                extDerivatives), externalMeasurements(extMeasurements), externalPrecisions(
                extPrecisions) {
 
    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].GetNcols();
    construct(); // construct (composed) trajectory
}
 
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() {
 
    SMatrix55 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>, TMatrixD> 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);
    TMatrixD aJacobian(nParLoc, nParBRL);
    aJacobian.Zero();
 
    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::vector<unsigned int> labDer(5);
    SMatrix55 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::vector<unsigned int> &anIndex,
        SMatrix55 &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();
 
    if (nOffset < 0) // need interpolation
            {
        SMatrix22 prevW, prevWJ, nextW, nextWJ, matN;
        SVector2 prevWd, nextWd;
        int ierr;
        aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
        aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
        const SMatrix22 sumWJ(prevWJ + nextWJ);
        matN = sumWJ.Inverse(ierr); // N = (W- * J- + W+ * J+)^-1
        // derivatives for u_int
        const SMatrix22 prevNW(matN * prevW); // N * W-
        const SMatrix22 nextNW(matN * nextW); // N * W+
        const SVector2 prevNd(matN * prevWd); // N * W- * d-
        const SVector2 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.Place_in_col(-prevNd - nextNd, 3, 0); // from curvature
            anIndex[0] = nLocals + 1;
        }
        aJacobian.Place_at(prevNW, 3, 1); // from 1st Offset
        aJacobian.Place_at(nextNW, 3, 3); // 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 SMatrix22 prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
            const SMatrix22 nextWPN(prevWJ * nextNW); // W- * J- * N * W+
            const SVector2 prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
            const SVector2 nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
            if (nCurv > 0) {
                aJacobian(0, 0) = 1.0;
                aJacobian.Place_in_col(prevWNd - nextWNd, 1, 0); // from curvature
            }
            aJacobian.Place_at(-prevWPN, 1, 1); // from 1st Offset
            aJacobian.Place_at(nextWPN, 1, 3); // 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) {
            SMatrix22 matW, matWJ;
            SVector2 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.Place_in_col(-sign * vecWd, 1, 0); // from curvature
                anIndex[0] = nLocals + 1;
            }
            aJacobian.Place_at(-sign * matWJ, 1, index1); // from 1st Offset
            aJacobian.Place_at(sign * matW, 1, index2); // from 2nd Offset
            for (unsigned int i = 0; i < nDim; ++i) {
                anIndex[index2 + theDimension[i]] = iOff2 + i;
            }
        }
    }
}
 
 
void GblTrajectory::getFitToKinkJacobian(std::vector<unsigned int> &anIndex,
        SMatrix27 &aJacobian, const GblPoint &aPoint) const {
 
    unsigned int nDim = theDimension.size();
    unsigned int nCurv = numCurvature;
    unsigned int nLocals = numLocals;
 
    int nOffset = aPoint.getOffset();
 
    SMatrix22 prevW, prevWJ, nextW, nextWJ;
    SVector2 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 SMatrix22 sumWJ(prevWJ + nextWJ); // W- * J- + W+ * J+
    const SVector2 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.Place_in_col(-sumWd, 0, 0); // from curvature
        anIndex[0] = nLocals + 1;
    }
    aJacobian.Place_at(prevW, 0, 1); // from 1st Offset
    aJacobian.Place_at(-sumWJ, 0, 3); // from 2nd Offset
    aJacobian.Place_at(nextW, 0, 5); // 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, TVectorD &localPar,
        TMatrixDSym &localCov) const {
    if (not fitOK)
        return 1;
    std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian =
            getJacobian(aSignedLabel);
    unsigned int nParBrl = indexAndJacobian.first.size();
    TVectorD aVec(nParBrl); // compressed vector
    for (unsigned int i = 0; i < nParBrl; ++i) {
        aVec[i] = theVector(indexAndJacobian.first[i] - 1);
    }
    TMatrixDSym aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
    localPar = indexAndJacobian.second * aVec;
    localCov = aMat.Similarity(indexAndJacobian.second);
    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, 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, aResiduals[i], aMeasErrors[i],
                aResErrors[i], aDownWeights[i]);
    }
    return 0;
}
 
 
void GblTrajectory::getLabels(std::vector<unsigned int> &aLabelList) {
    unsigned int aLabel = 0;
    unsigned int nPoint = thePoints[0].size();
    aLabelList.resize(nPoint);
    for (unsigned i = 0; i < nPoint; ++i) {
        aLabelList[i] = ++aLabel;
    }
}
 
 
void GblTrajectory::getLabels(
        std::vector<std::vector<unsigned int> > &aLabelList) {
    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;
        }
    }
}
 
 
void GblTrajectory::getResAndErr(unsigned int aData, double &aResidual,
        double &aMeasError, double &aResError, double &aDownWeight) {
 
    double aMeasVar;
    std::vector<unsigned int>* indLocal;
    std::vector<double>* derLocal;
    theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, indLocal,
            derLocal);
    unsigned int nParBrl = (*indLocal).size();
    TVectorD aVec(nParBrl); // compressed vector of derivatives
    for (unsigned int j = 0; j < nParBrl; ++j) {
        aVec[j] = (*derLocal)[j];
    }
    TMatrixDSym aMat = theMatrix.getBlockMatrix(*indLocal); // compressed (covariance) matrix
    double aFitVar = aMat.Similarity(aVec); // variance from track fit
    aMeasError = sqrt(aMeasVar); // error of measurement
    aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of residual
}
 
void GblTrajectory::buildLinearEquationSystem() {
    unsigned int nBorder = numCurvature + numLocals;
    theVector.resize(numParameters);
    theMatrix.resize(numParameters, nBorder);
    double aValue, aWeight;
    std::vector<unsigned int>* indLocal;
    std::vector<double>* derLocal;
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData < theData.end(); ++itData) {
        itData->getLocalData(aValue, aWeight, indLocal, derLocal);
        for (unsigned int j = 0; j < indLocal->size(); ++j) {
            theVector((*indLocal)[j] - 1) += (*derLocal)[j] * aWeight * aValue;
        }
        theMatrix.addBlockMatrix(aWeight, indLocal, derLocal);
    }
}
 
 
void GblTrajectory::prepare() {
    unsigned int nDim = theDimension.size();
    // upper limit
    unsigned int maxData = numMeasurements + nDim * (numOffsets - 2)
            + externalSeed.GetNrows();
    theData.reserve(maxData);
    measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
    scatDataIndex.resize(numAllPoints + 1);
    unsigned int nData = 0;
    std::vector<TMatrixD> innerTransDer;
    std::vector<std::vector<unsigned int> > 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::vector<unsigned int> firstLabels(5);
            SMatrix55 matFitToLocal, matLocalToFit;
            getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
            // transformation local track to fit parameters
            int ierr;
            matLocalToFit = matFitToLocal.Inverse(ierr);
            TMatrixD localToFit(5, 5);
            for (unsigned int i = 0; i < 5; ++i) {
                for (unsigned int j = 0; j < 5; ++j) {
                    localToFit(i, j) = matLocalToFit(i, j);
                }
            }
            // transformation external to fit parameters at inner (first) point
            innerTransDer.push_back(localToFit * innerTransformations[iTraj]);
            innerTransLab.push_back(firstLabels);
        }
    }
    // measurements
    SMatrix55 matP;
    // loop over trajectories
    std::vector<GblPoint>::iterator itPoint;
    for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
        for (itPoint = thePoints[iTraj].begin();
                itPoint < thePoints[iTraj].end(); ++itPoint) {
            SVector5 aMeas, aPrec;
            unsigned int nLabel = itPoint->getLabel();
            unsigned int measDim = itPoint->hasMeasurement();
            if (measDim) {
                const TMatrixD localDer = itPoint->getLocalDerivatives();
                const std::vector<int> globalLab = itPoint->getGlobalLabels();
                const TMatrixD globalDer = itPoint->getGlobalDerivatives();
                TMatrixD transDer;
                itPoint->getMeasurement(matP, aMeas, aPrec);
                unsigned int iOff = 5 - measDim; // first active component
                std::vector<unsigned int> labDer(5);
                SMatrix55 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.Place_at(
                            matP.Sub<SMatrix22>(3, 3)
                                    * matDer.Sub<SMatrix25>(3, 0), 3, 0);
                }
 
                if (numInnerTrans > 0) {
                    // transform for external parameters
                    TMatrixD proDer(measDim, 5);
                    // 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.ResizeTo(measDim, numCurvature);
                    transDer = proDer * innerTransDer[iTraj];
                }
                for (unsigned int i = iOff; i < 5; ++i) {
                    if (aPrec(i) > 0.) {
                        GblData aData(nLabel, aMeas(i), aPrec(i));
                        aData.addDerivatives(i, labDer, matPDer, iOff, localDer,
                                globalLab, globalDer, numLocals, transDer);
                        theData.push_back(aData);
                        nData++;
                    }
                }
 
            }
            measDataIndex[nLabel] = nData;
        }
    }
 
    // pseudo measurements from kinks
    SMatrix22 matT;
    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) {
            SVector2 aMeas, aPrec;
            unsigned int nLabel = itPoint->getLabel();
            if (itPoint->hasScatterer()) {
                itPoint->getScatterer(matT, aMeas, aPrec);
                TMatrixD transDer;
                std::vector<unsigned int> labDer(7);
                SMatrix27 matDer, matTDer;
                getFitToKinkJacobian(labDer, matDer, *itPoint);
                matTDer = matT * matDer;
                if (numInnerTrans > 0) {
                    // transform for external parameters
                    TMatrixD proDer(nDim, 5);
                    // 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.ResizeTo(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, aMeas(iDim), aPrec(iDim));
                        aData.addDerivatives(iDim, labDer, matTDer, numLocals,
                                transDer);
                        theData.push_back(aData);
                        nData++;
                    }
                }
            }
            scatDataIndex[nLabel] = nData;
        }
        scatDataIndex[thePoints[iTraj].back().getLabel()] = nData;
    }
 
    // external seed
    if (externalPoint > 0) {
        std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian =
                getJacobian(externalPoint);
        std::vector<unsigned int> externalSeedIndex = indexAndJacobian.first;
        std::vector<double> externalSeedDerivatives(externalSeedIndex.size());
        const TMatrixDSymEigen externalSeedEigen(externalSeed);
        const TVectorD valEigen = externalSeedEigen.GetEigenValues();
        TMatrixD vecEigen = externalSeedEigen.GetEigenVectors();
        vecEigen = vecEigen.T() * indexAndJacobian.second;
        for (int i = 0; i < externalSeed.GetNrows(); ++i) {
            if (valEigen(i) > 0.) {
                for (int j = 0; j < externalSeed.GetNcols(); ++j) {
                    externalSeedDerivatives[j] = vecEigen(i, j);
                }
                GblData aData(externalPoint, 0., valEigen(i));
                aData.addDerivatives(externalSeedIndex, externalSeedDerivatives);
                theData.push_back(aData);
                nData++;
            }
        }
    }
    measDataIndex[numAllPoints + 1] = nData;
    // external measurements
    unsigned int nExt = externalMeasurements.GetNrows();
    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, externalMeasurements(iExt),
                    externalPrecisions(iExt));
            aData.addDerivatives(index, derivatives);
            theData.push_back(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,
        std::string optionList) {
    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;
 
    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 = theData.size() - numParameters;
        Chi2 = 0.;
        for (unsigned int i = 0; i < theData.size(); ++i) {
            Chi2 += theData[i].getChi2();
        }
        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;
    std::vector<unsigned int>* indLocal;
    std::vector<double>* derLocal;
    std::vector<int>* labGlobal;
    std::vector<double>* derGlobal;
 
    if (not constructOK)
        return;
 
//   data: measurements, kinks and external seed
    std::vector<GblData>::iterator itData;
    for (itData = theData.begin(); itData != theData.end(); ++itData) {
        itData->getAllData(aValue, aErr, indLocal, derLocal, labGlobal,
                derGlobal);
        aMille.addData(aValue, aErr, *indLocal, *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.GetNrows()) {
        std::cout << " Number of ext. measurements  : "
                << externalMeasurements.GetNrows() << 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) {
        if (numInnerTrans) {
            std::cout << " Inner transformations" << std::endl;
            for (unsigned int i = 0; i < numInnerTrans; ++i) {
                innerTransformations[i].Print();
            }
        }
        if (externalMeasurements.GetNrows()) {
            std::cout << " External measurements" << std::endl;
            std::cout << "  Measurements:" << std::endl;
            externalMeasurements.Print();
            std::cout << "  Precisions:" << std::endl;
            externalPrecisions.Print();
            std::cout << "  Derivatives:" << std::endl;
            externalDerivatives.Print();
        }
        if (externalPoint) {
            std::cout << " External seed:" << std::endl;
            externalSeed.Print();
        }
        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();
    }
}
 
}