GblPoint.cc

Go to the documentation of this file.

/*
 * GblPoint.cpp
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */
 
#include "GblPoint.h"
 
namespace gbl {
 
 
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 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.);
  }
  measProjection = ROOT::Math::SMatrixIdentity();
}
 
 
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::getMeasTransformation(TMatrixD &aTransformation) const {
  aTransformation.ResizeTo(measDim, measDim);
  if (transFlag) {
    aTransformation = measTransformation;
  } else {
    aTransformation.UnitMatrix();
  }
}
 
 
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 = ROOT::Math::SMatrixIdentity();
}
 
 
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();
  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);
    }
  }
}
 
bool GblPoint::hasScatterer() const {
  return scatFlag;
}
 
 
void GblPoint::getScatterer(SMatrix22 &aTransformation, SVector2 &aResiduals,
    SVector2 &aPrecision) const {
  aTransformation = scatTransformation;
  aResiduals = scatResiduals;
  aPrecision = scatPrecision;
}
 
 
void GblPoint::getScatTransformation(TMatrixD &aTransformation) const {
  aTransformation.ResizeTo(2, 2);
  if (scatFlag) {
    for (unsigned int i = 0; i < 2; ++i) {
      for (unsigned int j = 0; j < 2; ++j) {
        aTransformation(i, j) = scatTransformation(i, j);
      }
    }
  } else {
    aTransformation.UnitMatrix();
  }
}
 
 
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);
  }
 
  if (!matW.InvertFast()) {
    std::cout << " GblPoint::getDerivatives failed to invert matrix: "
        << matW << 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");
  }
  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.GetNcols()) {
    std::cout << ", " << localDerivatives.GetNcols()
        << " local derivatives";
  }
  if (globalDerivatives.GetNcols()) {
    std::cout << ", " << globalDerivatives.GetNcols()
        << " global derivatives";
  }
  std::cout << std::endl;
  if (level > 0) {
    if (measDim) {
      std::cout << "  Measurement" << std::endl;
      std::cout << "   Projection: " << std::endl << measProjection
          << std::endl;
      std::cout << "   Residuals: " << measResiduals << std::endl;
      std::cout << "   Precision: " << measPrecision << std::endl;
    }
    if (scatFlag) {
      std::cout << "  Scatterer" << std::endl;
      std::cout << "   Residuals: " << scatResiduals << std::endl;
      std::cout << "   Precision: " << scatPrecision << std::endl;
    }
    if (localDerivatives.GetNcols()) {
      std::cout << "  Local Derivatives:" << std::endl;
      localDerivatives.Print();
    }
    if (globalDerivatives.GetNcols()) {
      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:" << std::endl;
      globalDerivatives.Print();
    }
    std::cout << "  Jacobian " << std::endl;
    std::cout << "   Point-to-point " << std::endl << p2pJacobian
        << std::endl;
    if (theLabel) {
      std::cout << "   To previous offset " << std::endl << prevJacobian
          << std::endl;
      std::cout << "   To next offset " << std::endl << nextJacobian
          << std::endl;
    }
  }
}
 
}