CDCObservations2D.cc

/**************************************************************************
 * basf2 (Belle II Analysis Software Framework)                           *
 * Author: The Belle II Collaboration                                     *
 *                                                                        *
 * See git log for contributors and copyright holders.                    *
 * This file is licensed under LGPL-3.0, see LICENSE.md.                  *
 **************************************************************************/
#include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
 
#include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
 
#include <tracking/trackingUtilities/eventdata/tracks/CDCTrack.h>
#include <tracking/trackingUtilities/eventdata/tracks/CDCAxialSegmentPair.h>
#include <tracking/trackingUtilities/eventdata/segments/CDCSegment3D.h>
#include <tracking/trackingUtilities/eventdata/segments/CDCSegment2D.h>
#include <tracking/trackingUtilities/eventdata/segments/CDCWireHitSegment.h>
#include <tracking/trackingUtilities/eventdata/hits/CDCFacet.h>
#include <tracking/trackingUtilities/eventdata/hits/CDCRLWireHitTriple.h>
#include <tracking/trackingUtilities/eventdata/hits/CDCRLWireHitPair.h>
#include <tracking/trackingUtilities/eventdata/hits/CDCRLWireHit.h>
#include <tracking/trackingUtilities/eventdata/hits/CDCWireHit.h>
#include <tracking/trackingUtilities/eventdata/trajectories/CDCTrajectory2D.h>
#include <tracking/trackingUtilities/geometry/VectorUtil.h>
 
#include <cdc/topology/CDCWire.h>
 
#include <Math/Vector2D.h>
 
using namespace Belle2;
using namespace CDC;
using namespace TrackFindingCDC;
using namespace TrackingUtilities;
 
double CDCObservations2D::getPseudoDriftLengthVariance(const CDCWireHit& wireHit)
{
  return getPseudoDriftLengthVariance(wireHit.getRefDriftLength(),
                                      wireHit.getRefDriftLengthVariance());
}
 
std::size_t
CDCObservations2D::fill(double x, double y, double signedRadius, double weight)
{
  if (std::isnan(x)) return 0;
  if (std::isnan(y)) return 0;
 
  if (std::isnan(signedRadius)) {
    B2WARNING("Signed radius is nan. Skipping observation");
    return 0;
  }
 
  if (std::isnan(weight)) {
    B2WARNING("Weight is nan. Skipping observation");
    return 0;
  }
 
  m_observations.push_back(x);
  m_observations.push_back(y);
  m_observations.push_back(signedRadius);
  m_observations.push_back(weight);
  return 1;
}
 
std::size_t
CDCObservations2D::fill(const ROOT::Math::XYVector& pos2D, double signedRadius, double weight)
{
  return fill(pos2D.x(), pos2D.y(), signedRadius, weight);
}
 
std::size_t CDCObservations2D::append(const CDCWireHit& wireHit, ERightLeft rlInfo)
{
  const ROOT::Math::XYVector& wireRefPos2D = wireHit.getRefPos2D();
 
  double signedDriftLength = 0;
  if (m_fitPos == EFitPos::c_RLDriftCircle and isValid(rlInfo)) {
    signedDriftLength = static_cast<double>(rlInfo) * wireHit.getRefDriftLength();
  } else {
    signedDriftLength = 0;
  }
 
  double variance = 1;
  if (m_fitVariance == EFitVariance::c_Unit) {
    variance = 1;
  } else if (m_fitVariance == EFitVariance::c_Nominal) {
    variance = CDCWireHit::c_simpleDriftLengthVariance;
  } else if (m_fitVariance == EFitVariance::c_DriftLength) {
    const double driftLength = wireHit.getRefDriftLength();
    variance = fabs(driftLength);
  } else if (m_fitVariance == EFitVariance::c_Pseudo) {
    variance = getPseudoDriftLengthVariance(wireHit);
  } else if (m_fitVariance == EFitVariance::c_Proper) {
    if (abs(rlInfo) != 1) {
      variance = getPseudoDriftLengthVariance(wireHit);
    } else {
      variance = wireHit.getRefDriftLengthVariance();
    }
  }
  return fill(wireRefPos2D, signedDriftLength, 1 / variance);
}
 
std::size_t CDCObservations2D::append(const CDCWireHit* wireHit, ERightLeft rlInfo)
{
  if (wireHit) {
    return append(*(wireHit), rlInfo);
  } else {
    return 0;
  }
}
 
std::size_t CDCObservations2D::append(const CDCRLWireHit& rlWireHit)
{
  const ERightLeft rlInfo = rlWireHit.getRLInfo();
 
  const double driftLength = rlWireHit.getRefDriftLength();
  const double driftLengthVariance = rlWireHit.getRefDriftLengthVariance();
 
  const ROOT::Math::XYVector& wireRefPos2D = rlWireHit.getRefPos2D();
 
  double signedDriftLength = 0;
  if (m_fitPos == EFitPos::c_RLDriftCircle and isValid(rlInfo)) {
    signedDriftLength = static_cast<double>(rlInfo) * driftLength;
  } else {
    signedDriftLength = 0;
  }
 
  double variance = 1;
  if (m_fitVariance == EFitVariance::c_Unit) {
    variance = 1;
  } else if (m_fitVariance == EFitVariance::c_Nominal) {
    variance = CDCWireHit::c_simpleDriftLengthVariance;
  } else if (m_fitVariance == EFitVariance::c_DriftLength) {
    variance = fabs(driftLength);
  } else if (m_fitVariance == EFitVariance::c_Pseudo) {
    variance = getPseudoDriftLengthVariance(driftLength, driftLengthVariance);
  } else if (m_fitVariance == EFitVariance::c_Proper) {
    if (abs(rlInfo) != 1) {
      variance = getPseudoDriftLengthVariance(driftLength, driftLengthVariance);
    } else {
      variance = driftLengthVariance;
    }
  }
 
  return fill(wireRefPos2D, signedDriftLength, 1 / variance);
}
 
std::size_t CDCObservations2D::append(const CDCRLWireHitPair& rlWireHitPair)
{
  return append(rlWireHitPair.getFromRLWireHit()) + append(rlWireHitPair.getToRLWireHit());
}
 
std::size_t CDCObservations2D::append(const CDCRLWireHitTriple& rlWireHitTriple)
{
  return append(rlWireHitTriple.getStartRLWireHit()) +
         append(rlWireHitTriple.getMiddleRLWireHit()) + append(rlWireHitTriple.getEndRLWireHit());
}
 
std::size_t CDCObservations2D::append(const CDCFacet& facet)
{
  if (m_fitPos == EFitPos::c_RecoPos) {
    return append(facet.getStartRecoHit2D()) + append(facet.getMiddleRecoHit2D()) +
           append(facet.getEndRecoHit2D());
  } else {
    const CDCRLWireHitTriple& rlWireHitTriple = facet;
    return append(rlWireHitTriple);
  }
}
 
std::size_t CDCObservations2D::append(const CDCRecoHit2D& recoHit2D)
{
  ROOT::Math::XYVector fitPos2D;
  double signedDriftLength = 0;
  if (m_fitPos == EFitPos::c_RecoPos) {
    fitPos2D = recoHit2D.getRecoPos2D();
    signedDriftLength = 0;
 
    // Fall back to the rl circle in case position is not setup
    if (VectorUtil::hasNAN(fitPos2D)) {
      fitPos2D = recoHit2D.getWire().getRefPos2D();
      signedDriftLength = recoHit2D.getSignedRefDriftLength();
    }
 
  } else if (m_fitPos == EFitPos::c_RLDriftCircle) {
    fitPos2D = recoHit2D.getWire().getRefPos2D();
    signedDriftLength = recoHit2D.getSignedRefDriftLength();
  } else if (m_fitPos == EFitPos::c_WirePos) {
    fitPos2D = recoHit2D.getWire().getRefPos2D();
    signedDriftLength = 0;
  }
 
  const double driftLength = recoHit2D.getRefDriftLength();
  const ERightLeft rlInfo = recoHit2D.getRLInfo();
 
  double variance = recoHit2D.getRefDriftLengthVariance();
  if (m_fitVariance == EFitVariance::c_Unit) {
    variance = 1;
  } else if (m_fitVariance == EFitVariance::c_Nominal) {
    variance = CDCWireHit::c_simpleDriftLengthVariance;
  } else if (m_fitVariance == EFitVariance::c_DriftLength) {
    variance = std::fabs(driftLength);
  } else if (m_fitVariance == EFitVariance::c_Pseudo or abs(rlInfo) != 1) {
    // Fall back to the pseudo variance if the rl information is not known
    variance = getPseudoDriftLengthVariance(driftLength, variance);
  }
  return fill(fitPos2D, signedDriftLength, 1 / variance);
}
 
std::size_t CDCObservations2D::append(const CDCRecoHit3D& recoHit3D)
{
  ROOT::Math::XYVector fitPos2D = recoHit3D.getRecoPos2D();
  double signedDriftLength = 0;
  if (m_fitPos == EFitPos::c_RecoPos) {
    signedDriftLength = 0;
  } else if (m_fitPos == EFitPos::c_RLDriftCircle) {
    fitPos2D = recoHit3D.getRecoWirePos2D();
    signedDriftLength = recoHit3D.getSignedRecoDriftLength();
  } else if (m_fitPos == EFitPos::c_WirePos) {
    fitPos2D = recoHit3D.getRecoWirePos2D();
    signedDriftLength = 0;
  }
 
  const double driftLength = std::fabs(recoHit3D.getSignedRecoDriftLength());
  const ERightLeft rlInfo = recoHit3D.getRLInfo();
 
  double variance = recoHit3D.getRecoDriftLengthVariance();
  if (m_fitVariance == EFitVariance::c_Unit) {
    variance = 1;
  } else if (m_fitVariance == EFitVariance::c_Nominal) {
    variance = CDCWireHit::c_simpleDriftLengthVariance;
  } else if (m_fitVariance == EFitVariance::c_DriftLength) {
    variance = std::fabs(driftLength);
  } else if (m_fitVariance == EFitVariance::c_Pseudo or abs(rlInfo) != 1) {
    // Fall back to the pseudo variance if the rl information is not known
    variance = getPseudoDriftLengthVariance(driftLength, variance);
  }
  return fill(fitPos2D, signedDriftLength, 1 / variance);
}
 
std::size_t CDCObservations2D::appendRange(const CDCSegment2D& segment2D)
{
  std::size_t nAppendedHits = 0;
  for (const CDCRecoHit2D& recoHit2D : segment2D) {
    nAppendedHits += append(recoHit2D);
  }
  return nAppendedHits;
}
 
std::size_t CDCObservations2D::appendRange(const CDCSegment3D& segment3D)
{
  std::size_t nAppendedHits = 0;
  for (const CDCRecoHit3D& recoHit3D : segment3D) {
    nAppendedHits += append(recoHit3D);
  }
  return nAppendedHits;
}
 
std::size_t CDCObservations2D::appendRange(const CDCAxialSegmentPair& axialSegmentPair)
{
  std::size_t nAppendedHits = 0;
  const CDCSegment2D* ptrStartSegment2D = axialSegmentPair.getStartSegment();
  if (ptrStartSegment2D) {
    const CDCSegment2D& startSegment2D = *ptrStartSegment2D;
    nAppendedHits += appendRange(startSegment2D);
  }
 
  const CDCSegment2D* ptrEndSegment2D = axialSegmentPair.getEndSegment();
  if (ptrEndSegment2D) {
    const CDCSegment2D& endSegment2D = *ptrEndSegment2D;
    nAppendedHits += appendRange(endSegment2D);
  }
  return nAppendedHits;
}
 
std::size_t CDCObservations2D::appendRange(const CDCTrack& track)
{
  std::size_t nAppendedHits = 0;
  for (const CDCRecoHit3D& recoHit3D : track) {
    nAppendedHits += append(recoHit3D);
  }
  return nAppendedHits;
}
 
std::size_t CDCObservations2D::appendRange(const std::vector<const CDCWire*>& wires)
{
  std::size_t nAppendedHits = 0;
  for (const CDCWire* ptrWire : wires) {
    if (not ptrWire) continue;
    const CDCWire& wire = *ptrWire;
    const ROOT::Math::XYVector& wirePos = wire.getRefPos2D();
    const double driftLength = 0.0;
    const double weight = 1.0;
    nAppendedHits += fill(wirePos, driftLength, weight);
  }
  return nAppendedHits;
}
 
std::size_t CDCObservations2D::appendRange(const CDCWireHitSegment& wireHits)
{
  std::size_t nAppendedHits = 0;
  for (const CDCWireHit* ptrWireHit : wireHits) {
    if (not ptrWireHit) continue;
    const CDCWireHit& wireHit = *ptrWireHit;
    nAppendedHits += append(wireHit);
  }
  return nAppendedHits;
}
 
double CDCObservations2D::getTotalPerpS(const CDCTrajectory2D& trajectory2D) const
{
  return trajectory2D.calcArcLength2DBetween(getFrontPos2D(), getBackPos2D());
}
 
std::size_t CDCObservations2D::getNObservationsWithDriftRadius() const
{
  std::size_t result = 0;
  Index nObservations = size();
 
  for (Index iObservation = 0; iObservation < nObservations; ++iObservation) {
    const double driftLength = getDriftLength(iObservation);
    bool hasDriftLength = (driftLength != 0.0);
    result += hasDriftLength ? 1 : 0;
  }
 
  return result;
}
 
ROOT::Math::XYVector CDCObservations2D::getCentralPoint() const
{
  std::size_t n = size();
  if (n == 0) return ROOT::Math::XYVector(NAN, NAN);
  std::size_t i = n / 2;
 
  if (isEven(n)) {
    // For even number of observations use the middle one with the bigger distance from IP
    ROOT::Math::XYVector center1(getX(i), getY(i));
    ROOT::Math::XYVector center2(getX(i - 1), getY(i - 1));
    return center1.Mag2() > center2.Mag2() ? center1 : center2;
  } else {
    ROOT::Math::XYVector center1(getX(i), getY(i));
    return center1;
  }
}
 
void CDCObservations2D::passiveMoveBy(const ROOT::Math::XYVector& origin)
{
  Eigen::Matrix<double, 1, 2> eigenOrigin(origin.x(), origin.y());
  EigenObservationMatrix eigenObservations = getEigenObservationMatrix(this);
  eigenObservations.leftCols<2>().rowwise() -= eigenOrigin;
}
 
ROOT::Math::XYVector CDCObservations2D::centralize()
{
  // Pick an observation at the center
  ROOT::Math::XYVector centralPoint = getCentralPoint();
  passiveMoveBy(centralPoint);
  return centralPoint;
}