GblPoint.cc

Go to the documentation of this file.

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

#include "Alignment/ReferenceTrajectories/interface/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


  void GblPoint::addScatterer(const Vector2d &aResiduals,
                              const Vector2d& aPrecision) {
    scatFlag = true;
    scatResiduals = aResiduals;
    scatPrecision = aPrecision;
    scatTransformation.setIdentity();
  }

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

}