/afs/cern.ch/work/a/aaltunda/public/www/CMSSW_5_3_13_patch3/src/RecoVertex/GhostTrackFitter/src/GhostTrackPrediction.cc

Go to the documentation of this file.

#include <cmath>

#include <Math/SMatrix.h>
#include <Math/MatrixFunctions.h>

#include "DataFormats/GeometryVector/interface/GlobalPoint.h"
#include "DataFormats/GeometryVector/interface/GlobalVector.h" 
#include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"
#include "DataFormats/TrackReco/interface/Track.h"

#include "TrackingTools/TrajectoryParametrization/interface/CurvilinearTrajectoryParameters.h"
#include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
#include "TrackingTools/TrajectoryState/interface/FreeTrajectoryState.h"

#include "RecoVertex/GhostTrackFitter/interface/GhostTrackPrediction.h"

using namespace reco;

namespace {
  inline double sqr(double arg) { return arg * arg; }
  
  typedef ROOT::Math::SMatrix<double, 3, 4> Matrix34;
  typedef ROOT::Math::SMatrix<double, 4, 5> Matrix45;
  typedef ROOT::Math::SMatrix<double, 5, 4> Matrix54;
  typedef ROOT::Math::SMatrix<double, 4, 6> Matrix46;
  typedef ROOT::Math::SMatrix<double, 6, 4> Matrix64;
  typedef ROOT::Math::SMatrix<double, 6, 6, ROOT::Math::MatRepSym<double, 6> > Matrix6S;

  inline 
  GhostTrackPrediction::Vector convert(const CurvilinearTrajectoryParameters &trajectory) {
    return GhostTrackPrediction::Vector(
                                        trajectory.yT() / std::cos(trajectory.lambda()),
                                        trajectory.xT(),
                                        std::tan(trajectory.lambda()),
                                        trajectory.phi());
  }
  
  
  inline 
  GhostTrackPrediction::Vector convert(const GlobalTrajectoryParameters &trajectory) {
    return convert(CurvilinearTrajectoryParameters(trajectory.position(), trajectory.momentum(), 0));
  }
  
  
  inline
  GhostTrackPrediction::Error convert(
                                      const GhostTrackPrediction::Vector &pred,
                                      const CurvilinearTrajectoryError &error) {
    
    double rho2 = pred[2] * pred[2] + 1.;
    double rho = std::sqrt(rho2);
    
    Matrix45 jacobian;
    jacobian(0, 1) = pred[0] * pred[2];
    jacobian(0, 4) = rho;
    jacobian(1, 3) = 1.;
    jacobian(2, 1) = rho2;
    jacobian(3, 2) = 1.;
    
    return ROOT::Math::Similarity(jacobian, error.matrix());
  }

}

void GhostTrackPrediction::init(
                        const GlobalPoint &priorPosition,
                        const GlobalError &priorError,
                        const GlobalVector &direction,
                        const GlobalError &directionError)
{

        double perp2 = direction.perp2();
        GlobalVector dir = direction / std::sqrt(perp2);
        double tip = priorPosition.y() * dir.x() - priorPosition.x() * dir.y();
        double l = priorPosition.x() * dir.x() + priorPosition.y() * dir.y();

        prediction_[0] = priorPosition.z() - dir.z() * l;
        prediction_[1] = tip;
        prediction_[2] = dir.z();
        prediction_[3] = dir.phi();

        Matrix46 jacobian;
        jacobian(0, 0) = -dir.x() * dir.z();
        jacobian(1, 0) = -dir.y();
        jacobian(0, 1) = -dir.y() * dir.z();
        jacobian(1, 1) = dir.x();
        jacobian(0, 2) = 1.;
        jacobian(0, 3) = -dir.z() * priorPosition.x();
        jacobian(1, 3) = priorPosition.y();
        jacobian(3, 3) = -dir.y();
        jacobian(0, 4) = -dir.z() * priorPosition.y();
        jacobian(1, 4) = -priorPosition.x();
        jacobian(3, 4) = dir.x();
        jacobian(0, 5) = -l;
        jacobian(2, 5) = 1.;

        Matrix6S origCov;
        origCov.Place_at(priorError.matrix_new(), 0, 0);
        origCov.Place_at(directionError.matrix_new() / perp2, 3, 3);

        covariance_ =  ROOT::Math::Similarity(jacobian, origCov);
}

GhostTrackPrediction::GhostTrackPrediction(
                        const GlobalPoint &priorPosition,
                        const GlobalError &priorError,
                        const GlobalVector &direction,
                        double coneRadius)
{
        double dTheta = std::cosh((double)direction.eta()) * coneRadius;

        double r2 = direction.mag2();
        double r = std::sqrt(r2);
        double perp = direction.perp();
        double P = direction.x() / perp;
        double p = direction.y() / perp;
        double T = direction.z() / r;
        double t = perp / r;
        double h2 = dTheta * dTheta;
        double d2 = coneRadius * coneRadius;

        GlobalError cov(r2 * (T*T * P*P * h2 + t*t * p*p * d2),
                        r2 * p*P * (T*T * h2 - t*t * d2),
                        r2 * (T*T * p*p * h2 + t*t * P*P * d2),
                        -r2 * t*T * P * h2,
                        -r2 * t*T * p * h2,
                        r2 * t*t * h2);

        init(priorPosition, priorError, direction, cov);
}


GhostTrackPrediction::GhostTrackPrediction(
                        const GlobalTrajectoryParameters &trajectory,
                        const CurvilinearTrajectoryError &error) :
        prediction_(convert(trajectory)),
        covariance_(convert(prediction_, error))
{
}

GhostTrackPrediction::GhostTrackPrediction(const Track &track) :
        prediction_(convert(
                            GlobalTrajectoryParameters(
                                                       GlobalPoint(track.vx(), track.vy(), track.vz()),
                                                       GlobalVector(track.px(), track.py(), track.pz()),
                                                       0, 0))),
        covariance_(convert(prediction_, track.covariance()))
{
}

GlobalError GhostTrackPrediction::positionError(double lambda) const
{
        double x = std::cos(phi());
        double y = std::sin(phi());

        Matrix34 jacobian;
        jacobian(0, 1) = -y;
        jacobian(0, 3) = -y * lambda - x * ip();
        jacobian(1, 1) = x;
        jacobian(1, 3) = x * lambda - y * ip();
        jacobian(2, 0) = 1.;
        jacobian(2, 2) = lambda;

        return ROOT::Math::Similarity(jacobian, covariance());
}

Matrix6S GhostTrackPrediction::cartesianError(double lambda) const
{
        double x = std::cos(phi());
        double y = std::sin(phi());

        Matrix64 jacobian;
        jacobian(0, 1) = -y;
        jacobian(0, 3) = -y * lambda - x * ip();
        jacobian(1, 1) = x;
        jacobian(1, 3) = x * lambda - y * ip();
        jacobian(2, 0) = 1.;
        jacobian(2, 2) = lambda;
        jacobian(3, 3) = -y;
        jacobian(4, 3) = x;
        jacobian(5, 2) = 1.;

        return ROOT::Math::Similarity(jacobian, covariance());
}


GlobalTrajectoryParameters GhostTrackPrediction::globalTrajectory(const MagneticField *fieldProvider) const {
  return GlobalTrajectoryParameters(origin(), direction(),
                                    0, fieldProvider);
}

CurvilinearTrajectoryError GhostTrackPrediction::curvilinearError() const
{
  double rho2I = 1. / rho2();
  double rhoI = std::sqrt(rho2I);
  
  Matrix54 jacobian;
  jacobian(1, 2) = rho2I;
  jacobian(2, 3) = 1.;
  jacobian(3, 1) = 1.;
  jacobian(4, 0) = rhoI;
  jacobian(4, 2) = - z() * rhoI * cotTheta() * rho2I;
  
  AlgebraicSymMatrix55 err = Similarity(jacobian, covariance());
  err(0, 0) = 1.;
  
  return CurvilinearTrajectoryError(err);
}

FreeTrajectoryState GhostTrackPrediction::fts(const MagneticField *fieldProvider) const
{
  return FreeTrajectoryState(globalTrajectory(fieldProvider),
                             curvilinearError());
}

Track GhostTrackPrediction::track(double ndof, double chi2) const
{
  GlobalPoint origin = this->origin();
  GlobalVector dir = direction().unit();
  
  Track::Point point(origin.x(), origin.y(), origin.z());
  Track::Vector vector(dir.x(), dir.y(), dir.z());
  
  return Track(chi2, ndof, point, vector, 0, curvilinearError());
}