GblPoint.cpp

Go to the documentation of this file.

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

#include "GblPoint.h"


GblPoint::GblPoint(const TMatrixD &aJacobian) :
                theLabel(0), theOffset(0), measDim(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);
                }
        }
}

GblPoint::GblPoint(const SMatrix55 &aJacobian) :
                theLabel(0), theOffset(0), p2pJacobian(aJacobian), measDim(0), transFlag(
                                false), measTransformation(), scatFlag(false), localDerivatives(), globalLabels(), globalDerivatives() {

}

GblPoint::~GblPoint() {
}


void GblPoint::addMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TVectorD &aPrecision,
                double minPrecision) {
        measDim = aResiduals.GetNrows();
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = aResiduals[i];
                measPrecision(iOff + i) = (
                                aPrecision[i] >= minPrecision ? aPrecision[i] : 0.);
                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();
        TMatrixDSymEigen measEigen(aPrecision);
        measTransformation.ResizeTo(measDim, measDim);
        measTransformation = measEigen.GetEigenVectors();
        measTransformation.T();
        transFlag = true;
        TVectorD transResiduals = measTransformation * aResiduals;
        TVectorD transPrecision = measEigen.GetEigenValues();
        TMatrixD transProjection = measTransformation * aProjection;
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = transResiduals[i];
                measPrecision(iOff + i) = (
                                transPrecision[i] >= minPrecision ? transPrecision[i] : 0.);
                for (unsigned int j = 0; j < measDim; ++j) {
                        measProjection(iOff + i, iOff + j) = transProjection(i, j);
                }
        }
}


void GblPoint::addMeasurement(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision, double minPrecision) {
        measDim = aResiduals.GetNrows();
        TMatrixDSymEigen measEigen(aPrecision);
        measTransformation.ResizeTo(measDim, measDim);
        measTransformation = measEigen.GetEigenVectors();
        measTransformation.T();
        transFlag = true;
        TVectorD transResiduals = measTransformation * aResiduals;
        TVectorD transPrecision = measEigen.GetEigenValues();
        unsigned int iOff = 5 - measDim;
        for (unsigned int i = 0; i < measDim; ++i) {
                measResiduals(iOff + i) = transResiduals[i];
                measPrecision(iOff + i) = (
                                transPrecision[i] >= minPrecision ? transPrecision[i] : 0.);
                for (unsigned int j = 0; j < measDim; ++j) {
                        measProjection(iOff + i, iOff + j) = measTransformation(i, j);
                }
        }
}


unsigned int GblPoint::hasMeasurement() const {
        return measDim;
}


void GblPoint::getMeasurement(SMatrix55 &aProjection, SVector5 &aResiduals,
                SVector5 &aPrecision) const {
        aProjection = measProjection;
        aResiduals = measResiduals;
        aPrecision = measPrecision;
}


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];
}

bool GblPoint::hasScatterer() const {
        return scatFlag;
}


void GblPoint::getScatterer(SVector2 &aResiduals, SVector2 &aPrecision) const {
        aResiduals = scatResiduals;
        aPrecision = scatPrecision;
}


void GblPoint::addLocals(const TMatrixD &aDerivatives) {
        if (measDim) {
                localDerivatives.ResizeTo(aDerivatives);
                if (transFlag) {
                        localDerivatives = measTransformation * aDerivatives;
                } else {
                        localDerivatives = aDerivatives;
                }
        }
}

unsigned int GblPoint::getNumLocals() const {
        return localDerivatives.GetNcols();
}

const TMatrixD& GblPoint::getLocalDerivatives() const {
        return localDerivatives;
}


void GblPoint::addGlobals(const std::vector<int> &aLabels,
                const TMatrixD &aDerivatives) {
        if (measDim) {
                globalLabels = aLabels;
                globalDerivatives.ResizeTo(aDerivatives);
                if (transFlag) {
                        globalDerivatives = measTransformation * aDerivatives;
                } else {
                        globalDerivatives = aDerivatives;
                }

        }
}

unsigned int GblPoint::getNumGlobals() const {
        return globalDerivatives.GetNcols();
}

std::vector<int> GblPoint::getGlobalLabels() const {
        return globalLabels;
}

const TMatrixD& GblPoint::getGlobalDerivatives() const {
        return globalDerivatives;
}


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 SMatrix55& GblPoint::getP2pJacobian() const {
        return p2pJacobian;
}


void GblPoint::addPrevJacobian(const SMatrix55 aJac) {
        int ifail = 0;
// to optimize: need only two last rows of inverse
//      prevJacobian = aJac.InverseFast(ifail);
//  block matrix algebra
        SMatrix23 CA = aJac.Sub<SMatrix23>(3, 0)
                        * aJac.Sub<SMatrix33>(0, 0).InverseFast(ifail); // C*A^-1
        SMatrix22 DCAB = aJac.Sub<SMatrix22>(3, 3) - CA * aJac.Sub<SMatrix32>(0, 3); // D - C*A^-1 *B
        DCAB.InvertFast();
        prevJacobian.Place_at(DCAB, 3, 3);
        prevJacobian.Place_at(-DCAB * CA, 3, 0);
}


void GblPoint::addNextJacobian(const SMatrix55 aJac) {
        nextJacobian = aJac;
}


void GblPoint::getDerivatives(int aDirection, SMatrix22 &matW, SMatrix22 &matWJ,
                SVector2 &vecWd) const {

        SMatrix22 matJ;
        SVector2 vecd;
        if (aDirection < 1) {
                matJ = prevJacobian.Sub<SMatrix22>(3, 3);
                matW = -prevJacobian.Sub<SMatrix22>(3, 1);
                vecd = prevJacobian.SubCol<SVector2>(0, 3);
        } else {
                matJ = nextJacobian.Sub<SMatrix22>(3, 3);
                matW = nextJacobian.Sub<SMatrix22>(3, 1);
                vecd = nextJacobian.SubCol<SVector2>(0, 3);
        }

        matW.InvertFast();
        matWJ = matW * matJ;
        vecWd = matW * vecd;

}