GblTrajectory.h

Go to the documentation of this file.

/*
 * GblTrajectory.h
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */
 
#ifndef GBLTRAJECTORY_H_
#define GBLTRAJECTORY_H_
 
#include <array>
#include "GblPoint.h"
#include "GblData.h"
#include "GblPoint.h"
#include "BorderedBandMatrix.h"
#include "MilleBinary.h"
 
namespace gbl {
 
 
class GblTrajectory {
public:
        GblTrajectory(const std::vector<GblPoint> &aPointList, bool flagCurv = true,
                        bool flagU1dir = true, bool flagU2dir = true);
        GblTrajectory(
                        const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList);
 
 
        template<typename Seed>
        GblTrajectory(const std::vector<GblPoint> &aPointList, unsigned int aLabel,
                        const Eigen::MatrixBase<Seed>& aSeed, bool flagCurv = true,
                        bool flagU1dir = true, bool flagU2dir = true);
 
 
        template<typename Derivatives, typename Measurements, typename Precision,
                        typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type* =
                                        nullptr>
        GblTrajectory(
                        const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList,
                        const Eigen::MatrixBase<Derivatives>& extDerivatives,
                        const Eigen::MatrixBase<Measurements>& extMeasurements,
                        const Eigen::MatrixBase<Precision>& extPrecisions);
 
 
        template<typename Derivatives, typename Measurements, typename Precision,
                        typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type* =
                                        nullptr>
        GblTrajectory(
                        const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList,
                        const Eigen::MatrixBase<Derivatives>& extDerivatives,
                        const Eigen::MatrixBase<Measurements>& extMeasurements,
                        const Eigen::MatrixBase<Precision>& extPrecisions);
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
        // input from ROOT
        GblTrajectory(const std::vector<GblPoint> &aPointList, unsigned int aLabel,
                        const TMatrixDSym &aSeed, bool flagCurv = true, bool flagU1dir =
                        true, bool flagU2dir = true);
        GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList);
        GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
                        const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
                        const TVectorD &extPrecisions);
        GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
                        const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
                        const TMatrixDSym &extPrecisions);
#endif
        virtual ~GblTrajectory();
        bool isValid() const;
        unsigned int getNumPoints() const;
        unsigned int getResults(int aSignedLabel, Eigen::VectorXd &localPar,
                        Eigen::MatrixXd &localCov) const;
        unsigned int getMeasResults(unsigned int aLabel, unsigned int &numData,
                        Eigen::VectorXd &aResiduals, Eigen::VectorXd &aMeasErrors,
                        Eigen::VectorXd &aResErrors, Eigen::VectorXd &aDownWeights);
        unsigned int getScatResults(unsigned int aLabel, unsigned int &numData,
                        Eigen::VectorXd &aResiduals, Eigen::VectorXd &aMeasErrors,
                        Eigen::VectorXd &aResErrors, Eigen::VectorXd &aDownWeights);
#ifdef GBL_EIGEN_SUPPORT_ROOT
        // input from ROOT
        unsigned int getResults(int aSignedLabel, TVectorD &localPar,
                        TMatrixDSym &localCov) const;
        unsigned int getMeasResults(unsigned int aLabel, unsigned int &numData,
                        TVectorD &aResiduals, TVectorD &aMeasErrors, TVectorD &aResErrors,
                        TVectorD &aDownWeights);
        unsigned int getScatResults(unsigned int aLabel, unsigned int &numData,
                        TVectorD &aResiduals, TVectorD &aMeasErrors, TVectorD &aResErrors,
                        TVectorD &aDownWeights);
#endif
        unsigned int getLabels(std::vector<unsigned int> &aLabelList) const;
        unsigned int getLabels(
                        std::vector<std::vector<unsigned int> > &aLabelList) const;
        unsigned int fit(double &Chi2, int &Ndf, double &lostWeight,
                        const std::string& optionList = "", unsigned int aLabel = 0);
        void milleOut(MilleBinary &aMille);
        void printTrajectory(unsigned int level = 0) const;
        void printPoints(unsigned int level = 0) const;
        void printData() const;
 
private:
        unsigned int numAllPoints; 
        std::vector<unsigned int> numPoints; 
        unsigned int numTrajectories; 
        unsigned int numOffsets; 
        unsigned int numInnerTransformations; 
        unsigned int numInnerTransOffsets; 
        unsigned int numCurvature; 
        unsigned int numParameters; 
        unsigned int numLocals; 
        unsigned int numMeasurements; 
        unsigned int externalPoint; 
        unsigned int skippedMeasLabel; 
        unsigned int maxNumGlobals; 
        bool constructOK; 
        bool fitOK; 
        std::vector<unsigned int> theDimension; 
        std::vector<std::vector<GblPoint> > thePoints; 
        std::vector<GblData> theData; 
        std::vector<unsigned int> measDataIndex; 
        std::vector<unsigned int> scatDataIndex; 
        Eigen::MatrixXd externalSeed; 
        // composed trajectory
        std::vector<Eigen::MatrixXd> innerTransformations; 
        std::vector<Eigen::MatrixXd> innerTransDer; 
        std::vector<std::array<unsigned int, 5> > innerTransLab; 
        Eigen::MatrixXd externalDerivatives; 
        Eigen::VectorXd externalMeasurements; 
        Eigen::VectorXd externalPrecisions; 
        // linear equation system
        VVector theVector; 
        BorderedBandMatrix theMatrix; 
 
        std::pair<std::vector<unsigned int>, Eigen::MatrixXd> getJacobian(
                        int aSignedLabel) const;
        void getFitToLocalJacobian(std::array<unsigned int, 5>& anIndex,
                        Matrix5d &aJacobian, const GblPoint &aPoint, unsigned int measDim,
                        unsigned int nJacobian = 1) const;
        void getFitToKinkJacobian(std::array<unsigned int, 7>& anIndex,
                        Matrix27d &aJacobian, const GblPoint &aPoint) const;
        void construct();
        void defineOffsets();
        void calcJacobians();
        void prepare();
        void buildLinearEquationSystem();
        void predict();
        double downWeight(unsigned int aMethod);
        void getResAndErr(unsigned int aData, bool used, double &aResidual,
                        double &aMeadsError, double &aResError, double &aDownWeight);
};
 
template<typename Seed>
GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
                unsigned int aLabel, const Eigen::MatrixBase<Seed>& aSeed,
                bool flagCurv, bool flagU1dir, bool flagU2dir) :
                numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTransformations(
                                0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(
                                0), numLocals(0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(
                                0), maxNumGlobals(0), 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
}
 
template<typename Derivatives, typename Measurements, typename Precision,
                typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type*>
GblTrajectory::GblTrajectory(
                const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList,
                const Eigen::MatrixBase<Derivatives>& extDerivatives,
                const Eigen::MatrixBase<Measurements>& extMeasurements,
                const Eigen::MatrixBase<Precision>& extPrecisions) :
                numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
                                aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
                                0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
                                0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
 
        static_assert(static_cast<int>(Measurements::ColsAtCompileTime) == 1, "GblTrajectory: cols(Measurements) must be 1 (vector)");
        static_assert(static_cast<int>(Measurements::RowsAtCompileTime) == static_cast<int>(Derivatives::RowsAtCompileTime), "GblTrajectory: rows(Measurements) and rows(Derivatives) must be equal");
        static_assert(static_cast<int>(Measurements::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "GblTrajectory: rows(Measurements) and rows(Precision) must be equal");
        static_assert(static_cast<int>(Precision::RowsAtCompileTime) == static_cast<int>(Precision::ColsAtCompileTime), "GblTrajectory: rows(Precision) and cols(Precision) must be equal");
        // diagonalize external measurement
        Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> extEigen {
                        extPrecisions };
        // @TODO   if (extEigen.info() != Success) abort();
        auto extTransformation = extEigen.eigenvectors().transpose();
        externalDerivatives.resize(extDerivatives.rows(), extDerivatives.cols());
        externalDerivatives = extTransformation * extDerivatives;
        externalMeasurements.resize(extMeasurements.size());
        externalMeasurements = extTransformation * extMeasurements;
        externalPrecisions.resize(extMeasurements.size());
        externalPrecisions = extEigen.eigenvalues();
 
        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);
        // kinematic (2) or geometric (1) constraint
        numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
        numCurvature =
                        innerTransformations[0].rows() == 5 ?
                                        innerTransformations[0].cols() :
                                        innerTransformations[0].cols() + numInnerTransformations;
        construct(); // construct (composed) trajectory
}
 
template<typename Derivatives, typename Measurements, typename Precision,
                typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type*>
GblTrajectory::GblTrajectory(
                const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList,
                const Eigen::MatrixBase<Derivatives>& extDerivatives,
                const Eigen::MatrixBase<Measurements>& extMeasurements,
                const Eigen::MatrixBase<Precision>& extPrecisions) :
                numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
                                aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
                                0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
                                0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
        static_assert(static_cast<int>(Measurements::ColsAtCompileTime) == 1, "GblTrajectory: cols(Measurements) must be 1 (vector)");
        static_assert(static_cast<int>(Measurements::RowsAtCompileTime) == static_cast<int>(Derivatives::RowsAtCompileTime), "GblTrajectory: rows(Measurements) and rows(Derivatives) must be equal");
        static_assert(static_cast<int>(Measurements::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "GblTrajectory: rows(Measurements) and rows(Precision) must be equal");
 
        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);
        // kinematic (2) or geometric (1) constraint
        numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
        numCurvature =
                        innerTransformations[0].rows() == 5 ?
                                        innerTransformations[0].cols() :
                                        innerTransformations[0].cols() + numInnerTransformations;
        construct(); // construct (composed) trajectory
}
 
}
#endif /* GBLTRAJECTORY_H_ */