GblPoint.cpp

Go to the documentation of this file.

/*
 * GblPoint.cpp
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */
 
#include "GblPoint.h"
using namespace Eigen;
 
namespace gbl {
 
 
GblPoint::GblPoint(const Matrix5d &aJacobian) :
                theLabel(0), theOffset(0), p2pJacobian(aJacobian), measDim(0), measPrecMin(
                                0.), transFlag(false), measTransformation(), scatFlag(false), localDerivatives(), globalLabels(), globalDerivatives() {
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
GblPoint::GblPoint(const TMatrixD &aJacobian) :
theLabel(0), theOffset(0), measDim(0), measPrecMin(0.), transFlag(false),
measTransformation(), scatFlag(false), localDerivatives(), globalLabels(),
globalDerivatives() {
 
        for (unsigned int i = 0; i < 5; ++i) {
                for (unsigned int j = 0; j < 5; ++j) {
                        p2pJacobian(i, j) = aJacobian(i, j);
                }
        }
}
#endif
 
GblPoint::~GblPoint() {
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::addMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TVectorD &aPrecision,
                double minPrecision) {
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        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);
                }
        }
}
 
 
void GblPoint::addMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TMatrixDSym &aPrecision,
                double minPrecision) {
        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);
                }
        }
}
 
 
void GblPoint::addMeasurement(const TVectorD &aResiduals,
                const TVectorD &aPrecision, double minPrecision) {
        measDim = aResiduals.GetNrows();
        measPrecMin = minPrecision;
        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();
}
 
 
void GblPoint::addMeasurement(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision, double minPrecision) {
        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
 
 
unsigned int GblPoint::hasMeasurement() const {
        return measDim;
}
 
 
double GblPoint::getMeasPrecMin() const {
        return measPrecMin;
}
 
 
void GblPoint::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 GblPoint::getMeasTransformation(MatrixXd &aTransformation) const {
        aTransformation.resize(measDim, measDim);
        if (transFlag) {
                aTransformation = measTransformation;
        } else {
                aTransformation.setIdentity();
        }
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::addScatterer(const TVectorD &aResiduals,
                const TVectorD &aPrecision) {
        scatFlag = true;
        scatResiduals(0) = aResiduals[0];
        scatResiduals(1) = aResiduals[1];
        scatPrecision(0) = aPrecision[0];
        scatPrecision(1) = aPrecision[1];
        scatTransformation.setIdentity();
}
 
 
void GblPoint::addScatterer(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision) {
        scatFlag = true;
        TMatrixDSymEigen scatEigen(aPrecision);
        TMatrixD aTransformation = scatEigen.GetEigenVectors();
        aTransformation.T();
        TVectorD transResiduals = aTransformation * aResiduals;
        TVectorD transPrecision = scatEigen.GetEigenValues();
        scatTransformation.resize(2, 2);
        for (unsigned int i = 0; i < 2; ++i) {
                scatResiduals(i) = transResiduals[i];
                scatPrecision(i) = transPrecision[i];
                for (unsigned int j = 0; j < 2; ++j) {
                        scatTransformation(i, j) = aTransformation(i, j);
                }
        }
}
#endif
 
bool GblPoint::hasScatterer() const {
        return scatFlag;
}
 
 
void GblPoint::getScatterer(Matrix2d &aTransformation, Vector2d &aResiduals,
                Vector2d &aPrecision) const {
        aTransformation = scatTransformation;
        aResiduals = scatResiduals;
        aPrecision = scatPrecision;
}
 
 
void GblPoint::getScatTransformation(Matrix2d &aTransformation) const {
        if (scatFlag) {
                aTransformation = scatTransformation;
        } else {
                aTransformation.setIdentity();
        }
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::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;
                }
        }
}
#endif
 
unsigned int GblPoint::getNumLocals() const {
        return localDerivatives.cols();
}
 
const MatrixXd& GblPoint::getLocalDerivatives() const {
        return localDerivatives;
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::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
 
unsigned int GblPoint::getNumGlobals() const {
        return globalDerivatives.cols();
}
 
 
void GblPoint::getGlobalLabels(std::vector<int> &aLabels) const {
        aLabels = globalLabels;
}
 
 
void GblPoint::getGlobalDerivatives(MatrixXd &aDerivatives) const {
        aDerivatives = globalDerivatives;
}
 
 
void GblPoint::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 GblPoint::setLabel(unsigned int aLabel) {
        theLabel = aLabel;
}
 
unsigned int GblPoint::getLabel() const {
        return theLabel;
}
 
 
void GblPoint::setOffset(int anOffset) {
        theOffset = anOffset;
}
 
int GblPoint::getOffset() const {
        return theOffset;
}
 
const Matrix5d& GblPoint::getP2pJacobian() const {
        return p2pJacobian;
}
 
 
void GblPoint::addPrevJacobian(const Matrix5d &aJac) {
        // to optimize: need only two last rows of inverse
        //  prevJacobian = aJac.inverse();
        //  block matrix algebra
        Matrix23d CA = aJac.block<2, 3>(3, 0) * aJac.block<3, 3>(0, 0).inverse(); // C*A^-1
        Matrix2d DCAB = aJac.block<2, 2>(3, 3) - CA * aJac.block<3, 2>(0, 3); // D - C*A^-1 *B
        Matrix2d DCABInv = DCAB.inverse();
        prevJacobian.block<2, 2>(3, 3) = DCABInv;
        prevJacobian.block<2, 3>(3, 0) = -DCABInv * CA;
}
 
 
void GblPoint::addNextJacobian(const Matrix5d &aJac) {
        nextJacobian = aJac;
}
 
 
void GblPoint::getDerivatives(int aDirection, Matrix2d &matW, Matrix2d &matWJ,
                Vector2d &vecWd) const {
 
        Matrix2d matJ;
        Vector2d vecd;
        if (aDirection < 1) {
                matJ = prevJacobian.block<2, 2>(3, 3);
                matW = -prevJacobian.block<2, 2>(3, 1);
                vecd = prevJacobian.block<2, 1>(3, 0);
        } else {
                matJ = nextJacobian.block<2, 2>(3, 3);
                matW = nextJacobian.block<2, 2>(3, 1);
                vecd = nextJacobian.block<2, 1>(3, 0);
        }
 
        if (!matW.determinant()) {
                std::cout << " GblPoint::getDerivatives failed to invert matrix "
                                << std::endl;
                std::cout
                                << " Possible reason for singular matrix: multiple GblPoints at same arc-length"
                                << std::endl;
                throw std::overflow_error("Singular matrix inversion exception");
        }
        matW = matW.inverse().eval();
        matWJ = matW * matJ;
        vecWd = matW * vecd;
}
 
 
void GblPoint::printPoint(unsigned int level) const {
        std::cout << " GblPoint";
        if (theLabel) {
                std::cout << ", label " << theLabel;
                if (theOffset >= 0) {
                        std::cout << ", offset " << theOffset;
                }
        }
        if (measDim) {
                std::cout << ", " << measDim << " measurements";
        }
        if (scatFlag) {
                std::cout << ", scatterer";
        }
        if (transFlag) {
                std::cout << ", diagonalized";
        }
        if (localDerivatives.cols()) {
                std::cout << ", " << localDerivatives.cols() << " local derivatives";
        }
        if (globalDerivatives.cols()) {
                std::cout << ", " << globalDerivatives.cols() << " global derivatives";
        }
        std::cout << std::endl;
        if (level > 0) {
                IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
                if (measDim) {
                        std::cout << "  Measurement" << std::endl;
                        std::cout << "   Projection: " << std::endl
                                        << measProjection.format(CleanFmt) << std::endl;
                        std::cout << "   Residuals: "
                                        << measResiduals.transpose().format(CleanFmt) << std::endl;
                        std::cout << "   Precision (min.: " << measPrecMin << "): "
                                        << measPrecision.transpose().format(CleanFmt) << std::endl;
                }
                if (scatFlag) {
                        std::cout << "  Scatterer" << std::endl;
                        std::cout << "   Residuals: "
                                        << scatResiduals.transpose().format(CleanFmt) << std::endl;
                        std::cout << "   Precision: "
                                        << scatPrecision.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;
                }
                std::cout << "  Jacobian " << std::endl;
                std::cout << "   Point-to-point " << std::endl
                                << p2pJacobian.format(CleanFmt) << std::endl;
                if (theLabel) {
                        std::cout << "   To previous offset " << std::endl
                                        << prevJacobian.format(CleanFmt) << std::endl;
                        std::cout << "   To next offset " << std::endl
                                        << nextJacobian.format(CleanFmt) << std::endl;
                }
        }
}
 
}