GblMeasurement.cpp

Go to the documentation of this file.

/*
 * GBLMeasurement.cpp
 *
 *  Created on: 31 Mar 2021
 *      Author: kleinwrt
 */
 
#include "GblMeasurement.h"
using namespace Eigen;
 
namespace gbl {
 
 
GblMeasurement::GblMeasurement(const Eigen::MatrixXd &aProjection,
                const Eigen::VectorXd &aResiduals, const Eigen::MatrixXd &aPrecision,
                double minPrecision) {
        enabled = true;
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        if (aPrecision.cols() > 1) {
                // arbitrary precision matrix
                SelfAdjointEigenSolver<MatrixXd> measEigen(aPrecision);
                measTransformation = measEigen.eigenvectors();
                measTransformation.transposeInPlace();
                transFlag = true;
                measResiduals.tail(measDim) = measTransformation * aResiduals;
                measPrecision.tail(measDim) = measEigen.eigenvalues();
                measProjection.bottomRightCorner(measDim, measDim) = measTransformation
                                * aProjection;
        } else {
                // diagonal precision matrix
                transFlag = false;
                measResiduals.tail(measDim) = aResiduals;
                measPrecision.tail(measDim) = aPrecision;
                measProjection.bottomRightCorner(measDim, measDim) = aProjection;
        }
}
 
 
GblMeasurement::GblMeasurement(const Eigen::VectorXd &aResiduals,
                const Eigen::MatrixXd &aPrecision, double minPrecision) {
        enabled = true;
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        if (aPrecision.cols() > 1) {
                // arbitrary precision matrix
                SelfAdjointEigenSolver<MatrixXd> measEigen(aPrecision);
                measTransformation = measEigen.eigenvectors();
                measTransformation.transposeInPlace();
                transFlag = true;
                measResiduals.tail(measDim) = measTransformation * aResiduals;
                measPrecision.tail(measDim) = measEigen.eigenvalues();
                measProjection.bottomRightCorner(measDim, measDim) = measTransformation;
        } else {
                // diagonal precision matrix
                transFlag = false;
                measResiduals.tail(measDim) = aResiduals;
                measPrecision.tail(measDim) = aPrecision;
                measProjection.setIdentity();
        }
}
 
GblMeasurement::~GblMeasurement() {
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
GblMeasurement::GblMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TVectorD &aPrecision,
                double minPrecision) {
        enabled = true;
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        transFlag = false;
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = aResiduals[i];
                measPrecision(iOff + i) = aPrecision[i];
                for (unsigned int j = 0; j < measDim; ++j) {
                        measProjection(iOff + i, iOff + j) = aProjection(i, j);
                }
        }
}
 
 
GblMeasurement::GblMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TMatrixDSym &aPrecision,
                double minPrecision) {
        enabled = true;
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        TMatrixDSymEigen measEigen(aPrecision);
        TMatrixD tmpTransformation(measDim, measDim);
        tmpTransformation = measEigen.GetEigenVectors();
        tmpTransformation.T();
        transFlag = true;
        TVectorD transResiduals = tmpTransformation * aResiduals;
        TVectorD transPrecision = measEigen.GetEigenValues();
        TMatrixD transProjection = tmpTransformation * aProjection;
        measTransformation.resize(measDim, measDim);
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = transResiduals[i];
                measPrecision(iOff + i) = transPrecision[i];
                for (unsigned int j = 0; j < measDim; ++j) {
                        measTransformation(i, j) = tmpTransformation(i, j);
                        measProjection(iOff + i, iOff + j) = transProjection(i, j);
                }
        }
}
 
 
GblMeasurement::GblMeasurement(const TVectorD &aResiduals,
                const TVectorD &aPrecision, double minPrecision) {
        enabled = true;
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        transFlag = false;
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = aResiduals[i];
                measPrecision(iOff + i) = aPrecision[i];
        }
        measProjection.setIdentity();
}
 
 
GblMeasurement::GblMeasurement(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision, double minPrecision) {
        enabled = true;
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        TMatrixDSymEigen measEigen(aPrecision);
        TMatrixD tmpTransformation(measDim, measDim);
        tmpTransformation = measEigen.GetEigenVectors();
        tmpTransformation.T();
        transFlag = true;
        TVectorD transResiduals = tmpTransformation * aResiduals;
        TVectorD transPrecision = measEigen.GetEigenValues();
        measTransformation.resize(measDim, measDim);
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = transResiduals[i];
                measPrecision(iOff + i) = transPrecision[i];
                for (unsigned int j = 0; j < measDim; ++j) {
                        measTransformation(i, j) = tmpTransformation(i, j);
                        measProjection(iOff + i, iOff + j) = measTransformation(i, j);
                }
        }
}
#endif
 
 
void GblMeasurement::addLocals(const Eigen::MatrixXd &aDerivatives) {
        if (measDim) {
                localDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
                if (transFlag) {
                        localDerivatives = measTransformation * aDerivatives;
                } else {
                        localDerivatives = aDerivatives;
                }
        }
}
 
 
void GblMeasurement::addGlobals(const std::vector<int> &aLabels,
                const Eigen::MatrixXd &aDerivatives) {
        if (measDim) {
                globalLabels = aLabels;
                globalDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
                if (transFlag) {
                        globalDerivatives = measTransformation * aDerivatives;
                } else {
                        globalDerivatives = aDerivatives;
                }
 
        }
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblMeasurement::addLocals(const TMatrixD &aDerivatives) {
        if (measDim) {
                unsigned int numDer = aDerivatives.GetNcols();
                localDerivatives.resize(measDim, numDer);
                // convert from ROOT
                MatrixXd tmpDerivatives(measDim, numDer);
                for (unsigned int i = 0; i < measDim; ++i) {
                        for (unsigned int j = 0; j < numDer; ++j)
                                tmpDerivatives(i, j) = aDerivatives(i, j);
                }
                if (transFlag) {
                        localDerivatives = measTransformation * tmpDerivatives;
                } else {
                        localDerivatives = tmpDerivatives;
                }
        }
}
 
 
void GblMeasurement::addGlobals(const std::vector<int> &aLabels,
                const TMatrixD &aDerivatives) {
        if (measDim) {
                globalLabels = aLabels;
                unsigned int numDer = aDerivatives.GetNcols();
                globalDerivatives.resize(measDim, numDer);
                // convert from ROOT
                MatrixXd tmpDerivatives(measDim, numDer);
                for (unsigned int i = 0; i < measDim; ++i) {
                        for (unsigned int j = 0; j < numDer; ++j)
                                tmpDerivatives(i, j) = aDerivatives(i, j);
                }
                if (transFlag) {
                        globalDerivatives = measTransformation * tmpDerivatives;
                } else {
                        globalDerivatives = tmpDerivatives;
                }
        }
}
#endif
 
 
void GblMeasurement::setEnabled(bool flag) {
        enabled = flag;
}
 
 
bool GblMeasurement::isEnabled() const {
        return enabled;
}
 
 
unsigned int GblMeasurement::getMeasDim() const {
        return measDim;
}
 
 
double GblMeasurement::getMeasPrecMin() const {
        return measPrecMin;
}
 
 
void GblMeasurement::getMeasurement(Matrix5d &aProjection, Vector5d &aResiduals,
                Vector5d &aPrecision) const {
        aProjection.bottomRightCorner(measDim, measDim) =
                        measProjection.bottomRightCorner(measDim, measDim);
        aResiduals.tail(measDim) = measResiduals.tail(measDim);
        aPrecision.tail(measDim) = measPrecision.tail(measDim);
}
 
 
void GblMeasurement::getMeasTransformation(MatrixXd &aTransformation) const {
        aTransformation.resize(measDim, measDim);
        if (transFlag) {
                aTransformation = measTransformation;
        } else {
                aTransformation.setIdentity();
        }
}
 
unsigned int GblMeasurement::getNumLocals() const {
        return localDerivatives.cols();
}
 
const MatrixXd& GblMeasurement::getLocalDerivatives() const {
        return localDerivatives;
}
 
unsigned int GblMeasurement::getNumGlobals() const {
        return globalDerivatives.cols();
}
 
 
void GblMeasurement::getGlobalLabels(std::vector<int> &aLabels) const {
        aLabels = globalLabels;
}
 
 
void GblMeasurement::getGlobalDerivatives(MatrixXd &aDerivatives) const {
        aDerivatives = globalDerivatives;
}
 
 
void GblMeasurement::getGlobalLabelsAndDerivatives(unsigned int aRow,
                std::vector<int> &aLabels, std::vector<double> &aDerivatives) const {
        aLabels.resize(globalDerivatives.cols());
        aDerivatives.resize(globalDerivatives.cols());
        for (unsigned int i = 0; i < globalDerivatives.cols(); ++i) {
                aLabels[i] = globalLabels[i];
                aDerivatives[i] = globalDerivatives(aRow, i);
        }
}
 
 
void GblMeasurement::printMeasurement(unsigned int level) const {
        if (level > 0)
                std::cout << "  GblMeasurement";
 
        if (measDim) {
                std::cout << ", " << measDim << " dimensions";
        }
        if (!enabled) {
                std::cout << ", disabled";
        }
        if (transFlag) {
                std::cout << ", diagonalized";
        }
        if (localDerivatives.cols()) {
                std::cout << ", " << localDerivatives.cols() << " local derivatives";
        }
        if (globalDerivatives.cols()) {
                std::cout << ", " << globalDerivatives.cols() << " global derivatives";
        }
        if (level > 0) {
                std::cout << std::endl;
                IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
                if (measDim) {
                        std::cout << "   Projection: " << std::endl
                                        << measProjection.bottomRightCorner(measDim, measDim).format(
                                                        CleanFmt) << std::endl;
                        std::cout << "   Residuals: "
                                        << measResiduals.tail(measDim).transpose().format(CleanFmt)
                                        << std::endl;
                        std::cout << "   Precision (min.: " << measPrecMin << "): "
                                        << measPrecision.tail(measDim).transpose().format(CleanFmt)
                                        << std::endl;
                }
                if (localDerivatives.cols()) {
                        std::cout << "  Local Derivatives:" << std::endl
                                        << localDerivatives.format(CleanFmt) << std::endl;
                }
                if (globalDerivatives.cols()) {
                        std::cout << "  Global Labels:";
                        for (unsigned int i = 0; i < globalLabels.size(); ++i) {
                                std::cout << " " << globalLabels[i];
                        }
                        std::cout << std::endl;
                        std::cout << "  Global Derivatives:"
                                        << globalDerivatives.format(CleanFmt) << std::endl;
                }
        }
}
 
}