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<(Residuals::ColsAtCompileTime == 1)>::type* = nullptr,
              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<(Residuals::ColsAtCompileTime == 1)>::type* = nullptr,
              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<(Residuals::ColsAtCompileTime == 1)>::type* = nullptr,
              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<(Residuals::ColsAtCompileTime == 1)>::type* = nullptr,
              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>
    void addScatterer(const Eigen::Vector2d &aResiduals,
                      const Eigen::MatrixBase<Precision>& aPrecision);
    void addScatterer(const Eigen::Vector2d &aResiduals,
                      const Eigen::Vector2d& aPrecision);
    //
    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;
    template <typename Derivative>
    void addLocals(const Eigen::MatrixBase<Derivative>& aDerivatives);
    unsigned int getNumLocals() const;
    const Eigen::MatrixXd& getLocalDerivatives() const;
    template <typename Derivative>
    void addGlobals(const std::vector<int> &aLabels,
                    const Eigen::MatrixBase<Derivative>& aDerivatives);
    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;
    void setLabel(unsigned int aLabel);
    unsigned int getLabel() const;
    void setOffset(int anOffset);
    int getOffset() const;
    const Matrix5d& getP2pJacobian() const;
    void addPrevJacobian(const Matrix5d &aJac);
    void addNextJacobian(const Matrix5d &aJac);
    void getDerivatives(int aDirection, Eigen::Matrix2d &matW, Eigen::Matrix2d &matWJ,
                        Eigen::Vector2d &vecWd) const;
    void printPoint(unsigned int level = 0) const;

  private:
    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<(Residuals::ColsAtCompileTime == 1)>::type*,
            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)
  {
    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<(Residuals::ColsAtCompileTime == 1)>::type*,
            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)
  {
    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<(Residuals::ColsAtCompileTime == 1)>::type*,
            typename std::enable_if<(Precision::ColsAtCompileTime != 1)>::type*>
  void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                                const Eigen::MatrixBase<Precision>& aPrecision,
                                double minPrecision)
  {
    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<(Residuals::ColsAtCompileTime == 1)>::type*,
            typename std::enable_if<(Precision::ColsAtCompileTime == 1)>::type*>
  void GblPoint::addMeasurement(const Eigen::MatrixBase<Residuals>& aResiduals,
                                const Eigen::MatrixBase<Precision>& aPrecision,
                                double minPrecision)
  {
    measDim = aResiduals.rows();
    measPrecMin = minPrecision;
    // diagonal precision matrix
    measResiduals.tail(measDim) = aResiduals;
    measPrecision.tail(measDim) = aPrecision;
    measProjection.setIdentity();
  }


  template <typename Precision>
  void GblPoint::addScatterer(const Eigen::Vector2d &aResiduals,
                              const Eigen::MatrixBase<Precision>& aPrecision)
  {
    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 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_ */