GblPoint.h

Go to the documentation of this file.

/*
 * GblPoint.h
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */
 
#ifndef GBLPOINT_H_
#define GBLPOINT_H_
 
#include<iostream>
#include<vector>
#include<math.h>
#include <stdexcept>
#ifdef GBL_EIGEN_SUPPORT_ROOT
#include "TVectorD.h"
#include "TMatrixD.h"
#include "TMatrixDSym.h"
#include "TMatrixDSymEigen.h"
#endif
 
#include <Eigen/Dense>
 
namespace gbl {
 
typedef Eigen::Matrix<double, 5, 1> Vector5d;
typedef Eigen::Matrix<double, 2, 3> Matrix23d;
typedef Eigen::Matrix<double, 2, 5> Matrix25d;
typedef Eigen::Matrix<double, 2, 7> Matrix27d;
typedef Eigen::Matrix<double, 3, 2> Matrix32d;
typedef Eigen::Matrix<double, 5, 5> Matrix5d;
 
 
class GblPoint {
public:
        GblPoint(const Matrix5d &aJacobian);
        GblPoint(const GblPoint&) = default;
        GblPoint& operator=(const GblPoint&) = default;
        GblPoint(GblPoint&&) = default;
        GblPoint& operator=(GblPoint&&) = default;
        virtual ~GblPoint();
#ifdef GBL_EIGEN_SUPPORT_ROOT
        // input via ROOT
        GblPoint(const TMatrixD &aJacobian);
        void addMeasurement(const TMatrixD &aProjection, const TVectorD &aResiduals,
                        const TVectorD &aPrecision, double minPrecision = 0.);
        void addMeasurement(const TMatrixD &aProjection, const TVectorD &aResiduals,
                        const TMatrixDSym &aPrecision, double minPrecision = 0.);
        void addMeasurement(const TVectorD &aResiduals, const TVectorD &aPrecision,
                        double minPrecision = 0.);
        void addMeasurement(const TVectorD &aResiduals,
                        const TMatrixDSym &aPrecision, double minPrecision = 0.);
        void addScatterer(const TVectorD &aResiduals, const TVectorD &aPrecision);
        void addScatterer(const TVectorD &aResiduals,
                        const TMatrixDSym &aPrecision);
        void addLocals(const TMatrixD &aDerivatives);
        void addGlobals(const std::vector<int> &aLabels,
                        const TMatrixD &aDerivatives);
#endif
        // input via Eigen
 
 
        template<typename Projection, typename Residuals, typename Precision,
                        typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type* =
                                        nullptr>
        void addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
                        const Eigen::MatrixBase<Residuals>& aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision,
                        double minPrecision = 0.);
 
 
        template<typename Projection, typename Residuals, typename Precision,
                        typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type* =
                                        nullptr>
        void addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
                        const Eigen::MatrixBase<Residuals>& aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision,
                        double minPrecision = 0.);
 
 
        template<typename Residuals, typename Precision, typename std::enable_if<
                        (Precision::ColsAtCompileTime != 1)>::type* = nullptr>
        void addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision,
                        double minPrecision = 0.);
 
 
        template<typename Residuals, typename Precision, typename std::enable_if<
                        (Precision::ColsAtCompileTime == 1)>::type* = nullptr>
        void addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision,
                        double minPrecision = 0.);
 
 
        template<typename Precision, typename std::enable_if<
                        (Precision::ColsAtCompileTime == 2)>::type* = nullptr>
        void addScatterer(const Eigen::Vector2d &aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision);
 
 
        template<typename Precision, typename std::enable_if<
                        (Precision::ColsAtCompileTime == 1)>::type* = nullptr>
        void addScatterer(const Eigen::Vector2d &aResiduals,
                        const Eigen::MatrixBase<Precision>& aPrecision);
 
 
        template<typename Derivative>
        void addLocals(const Eigen::MatrixBase<Derivative>& aDerivatives);
        template<typename Derivative>
 
 
        void addGlobals(const std::vector<int> &aLabels,
                        const Eigen::MatrixBase<Derivative>& aDerivatives);
        //
        unsigned int hasMeasurement() const;
        double getMeasPrecMin() const;
        void getMeasurement(Matrix5d &aProjection, Vector5d &aResiduals,
                        Vector5d &aPrecision) const;
        void getMeasTransformation(Eigen::MatrixXd &aTransformation) const;
        bool hasScatterer() const;
        void getScatterer(Eigen::Matrix2d &aTransformation,
                        Eigen::Vector2d &aResiduals, Eigen::Vector2d &aPrecision) const;
        void getScatTransformation(Eigen::Matrix2d &aTransformation) const;
        unsigned int getNumLocals() const;
        const Eigen::MatrixXd& getLocalDerivatives() const;
        unsigned int getNumGlobals() const;
        void getGlobalLabels(std::vector<int> &aLabels) const;
        void getGlobalDerivatives(Eigen::MatrixXd &aDerivatives) const;
        void getGlobalLabelsAndDerivatives(unsigned int aRow,
                        std::vector<int> &aLabels, std::vector<double> &aDerivatives) const;
        unsigned int getLabel() const;
        int getOffset() const;
        const Matrix5d& getP2pJacobian() const;
        void getDerivatives(int aDirection, Eigen::Matrix2d &matW,
                        Eigen::Matrix2d &matWJ, Eigen::Vector2d &vecWd) const;
        void printPoint(unsigned int level = 0) const;
 
private:
        friend class GblTrajectory; // to have the following setters private
        void setLabel(unsigned int aLabel);
        void setOffset(int anOffset);
        void addPrevJacobian(const Matrix5d &aJac);
        void addNextJacobian(const Matrix5d &aJac);
 
        unsigned int theLabel; 
        int theOffset; 
        Matrix5d p2pJacobian; 
        Matrix5d prevJacobian; 
        Matrix5d nextJacobian; 
        unsigned int measDim; 
        double measPrecMin; 
        Matrix5d measProjection; 
 
        Vector5d measResiduals; 
        Vector5d measPrecision; 
        bool transFlag; 
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                        Eigen::ColMajor /* default */, 5, 5> measTransformation; 
        bool scatFlag; 
        Eigen::Matrix2d scatTransformation; 
        Eigen::Vector2d scatResiduals; 
        Eigen::Vector2d scatPrecision; 
        Eigen::MatrixXd localDerivatives; 
        std::vector<int> globalLabels; 
        Eigen::MatrixXd globalDerivatives; 
};
 
template<typename Projection, typename Residuals, typename Precision,
                typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type*>
void GblPoint::addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
                const Eigen::MatrixBase<Residuals>& aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
        static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Projection::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Projection) must be equal");
        static_assert(static_cast<int>(Precision::RowsAtCompileTime) == static_cast<int>(Precision::ColsAtCompileTime), "addMeasurement: rows(Precision) and cols(Precision) must be equal");
        static_assert(static_cast<int>(Projection::RowsAtCompileTime) == static_cast<int>(Projection::ColsAtCompileTime), "addMeasurement: rows(Projection) and cols(Projection) must be equal");
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        // arbitrary precision matrix
        Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> measEigen {
                        aPrecision };
        measTransformation = measEigen.eigenvectors().transpose();
        transFlag = true;
        measResiduals.tail(measDim) = measTransformation * aResiduals;
        measPrecision.tail(measDim) = measEigen.eigenvalues();
        measProjection.bottomRightCorner(measDim, measDim) = measTransformation
                        * aProjection;
}
 
template<typename Projection, typename Residuals, typename Precision,
                typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type*>
void GblPoint::addMeasurement(const Eigen::MatrixBase<Projection>& aProjection,
                const Eigen::MatrixBase<Residuals>& aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
        static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Projection::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Projection) must be equal");
        static_assert(static_cast<int>(Projection::RowsAtCompileTime) == static_cast<int>(Projection::ColsAtCompileTime), "addMeasurement: rows(Projection) and cols(Projection) must be equal");
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        // diagonal precision matrix
        measResiduals.tail(measDim) = aResiduals;
        measPrecision.tail(measDim) = aPrecision;
        measProjection.bottomRightCorner(measDim, measDim) = aProjection;
}
 
template<typename Residuals, typename Precision, typename std::enable_if<
                (Precision::ColsAtCompileTime != 1)>::type*>
void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
        static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        // arbitrary precision matrix
        Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> measEigen {
                        aPrecision };
        measTransformation = measEigen.eigenvectors().transpose();
        transFlag = true;
        measResiduals.tail(measDim) = measTransformation * aResiduals;
        measPrecision.tail(measDim) = measEigen.eigenvalues();
        measProjection.bottomRightCorner(measDim, measDim) = measTransformation;
}
 
template<typename Residuals, typename Precision, typename std::enable_if<
                (Precision::ColsAtCompileTime == 1)>::type*>
void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision, double minPrecision) {
        static_assert(static_cast<int>(Residuals::ColsAtCompileTime) == 1, "addMeasurement: cols(Residuals) must be 1 (vector)");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) <= 5 or static_cast<int>(Residuals::RowsAtCompileTime) == Eigen::Dynamic, "addMeasurement: rows(Residuals) must be 1-5 or dynamic");
        static_assert(static_cast<int>(Residuals::RowsAtCompileTime) == static_cast<int>(Precision::RowsAtCompileTime), "addMeasurement: rows(Residuals) and rows(Precision) must be equal");
        measDim = aResiduals.rows();
        measPrecMin = minPrecision;
        // diagonal precision matrix
        measResiduals.tail(measDim) = aResiduals;
        measPrecision.tail(measDim) = aPrecision;
        measProjection.setIdentity();
}
 
template<typename Precision, typename std::enable_if<
                (Precision::ColsAtCompileTime == 2)>::type*>
void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision) {
        static_assert(static_cast<int>(Precision::RowsAtCompileTime) == 2 or static_cast<int>(Precision::RowsAtCompileTime) == Eigen::Dynamic, "addScatterer: rows(Precision) must be 2 or dynamic");
        scatFlag = true;
        // arbitrary precision matrix
        Eigen::SelfAdjointEigenSolver<typename Precision::PlainObject> scatEigen {
                        aPrecision };
        scatTransformation = scatEigen.eigenvectors();
        scatTransformation.transposeInPlace();
        scatResiduals = scatTransformation * aResiduals;
        scatPrecision = scatEigen.eigenvalues();
}
 
template<typename Precision, typename std::enable_if<
                (Precision::ColsAtCompileTime == 1)>::type*>
void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
                const Eigen::MatrixBase<Precision>& aPrecision) {
        static_assert(static_cast<int>(Precision::RowsAtCompileTime) == 2 or static_cast<int>(Precision::RowsAtCompileTime) == Eigen::Dynamic, "addScatterer: rows(Precision) must be 2 or dynamic");
        scatFlag = true;
        scatResiduals = aResiduals;
        scatPrecision = aPrecision;
        scatTransformation.setIdentity();
}
 
template<typename Derivative>
void GblPoint::addLocals(const Eigen::MatrixBase<Derivative>& aDerivatives) {
        if (measDim) {
                localDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
                if (transFlag) {
                        localDerivatives = measTransformation * aDerivatives;
                } else {
                        localDerivatives = aDerivatives;
                }
        }
}
 
template<typename Derivative>
void GblPoint::addGlobals(const std::vector<int> &aLabels,
                const Eigen::MatrixBase<Derivative>& aDerivatives) {
        if (measDim) {
                globalLabels = aLabels;
                globalDerivatives.resize(aDerivatives.rows(), aDerivatives.cols());
                if (transFlag) {
                        globalDerivatives = measTransformation * aDerivatives;
                } else {
                        globalDerivatives = aDerivatives;
                }
 
        }
}
 
}
#endif /* GBLPOINT_H_ */