KfVertexFitter.cxx

Go to the documentation of this file.

/* Copyright (C) 2007-2023 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Sergey Gorbunov [committer] */
 

 
#include "KfVertexFitter.h"
 
#include "AlgoFairloggerCompat.h"
#include "KfFieldRegion.h"
#include "KfSetup.h"
#include "KfTrackKalmanFilter.h"
 
using namespace cbm::algo::kf;
 
template<DoFitTime FlagFitTime>
VertexD VertexFitter<FlagFitTime>::GetGuessFromTheTarget() const
{
  if (!fKfSetup) {
    LOG(fatal) << "VertexFitter: KfSetup is not set!";
  }
  const auto& target = fKfSetup->GetTarget();
  VertexD v;
  v.SetX(target.GetX());
  v.SetY(target.GetY());
  v.SetZ(target.GetZ());
  v.SetTime(0.);
  v.ResetErrors(target.GetR() * target.GetR() / 3.5 / 3.5, target.GetR() * target.GetR() / 3.5 / 3.5,
                target.GetDz() * target.GetDz() / 3.5 / 3.5, 1.e5);
  return v;
}
 
template<DoFitTime FlagFitTime>
template<typename T>
std::tuple<VertexD, std::vector<char>>
VertexFitter<FlagFitTime>::FitVertex(const std::vector<const TrackParam<T>*>& tracks,
                                     const std::vector<const TrackParam<T>*>& linearizations,
                                     const VertexD& vtxGuess) const
{
  if (!fKfSetup) {
    LOG(fatal) << "VertexFitter: KfSetup is not set!";
  }
 
  if (tracks.size() != linearizations.size()) {
    LOG(fatal) << "VertexFitter: number of tracks and number of linearizations do not match!";
  }
 
  for (unsigned int itr = 0; itr < tracks.size(); ++itr) {
    const auto trk = tracks[itr];
    const auto lin = linearizations[itr];
    if (!trk || !lin) {
      LOG(fatal) << "VertexFitter: null track or linearization pointer!";
    }
    if (!(fabs(trk->GetZ() - lin->GetZ()) < 1.e-10)) {
      LOG(fatal) << "VertexFitter: track and linearization z positions do not match!";
    }
  }
 
  std::vector<char> usedTracks(tracks.size(), 0);
 
  const auto& target = fKfSetup->GetTarget();
 
  TrackKalmanFilter<T, FilterSettings<LinearizationFull, FlagFitTime>> trackFitter;
  FieldRegion<T> field(GlobalField::fgOriginalFieldType, GlobalField::fgOriginalField);
 
  trackFitter.SetMaxExtrapolationStep(1.0);  // [cm]
 
  // Vertex state vector and the covariance matrix
 
  VertexD vtx = vtxGuess;
  auto& r     = vtx.Parameters();
  auto& C     = vtx.CovMatrix();
 
  // Iteratively fit the vertex
  for (int iteration = 0; iteration < fNofIterations; ++iteration) {
 
    // Store the vertex from the previous iteration
 
    auto vtx0      = vtx;
    const auto& r0 = vtx0.Parameters();
    const auto& C0 = vtx0.CovMatrix();
 
    // Initialize the vertex covariance matrix, Chi^2 & NDF
 
    vtx.ResetErrors(1.e1, 1.e1, 1.e1, 1.e1);
    std::fill(usedTracks.begin(), usedTracks.end(), 0);
 
    if (fConstrainToTargetZ) {
      vtx.SetZ(target.GetZ());
      C(Tag22) = target.GetDz() * target.GetDz() / 3.5 / 3.5;
    }
    if (fConstrainToBeamXY) {
      vtx.SetX(fBeamX);
      vtx.SetY(fBeamY);
      C(Tag00) = fBeamSigmaX * fBeamSigmaX;
      C(Tag11) = fBeamSigmaY * fBeamSigmaY;
    }
 
    // Loop over tracks to update the vertex with each track information
 
    //LOG(info) << "\n\n===========================";
    //LOG(info) << "VertexFitter: iteration " << iteration;
 
    for (unsigned int itr = 0; itr < tracks.size(); ++itr) {
      //LOG(info) << "VertexFitter: processing track " << itr;
      //LOG(info) << vtx.ToString();
      // Transport track to the vertex z position
      trackFitter.SetTrack(*tracks[itr]);
      trackFitter.SetLinearization(*linearizations[itr]);
      trackFitter.Extrapolate(vtx0.GetZ(), field);
 
      const auto& tr  = trackFitter.Tr();
      const auto& lin = trackFitter.Linearization();
 
      //LOG(info) << tr.ToString();
 
      std::array<T, 6> m{tr.X(), tr.Y(), tr.Tx(), tr.Ty(), tr.Qp(), tr.Time()};
      const auto& V = tr.GetCovMatrix();
 
      // Check the track deviation from the vtx0 estimate. The covariance matrix of the vtxGuess must be set when using this option.
      if (fRmsCut > 0.) {
        double zeta[2] = {r0[0] - m[0], r0[1] - m[1]};
        double S[3]    = {(C0(Tag11) + V[2]), -(C0(Tag10) + V[1]), (C0(Tag00) + V[0])};
        double s       = S[2] * S[0] - S[1] * S[1];
        double chi2    = zeta[0] * zeta[0] * S[0] + 2 * zeta[0] * zeta[1] * S[1] + zeta[1] * zeta[1] * S[2];
        if (chi2 > s * fRmsCut * fRmsCut) {
          continue;
        }
      }
 
      // Fit the track linearization to the r0 vertex. The covmatrix is taken from the track.
 
      double a = 0, b = 0;  // slopes at the vertex
      {
        std::array<T, 6> ml{lin.x, lin.y, lin.tx, lin.ty, lin.qp, lin.time};
        double zeta[2] = {r0[0] - ml[0], r0[1] - ml[1]};
 
        double s = V[0] * V[2] - V[1] * V[1];
        if (s < 1.E-20) {
          LOG(warning) << "VertexFitter: track covariance matrix is singular, skipping track!";
          continue;
        }
        a = m[2] + ((V[3] * V[2] - V[4] * V[1]) * zeta[0] + (-V[3] * V[1] + V[4] * V[0]) * zeta[1]) / s;
        b = m[3] + ((V[6] * V[2] - V[7] * V[1]) * zeta[0] + (-V[6] * V[1] + V[7] * V[0]) * zeta[1]) / s;
      }
 
      //LOG(info) << "VertexFitter: track slopes at vertex a=" << a << " b=" << b;
 
      // Update the vertex (r,C) with the track estimate (m,V) :
 
      // Matrix of the linearized measurement
      // H = { 1, 0, -a }
      //     { 0, 1, -b }
 
      //      ( 1   0 )
      // Ht = ( 0   1 )
      //      (-a  -b )
 
      // Residual (measured - estimated) zeta = m - H*r
 
      double zeta[2] = {m[0] + a * (r[2] - r0[2]) - r[0],  // propagate track from vtx0 to vtx using slopes a, b at vtx0
                        m[1] + b * (r[2] - r0[2]) - r[1]};
 
      // CHt = CH'
 
      double CHt[3][2] = {{C(Tag00) - a * C(Tag20), C(Tag10) - b * C(Tag20)},
                          {C(Tag10) - a * C(Tag21), C(Tag11) - b * C(Tag21)},
                          {C(Tag20) - a * C(Tag22), C(Tag21) - b * C(Tag22)}};
 
      // HCHt = H*C*H', low-triangular storage
 
      double HCHt[3] = {CHt[0][0] - a * CHt[2][0],  //
                        CHt[1][0] - b * CHt[2][0], CHt[1][1] - b * CHt[2][1]};
 
      // S = (H*C*H' + V ), low-triangular storage
 
      double S[3] = {V[0] + HCHt[0],  //
                     V[1] + HCHt[1], V[2] + HCHt[2]};
 
      // Invert S: S -> S^{-1}
      {
        double det = S[0] * S[2] - S[1] * S[1];
        if (det < 1.E-20) {
          LOG(warn) << "VertexFitter: S matrix is singular, skipping track!";
          LOG(warn) << " S00=" << S[0] << " S11=" << S[2] << " S01=" << S[1];
          LOG(warn) << " V00=" << V[0] << " V11=" << V[2] << " V01=" << V[1];
          LOG(warn) << " a = " << a << " b=" << b;
          LOG(warn) << vtx.ToString();
          continue;
        }
        double S0 = S[0];
        S[0]      = S[2] / det;
        S[1]      = -S[1] / det;
        S[2]      = S0 / det;
      }
 
      vtx.NofTracks()++;
      usedTracks[itr] = 1;
 
      // Calculate Chi^2
      if (vtx.NofTracks() > 2) {
        vtx.ChiSq() += zeta[0] * zeta[0] * S[0] + 2 * zeta[0] * zeta[1] * S[1] + zeta[1] * zeta[1] * S[2];
      }
      vtx.Ndf() += 2;
 
      // Kalman gain K = CH'*S
 
      double K[3][2];
 
      for (int i = 0; i < 3; ++i) {
        K[i][0] = CHt[i][0] * S[0] + CHt[i][1] * S[1];
        K[i][1] = CHt[i][0] * S[1] + CHt[i][1] * S[2];
      }
 
      // New estimation of the vertex position r += K*zeta
 
      for (int i = 0; i < 3; ++i) {
        r[i] += K[i][0] * zeta[0] + K[i][1] * zeta[1];
      }
 
      // New covariance matrix C -= K*(CH')'
 
      C(Tag00) -= K[0][0] * CHt[0][0] + K[0][1] * CHt[0][1];
 
      C(Tag10) -= K[1][0] * CHt[0][0] + K[1][1] * CHt[0][1];
      C(Tag11) -= K[1][0] * CHt[1][0] + K[1][1] * CHt[1][1];
 
      C(Tag20) -= K[2][0] * CHt[0][0] + K[2][1] * CHt[0][1];
      C(Tag21) -= K[2][0] * CHt[1][0] + K[2][1] * CHt[1][1];
      C(Tag22) -= K[2][0] * CHt[2][0] + K[2][1] * CHt[2][1];
 
      if constexpr (FlagFitTime == DoFitTime::Y) {  // fit the time separately in a simple way
 
        double dt = m[5] - vtx.Time();  // time residual
        double s  = V[15] + C(Tag33);
        if (s > 1.E-10) {
          vtx.ChiSqTime() += dt * dt / s;
          vtx.NdfTime() += 1;
          double Kt = C(Tag33) / s;
          vtx.Time() += Kt * dt;
          C(Tag33) -= Kt * C(Tag33);
        }
      }
 
    }  // end loop over tracks
 
    if (vtx.GetNofTracks() < 2) {
      if (iteration > 0) {
        LOG(warn) << "VertexFitter: only " << vtx.GetNofTracks() << " tracks contributed to the vertex in the "
                  << iteration << " iteration, vertex fit failed.";
      }
      break;
    }
  }  // end of vertex fitting iterations
 
  return std::make_tuple(vtx, usedTracks);
}
 
template<DoFitTime FlagFitTime>
void VertexFitter<FlagFitTime>::dummy()
{
  // instantiations of FitVertex method
  FitVertex<double>(std::vector<const TrackParam<double>*>(), std::vector<const TrackParam<double>*>(), VertexD{});
  FitVertex<float>(std::vector<const TrackParam<float>*>(), std::vector<const TrackParam<float>*>(), VertexD{});
}
 
// Template instantiations
 
template class cbm::algo::kf::VertexFitter<DoFitTime::N>;
template class cbm::algo::kf::VertexFitter<DoFitTime::Y>;