Add regularization for degenerate matrices

Author: shahor02

Common/DCAFitter/DCAFitterN_derivation.md

@@ -431,7 +431,112 @@ TrackParCov(xyz, pxpypz, cov, charge, sectorAlpha)
 then transforms this lab covariance to the selected parent track frame,
 including the parent `alpha` convention.
 
-## 10. Code changes summarized
+## 10. Regularization of singular or weakly constrained covariance matrices
+
+### Single-track `TrackCovI`
+
+The matrix
+
+```text
+I = H^T W H
+```
+
+has rank at most two for one track, because one track measures only the two
+coordinates transverse to its own trajectory.  The null direction is the local
+track tangent
+
+```text
+t = (1, y_prime, z_prime)^T .
+```
+
+Indeed
+
+```text
+H t = 0,        I t = 0 .
+```
+
+Therefore a single-track `TrackCovI` is positive semidefinite, not positive
+definite.  Inverting this 3D matrix by itself is not meaningful; the apparent
+negative diagonal elements seen in such an inverse are numerical symptoms of
+trying to invert a rank-deficient information matrix.  The physically meaningful
+inverse at single-track level is only the original 2 by 2 `(Y,Z)` covariance.
+
+For the multi-track vertex fit, different track directions usually make
+
+```text
+A = sum_i R_i I_i R_i^T
+```
+
+full rank.  However, nearly parallel or otherwise weak geometries can still make
+the longitudinal eigenvalue very small.  To avoid an exactly singular
+single-track contribution while preserving the measured `(Y,Z)` block, the code
+may add a weak positive longitudinal prior only to `I_XX`:
+
+```text
+I_XX -> I_XX + 1/sigma_X,prior^2 .
+```
+
+In code this is implemented as
+
+```cpp
+constexpr float XRegErrFactor = 10.f;
+const float sigmaX2 = C_YY * XRegErrFactor;
+sxx += 1.f / sigmaX2;
+```
+
+This is different from multiplying `I_XX` by a number below one.  A reduction of
+`I_XX` can make the matrix indefinite, while adding a positive diagonal term
+keeps the information matrix positive definite in the tangent direction and does
+not alter `I_YY`, `I_YZ`, or `I_ZZ`.
+
+The regularization should remain weak.  It is a numerical stabilizer for badly
+conditioned geometries, not an additional detector measurement of the local
+track `X` coordinate.
+
+### `calcPCACovMatrix()`
+
+The vertex covariance is
+
+```text
+C_V = A^-1,        A = sum_i R_i I_i R_i^T .
+```
+
+If `A` is singular or ill-conditioned, returning a small fallback covariance
+would incorrectly shrink the uncertainty along the weakly constrained direction.
+The conservative behavior is:
+
+1. Check that `A` is compatible with a positive-definite information matrix.
+2. Reject cases where the determinant is too small compared with the diagonal
+   scale.
+3. Invert only when these checks pass.
+4. If inversion fails, or if the inverted covariance has non-positive diagonal
+   elements, return a deliberately loose fallback covariance.
+
+The code uses the leading-minor checks
+
+```text
+A_XX > 0,
+det(A_XY block) > 0,
+det(A) > epsilon max(diag(A))^3
+```
+
+with
+
+```text
+epsilon = 1e-12 .
+```
+
+On failure, it returns a diagonal covariance with
+
+```text
+sigma^2 = 1e6 cm^2 .
+```
+
+This corresponds to a 10 m uncertainty in each coordinate.  It is intentionally
+loose: the fallback marks the parent vertex as poorly constrained rather than
+creating an artificially precise parent covariance.
+
+## 11. Code changes summarized
 
 1. `TrackCovI` now stores a full symmetric local 3D information matrix.
 2. The dummy `XerrFactor` approximation was removed.

Common/DCAFitter/include/DCAFitter/DCAFitterN.h

@@ -59,6 +59,10 @@ struct TrackCovI {
     sxy = -(syy * dydx + syz * dzdx);
     sxz = -(syz * dydx + szz * dzdx);
     sxx = dydx * dydx * syy + 2.f * dydx * dzdx * syz + dzdx * dzdx * szz;
+    // The matrix is degenerate by construction, regularize sxx term to preserve the original YZ block exactly
+    constexpr float XRegErrFactor = 10.f;
+    const float sigmaX2 = cyy * XRegErrFactor;
+    sxx += 1.f / sigmaX2;
     return res;
   }
 };
@@ -220,6 +224,7 @@ class DCAFitterN
   ///< recalculate PCA as a cov-matrix weighted mean, even if absDCA method was used
   GPUd() bool recalculatePCAWithErrors(int cand = 0);
 
+  GPUd() double calcCollinearInflation(int cand) const;
   GPUd() MatSym3D calcPCACovMatrix(int cand = 0) const;
 
   std::array<float, 6> calcPCACovMatrixFlat(int cand = 0) const
@@ -385,9 +390,10 @@ class DCAFitterN
   std::array<int, MAXHYP> mNIters;           // number of iterations for each seed
   std::array<bool, MAXHYP> mTrPropDone{};    // Flag that the tracks are fully propagated to PCA
   std::array<bool, MAXHYP> mPropFailed{};    // Flag that some propagation failed for this PCA candidate
-  LogLogThrottler mLoggerBadCov{};
-  LogLogThrottler mLoggerBadInv{};
-  LogLogThrottler mLoggerBadProp{};
+  mutable LogLogThrottler mLoggerBadCov{};
+  mutable LogLogThrottler mLoggerBadInv{};
+  mutable LogLogThrottler mLoggerBadProp{};
+  mutable LogLogThrottler mLoggerBadPCACov{};
   MatSym3D mWeightInv; // inverse weight of single track, [sum{M^T E M}]^-1 in EQ.T
   std::array<int, MAXHYP> mOrder{0};
   int mCurHyp = 0;
@@ -800,6 +806,50 @@ GPUd() void DCAFitterN<N, Args...>::calcPCANoErr()
   pca[2] *= NInv;
 }
 
+//___________________________________________________________________
+template <int N, typename... Args>
+GPUd() double DCAFitterN<N, Args...>::calcCollinearInflation(int cand) const
+{
+  std::array<std::array<double, 3>, N> u{};
+  int nu = 0;
+
+  for (int i = 0; i < N; ++i) {
+    std::array<float, 3> p{};
+    if (!getTrack(i, cand).getPxPyPzGlo(p)) {
+      continue;
+    }
+    const double p2 = p[0] * p[0] + p[1] * p[1] + p[2] * p[2];
+    if (p2 <= 0.) {
+      continue;
+    }
+    const double pI = 1. / std::sqrt(p2);
+    u[nu++] = {p[0] * pI, p[1] * pI, p[2] * pI};
+  }
+
+  if (nu < 2) {
+    return 1.;
+  }
+
+  double sin2Mean = 0.;
+  int npairs = 0;
+  for (int i = 0; i < nu; ++i) {
+    for (int j = i + 1; j < nu; ++j) {
+      double cij = u[i][0] * u[j][0] + u[i][1] * u[j][1] + u[i][2] * u[j][2];
+      cij = std::clamp(cij, -1., 1.);
+      sin2Mean += std::max(0., 1. - cij * cij);
+      ++npairs;
+    }
+  }
+  sin2Mean /= npairs;
+
+  constexpr double Sin2Ref = 1.e-5;
+  constexpr double MaxInflation = 1.e4;
+  if (sin2Mean <= 0.) {
+    return MaxInflation;
+  }
+  return sin2Mean < Sin2Ref ? std::min(MaxInflation, Sin2Ref / sin2Mean) : 1.;
+}
+
 //___________________________________________________________________
 template <int N, typename... Args>
 GPUd() o2::math_utils::SMatrix<double, 3, 3, o2::math_utils::MatRepSym<double, 3>> DCAFitterN<N, Args...>::calcPCACovMatrix(int cand) const
@@ -829,14 +879,34 @@ GPUd() o2::math_utils::SMatrix<double, 3, 3, o2::math_utils::MatRepSym<double, 3
     arrmat[YZ] += taux.s * tcov.sxz + taux.c * tcov.syz;
     arrmat[ZZ] += tcov.szz;
   }
-  if (info.Invert()) {
-    return info;
+  const double maxDiag = o2::gpu::GPUCommonMath::Max(o2::gpu::GPUCommonMath::Max(info(0, 0), info(1, 1)), info(2, 2));
+  const double det2 = info(0, 0) * info(1, 1) - info(1, 0) * info(1, 0);
+  const double det3 = info(0, 0) * (info(1, 1) * info(2, 2) - info(2, 1) * info(2, 1)) -
+                      info(1, 0) * (info(1, 0) * info(2, 2) - info(2, 1) * info(2, 0)) +
+                      info(2, 0) * (info(1, 0) * info(2, 1) - info(1, 1) * info(2, 0));
+  constexpr double MinRelDet = 1.e-12;
+  constexpr double InflateRelDet = 1.e-6;
+  constexpr double MaxInflation = 1.e4;
+  const bool isWellConditionedInfo = maxDiag > 0. && info(0, 0) > 0. && det2 > 0. && det3 > MinRelDet * maxDiag * maxDiag * maxDiag;
+  if (isWellConditionedInfo) {
+    auto cov = info;
+    if (cov.Invert() && cov(0, 0) > 0. && cov(1, 1) > 0. && cov(2, 2) > 0.) {
+      // if (mIsCollinear) {
+      //   cov *= calcCollinearInflation(cand);
+      // }
+      return cov;
+    }
+  }
+  if (mLoggerBadPCACov.needToLog()) {
+    printf("fitter %d: error (%ld muted): override ill-conditioned PCACovMatrix by dummy matrix", mFitterID, mLoggerBadPCACov.evCount);
   }
-  // Fall back on identity diagonal matrix with 1 cm error.
+  // Fall back on a deliberately loose vertex covariance. Returning a tight
+  // identity covariance for a singular or ill-conditioned information matrix
+  // would shrink the uncertainty in the weakly constrained direction.
   memset(arrmat, 0, sizeof(info));
-  info(0, 0) = 1.;
-  info(1, 1) = 1.;
-  info(2, 2) = 1.;
+  info(0, 0) = 4.;
+  info(1, 1) = 4.;
+  info(2, 2) = 4.;
   return info;
 }
 
@@ -900,6 +970,15 @@ GPUd() bool DCAFitterN<N, Args...>::correctTracks(const VecND& corrX)
   // constant-Bz transport here: Newton corrections are small, but the track
   // state must stay synchronized with mTrPos for the next derivative update.
   for (int i = N; i--;) {
+    /*
+      // Updating only mTrPos by Taylor expansion leaves mCandTr at the previous X,
+      // leaving calcTrackDerivatives() insensitive to the update. Use full fast propagation instead.
+      const auto& trDer = mTrDer[mCurHyp][i];
+      auto dx2h = 0.5 * corrX[i] * corrX[i];
+      mTrPos[mCurHyp][i][0] -= corrX[i];
+      mTrPos[mCurHyp][i][1] -= trDer.dydx * corrX[i] - dx2h * trDer.d2ydx2;
+      mTrPos[mCurHyp][i][2] -= trDer.dzdx * corrX[i] - dx2h * trDer.d2zdx2;
+    */
     auto& trc = mCandTr[mCurHyp][i];
     const float x = static_cast<float>(mTrPos[mCurHyp][i][0] - corrX[i]);
     const bool propagated = mUseAbsDCA ? trc.propagateParamTo(x, mBz) : trc.propagateTo(x, mBz);
@@ -1291,9 +1370,9 @@ GPUdi() bool DCAFitterN<N, Args...>::propagateParamToX(o2::track::TrackPar& t, f
     mPropFailed[mCurHyp] = true;
     if (mLoggerBadProp.needToLog()) {
 #ifndef GPUCA_GPUCODE
-      printf("fitter %d: error (%ld muted): propagation failed for %s\n", mFitterID, mLoggerBadProp.evCount, t.asString().c_str());
+      printf("fitter %d: error (%ld muted): propagation to %.4f failed for %s\n", mFitterID, mLoggerBadProp.evCount, x, t.asString().c_str());
 #else
-      printf("fitter %d: error (%ld muted): propagation failed\n", mFitterID, mLoggerBadProp.evCount);
+      printf("fitter %d: error (%ld muted): propagation to %.4f failed\n", mFitterID, mLoggerBadProp.evCount, x);
 #endif
     }
   }
@@ -1317,9 +1396,9 @@ GPUdi() bool DCAFitterN<N, Args...>::propagateToX(o2::track::TrackParCov& t, flo
     mPropFailed[mCurHyp] = true;
     if (mLoggerBadProp.needToLog()) {
 #ifndef GPUCA_GPUCODE
-      printf("fitter %d: error (%ld muted): propagation failed for %s\n", mFitterID, mLoggerBadProp.evCount, t.asString().c_str());
+      printf("fitter %d: error (%ld muted): propagation to %.4f failed for %s\n", mFitterID, mLoggerBadProp.evCount, x, t.asString().c_str());
 #else
-      printf("fitter %d: error (%ld muted): propagation failed\n", mFitterID, mLoggerBadProp.evCount);
+      printf("fitter %d: error (%ld muted): propagation to %.4f failed\n", mFitterID, mLoggerBadProp.evCount, x);
 #endif
     }
   }