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, unsigned int numMeasReserve) :
                theLabel(0), theOffset(0), theType(0), p2pJacobian(aJacobian), scatDim(
                                0) {
        theMeasurements.reserve(numMeasReserve);
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
GblPoint::GblPoint(const TMatrixD &aJacobian) :
                theLabel(0), theOffset(0), theType(0), scatDim(0) {
 
        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) {
        theMeasurements.emplace_back(aProjection, aResiduals, aPrecision,
                        minPrecision);
}
 
 
void GblPoint::addMeasurement(const TMatrixD &aProjection,
                const TVectorD &aResiduals, const TMatrixDSym &aPrecision,
                double minPrecision) {
        theMeasurements.emplace_back(aProjection, aResiduals, aPrecision,
                        minPrecision);
}
 
 
void GblPoint::addMeasurement(const TVectorD &aResiduals,
                const TVectorD &aPrecision, double minPrecision) {
        theMeasurements.emplace_back(aResiduals, aPrecision, minPrecision);
}
 
 
void GblPoint::addMeasurement(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision, double minPrecision) {
        theMeasurements.emplace_back(aResiduals, aPrecision, minPrecision);
}
#endif
 
 
unsigned int GblPoint::numMeasurements() const {
        return theMeasurements.size();
}
 
 
void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
                const Eigen::Matrix2d &aPrecision) {
        scatDim = 2;
        // arbitrary precision matrix
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> scatEigen { aPrecision };
        scatTransformation.topLeftCorner<2, 2>() = scatEigen.eigenvectors();
        scatTransformation.topLeftCorner<2, 2>().transposeInPlace();
        scatResiduals.head<2>() = scatTransformation.topLeftCorner<2, 2>()
                        * aResiduals;
        scatPrecision.head<2>() = scatEigen.eigenvalues();
}
 
 
void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
                const Eigen::Vector2d &aPrecision) {
        scatDim = 2;
        scatResiduals.head<2>() = aResiduals;
        scatPrecision.head<2>() = aPrecision;
        scatTransformation.topLeftCorner<2, 2>().setIdentity();
}
 
 
void GblPoint::addThickScatterer(const Eigen::Vector4d &aResiduals,
                const Eigen::Matrix4d &aPrecision) {
        scatDim = 4;
        // dense precision matrix
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> scatEigen { aPrecision };
        scatTransformation = scatEigen.eigenvectors();
        scatTransformation.transposeInPlace();
        scatResiduals = scatTransformation * aResiduals;
        scatPrecision = scatEigen.eigenvalues();
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::addScatterer(const TVectorD &aResiduals,
                const TVectorD &aPrecision) {
        scatDim = 2;
        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) {
        scatDim = 2;
        TMatrixDSymEigen scatEigen(aPrecision);
        TMatrixD aTransformation = scatEigen.GetEigenVectors();
        aTransformation.T();
        TVectorD transResiduals = aTransformation * aResiduals;
        TVectorD transPrecision = scatEigen.GetEigenValues();
        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);
                }
        }
}
 
 
void GblPoint::addThickScatterer(const TVectorD &aResiduals,
                const TMatrixDSym &aPrecision) {
        scatDim = 4;
        TMatrixDSymEigen scatEigen(aPrecision);
        TMatrixD aTransformation = scatEigen.GetEigenVectors();
        aTransformation.T();
        TVectorD transResiduals = aTransformation * aResiduals;
        TVectorD transPrecision = scatEigen.GetEigenValues();
        for (unsigned int i = 0; i < 4; ++i) {
                scatResiduals(i) = transResiduals[i];
                scatPrecision(i) = transPrecision[i];
                for (unsigned int j = 0; j < 4; ++j) {
                        scatTransformation(i, j) = aTransformation(i, j);
                }
        }
}
#endif
 
 
unsigned int GblPoint::getScatDim() const {
        return scatDim;
}
 
 
void GblPoint::getScatterer(Matrix4d &aTransformation, Vector4d &aResiduals,
                Vector4d &aPrecision) const {
        aTransformation.topLeftCorner(scatDim, scatDim) =
                        scatTransformation.topLeftCorner(scatDim, scatDim);
        aResiduals.head(scatDim) = scatResiduals.head(scatDim);
        aPrecision.head(scatDim) = scatPrecision.head(scatDim);
}
 
 
void GblPoint::getReducedScatterer(Matrix4d &aTransformation,
                Vector4d &aResiduals, Vector4d &aPrecision) const {
        // get scatterer input back from diagonalization
        Matrix4d scatPrec = scatTransformation.transpose()
                        * scatPrecision.asDiagonal() * scatTransformation;
        Vector4d scatRes = scatTransformation.transpose() * scatResiduals;
        // block matrix algebra to get positional precision
        Matrix2d posPrec = scatPrec.bottomRightCorner<2, 2>()
                        - scatPrec.bottomLeftCorner<2, 2>()
                                        * scatPrec.topLeftCorner<2, 2>().inverse()
                                        * scatPrec.topRightCorner<2, 2>();
        // scatPrec with only positional precision
        scatPrec.setZero();
        scatPrec.bottomRightCorner<2, 2>() = posPrec;
        // diagonalize again
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> scatEigen { scatPrec };
        //
        aTransformation = scatEigen.eigenvectors().transpose();
        aResiduals = aTransformation * scatRes;
        aPrecision = scatEigen.eigenvalues();
}
 
 
void GblPoint::getScatTransformation(MatrixXd &aTransformation) const {
        aTransformation.resize(scatDim, scatDim);
        if (scatDim > 0) {
                aTransformation = scatTransformation.topLeftCorner(scatDim, scatDim);
        } else {
                aTransformation.setIdentity();
        }
}
 
#ifdef GBL_EIGEN_SUPPORT_ROOT
 
void GblPoint::addLocals(const TMatrixD &aDerivatives) {
        if (theMeasurements.size())
                theMeasurements.back().addLocals(aDerivatives);
}
 
 
void GblPoint::addGlobals(const std::vector<int> &aLabels,
                const TMatrixD &aDerivatives) {
        if (theMeasurements.size())
                theMeasurements.back().addGlobals(aLabels, aDerivatives);
}
#endif
 
 
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;
}
 
 
void GblPoint::setType(int aType) {
        theType = aType;
}
 
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 (theMeasurements.size()) {
                std::cout << ", " << theMeasurements.size() << " measurements";
        }
        std::vector<GblMeasurement>::const_iterator itMeas;
        for (itMeas = theMeasurements.begin(); itMeas < theMeasurements.end();
                        ++itMeas) {
                itMeas->printMeasurement(0);
        }
        if (scatDim == 2) {
                std::cout << ", thin scatterer";
        }
        if (scatDim == 4) {
                std::cout << ", thick scatterer";
        }
 
        std::cout << std::endl;
        if (level > 0) {
                for (itMeas = theMeasurements.begin(); itMeas < theMeasurements.end();
                                ++itMeas) {
                        itMeas->printMeasurement(level);
                }
                IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
                if (scatDim) {
                        std::cout << "  Scatterer" << std::endl;
                        std::cout << "   Residuals: "
                                        << scatResiduals.head(scatDim).transpose().format(CleanFmt)
                                        << std::endl;
                        std::cout << "   Precision: "
                                        << scatPrecision.head(scatDim).transpose().format(CleanFmt)
                                        << std::endl;
                }
                std::cout << "  Jacobian " << std::endl;
                std::cout << "   Point-to-point " << std::endl
                                << p2pJacobian.format(CleanFmt) << std::endl;
                if (theLabel) {
                        if (!isFirst())
                                std::cout << "   To previous offset (last 2 rows)" << std::endl
                                                << prevJacobian.bottomRows<2>().format(CleanFmt)
                                                << std::endl;
                        if (!isLast())
                                std::cout << "   To next offset " << std::endl
                                                << nextJacobian.format(CleanFmt) << std::endl;
                }
        }
}
 
std::vector<GblMeasurement>::iterator GblPoint::getMeasBegin() {
        return theMeasurements.begin();
}
 
std::vector<GblMeasurement>::iterator GblPoint::getMeasEnd() {
        return theMeasurements.end();
}
 
 
void GblPoint::getGlobalLabelsAndDerivatives(unsigned int aMeas,
                unsigned int aRow, std::vector<int> &aLabels,
                std::vector<double> &aDerivatives) const {
        theMeasurements[aMeas].getGlobalLabelsAndDerivatives(aRow, aLabels,
                        aDerivatives);
}
 
bool GblPoint::isFirst() const {
        return (theType < 0);
}
 
bool GblPoint::isLast() const {
        return (theType > 0);
}
 
}