BBA: bugfix in the track smoothing

algo/ca/core/utils/CaUtils.cxx

@@ -40,3 +40,330 @@ namespace cbm::algo::ca::utils
     }
   }
 }  // namespace cbm::algo::ca::utils
+
+
+namespace cbm::algo::ca::utils::math
+{
+
+  void CholeskyFactorization(const double a[], const int n, int nn, double u[], int* nullty, int* ifault)
+  {
+    //
+    //  Purpose:
+    //
+    //    CHOLESKY computes the Cholesky factorization of a PDS matrix.
+    //
+    //  Discussion:
+    //
+    //    For a positive definite symmetric matrix A, the Cholesky factor U
+    //    is an upper triangular matrix such that A = U' * U.
+    //
+    //    This routine was originally named "CHOL", but that conflicted with
+    //    a built in MATLAB routine name.
+    //
+    //    The missing initialization "II = 0" has been added to the code.
+    //
+    //  Licensing:
+    //
+    //    This code is distributed under the GNU LGPL license.
+    //
+    //  Modified:
+    //
+    //    12 February 2008
+    //
+    //  Author:
+    //
+    //    Original FORTRAN77 version by Michael Healy.
+    //    Modifications by AJ Miller.
+    //    C++ version by John Burkardt.
+    //
+    //  Reference:
+    //
+    //    Michael Healy,
+    //    Algorithm AS 6:
+    //    Triangular decomposition of a symmetric matrix,
+    //    Applied Statistics,
+    //    Volume 17, Number 2, 1968, pages 195-197.
+    //
+    //  Parameters:
+    //
+    //    Input, double A((N*(N+1))/2), a positive definite matrix
+    //    stored by rows in lower triangular form as a one dimensional array,
+    //    in the sequence
+    //    A(1,1),
+    //    A(2,1), A(2,2),
+    //    A(3,1), A(3,2), A(3,3), and so on.
+    //
+    //    Input, int N, the order of A.
+    //
+    //    Input, int NN, the dimension of the array used to store A,
+    //    which should be at least (N*(N+1))/2.
+    //
+    //    Output, double U((N*(N+1))/2), an upper triangular matrix,
+    //    stored by columns, which is the Cholesky factor of A.  The program is
+    //    written in such a way that A and U can share storage.
+    //
+    //    Output, int NULLTY, the rank deficiency of A.  If NULLTY is zero,
+    //    the matrix is judged to have full rank.
+    //
+    //    Output, int IFAULT, an error indicator.
+    //    0, no error was detected;
+    //    1, if N < 1;
+    //    2, if A is not positive semi-definite.
+    //    3, if NN < (N*(N+1))/2.
+    //
+    //  Local Parameters:
+    //
+    //    Local, double ETA, should be set equal to the smallest positive
+    //    value such that 1.0 + ETA is calculated as being greater than 1.0 in the
+    //    accuracy being used.
+    //
+
+    double eta = 1.0E-09;
+    int i;
+    int icol;
+    int ii;
+    int irow;
+    int j;
+    int k;
+    int kk;
+    int l;
+    int m;
+    double w;
+    double x;
+
+    *ifault = 0;
+    *nullty = 0;
+
+    if (n <= 0) {
+      *ifault = 1;
+      return;
+    }
+
+    if (nn < (n * (n + 1)) / 2) {
+      *ifault = 3;
+      return;
+    }
+
+    j  = 1;
+    k  = 0;
+    ii = 0;
+    //
+    //  Factorize column by column, ICOL = column number.
+    //
+    for (icol = 1; icol <= n; icol++) {
+      ii = ii + icol;
+      x  = eta * eta * a[ii - 1];
+      l  = 0;
+      kk = 0;
+      //
+      //  IROW = row number within column ICOL.
+      //
+      for (irow = 1; irow <= icol; irow++) {
+        kk = kk + irow;
+        k  = k + 1;
+        w  = a[k - 1];
+        m  = j;
+
+        for (i = 1; i < irow; i++) {
+          l = l + 1;
+          w = w - u[l - 1] * u[m - 1];
+          m = m + 1;
+        }
+
+        l = l + 1;
+
+        if (irow == icol) {
+          break;
+        }
+
+        if (u[l - 1] != 0.0) {
+          u[k - 1] = w / u[l - 1];
+        }
+        else {
+          u[k - 1] = 0.0;
+
+          if (fabs(x * a[k - 1]) < w * w) {
+            *ifault = 2;
+            return;
+          }
+        }
+      }
+      //
+      //  End of row, estimate relative accuracy of diagonal element.
+      //
+      if (fabs(w) <= fabs(eta * a[k - 1])) {
+        u[k - 1] = 0.0;
+        *nullty  = *nullty + 1;
+      }
+      else {
+        if (w < 0.0) {
+          *ifault = 2;
+          return;
+        }
+        u[k - 1] = sqrt(w);
+      }
+      j = j + icol;
+    }
+
+  }  // CholeskyFactorization
+
+
+  void SymInv(const double a[], const int n, double c[], double w[], int* nullty, int* ifault)
+  {
+    //
+    //  Purpose:
+    //
+    //    SYMINV computes the inverse of a symmetric matrix.
+    //
+    //  Licensing:
+    //
+    //    This code is distributed under the GNU LGPL license.
+    //
+    //  Modified:
+    //
+    //    11 February 2008
+    //
+    //  Author:
+    //
+    //    Original FORTRAN77 version by Michael Healy.
+    //    C++ version by John Burkardt.
+    //
+    //  Reference:
+    //
+    //    Michael Healy,
+    //    Algorithm AS 7:
+    //    Inversion of a Positive Semi-Definite Symmetric Matrix,
+    //    Applied Statistics,
+    //    Volume 17, Number 2, 1968, pages 198-199.
+    //
+    //  Parameters:
+    //
+    //    Input, double A((N*(N+1))/2), a positive definite matrix stored
+    //    by rows in lower triangular form as a one dimensional array, in the sequence
+    //    A(1,1),
+    //    A(2,1), A(2,2),
+    //    A(3,1), A(3,2), A(3,3), and so on.
+    //
+    //    Input, int N, the order of A.
+    //
+    //    Output, double C((N*(N+1))/2), the inverse of A, or generalized
+    //    inverse if A is singular, stored using the same storage scheme employed
+    //    for A.  The program is written in such a way that A and U can share storage.
+    //
+    //    Workspace, double W(N).
+    //
+    //    Output, int *NULLTY, the rank deficiency of A.  If NULLTY is zero,
+    //    the matrix is judged to have full rank.
+    //
+    //    Output, int *IFAULT, error indicator.
+    //    0, no error detected.
+    //    1, N < 1.
+    //    2, A is not positive semi-definite.
+    //
+
+    int i;
+    int icol;
+    int irow;
+    int j;
+    int jcol;
+    int k;
+    int l;
+    int mdiag;
+    int ndiag;
+    int nn;
+    int nrow;
+    double x;
+
+    *ifault = 0;
+
+    if (n <= 0) {
+      *ifault = 1;
+      return;
+    }
+
+    nrow = n;
+    //
+    //  Compute the Cholesky factorization of A.
+    //  The result is stored in C.
+    //
+    nn = (n * (n + 1)) / 2;
+
+    CholeskyFactorization(a, n, nn, c, nullty, ifault);
+
+    if (*ifault != 0) {
+      return;
+    }
+    //
+    //  Invert C and form the product (Cinv)' * Cinv, where Cinv is the inverse
+    //  of C, row by row starting with the last row.
+    //  IROW = the row number,
+    //  NDIAG = location of last element in the row.
+    //
+    irow  = nrow;
+    ndiag = nn;
+    //
+    //  Special case, zero diagonal element.
+    //
+    for (;;) {
+      if (c[ndiag - 1] == 0.0) {
+        l = ndiag;
+        for (j = irow; j <= nrow; j++) {
+          c[l - 1] = 0.0;
+          l        = l + j;
+        }
+      }
+      else {
+        l = ndiag;
+        for (i = irow; i <= nrow; i++) {
+          w[i - 1] = c[l - 1];
+          l        = l + i;
+        }
+
+        icol  = nrow;
+        jcol  = nn;
+        mdiag = nn;
+
+        for (;;) {
+          l = jcol;
+
+          if (icol == irow) {
+            x = 1.0 / w[irow - 1];
+          }
+          else {
+            x = 0.0;
+          }
+
+          k = nrow;
+
+          while (irow < k) {
+            x = x - w[k - 1] * c[l - 1];
+            k = k - 1;
+            l = l - 1;
+
+            if (mdiag < l) {
+              l = l - k + 1;
+            }
+          }
+
+          c[l - 1] = x / w[irow - 1];
+
+          if (icol <= irow) {
+            break;
+          }
+          mdiag = mdiag - icol;
+          icol  = icol - 1;
+          jcol  = jcol - 1;
+        }
+      }
+
+      ndiag = ndiag - irow;
+      irow  = irow - 1;
+
+      if (irow <= 0) {
+        break;
+      }
+    }
+
+  }  // SymInv
+
+}  // namespace cbm::algo::ca::utils::math

algo/ca/core/utils/CaUtils.h

@@ -320,3 +320,16 @@ namespace cbm::algo::ca::utils::simd
     out[i] = in;
   }
 }  // namespace cbm::algo::ca::utils::simd
+
+
+/// Namespace contains compile-time constants definition for SIMD operations
+/// in the CA tracking algorithm
+///
+namespace cbm::algo::ca::utils::math
+{
+
+  void CholeskyFactorization(const double a[], const int n, int nn, double u[], int* nullty, int* ifault);
+
+  void SymInv(const double a[], const int n, double c[], double w[], int* nullty, int* ifault);
+
+}  // namespace cbm::algo::ca::utils::math

reco/KF/CbmKfTrackFitter.cxx

@@ -473,7 +473,9 @@ void CbmKfTrackFitter::AddMaterialEffects(const CbmKfTrackFitter::Track& t, CbmK
   fvec msQp0 = t.fIsMsQp0Set ? t.fMsQp0 : tr.GetQp();
 
   fFit.MultipleScattering(n.fRadThick, msQp0);
-  fFit.EnergyLossCorrection(n.fRadThick, upstreamDirection ? fvec::One() : fvec::Zero());
+  if (!t.fIsMsQp0Set) {
+    fFit.EnergyLossCorrection(n.fRadThick, upstreamDirection ? fvec::One() : fvec::Zero());
+  }
 }
 
 
@@ -571,53 +573,95 @@ void CbmKfTrackFitter::FitTrack(CbmKfTrackFitter::Track& t)
 
 void CbmKfTrackFitter::Smooth(ca::TrackParamV& t1, const ca::TrackParamV& t2)
 {
-  // combine two tracks
-  std::tie(t1.X(), t1.Y(), t1.C00(), t1.C10(), t1.C11()) =
-    Smooth2D(t1.X(), t1.Y(), t1.C00(), t1.C10(), t1.C11(), t2.X(), t2.Y(), t2.C00(), t2.C10(), t2.C11());
-
-  std::tie(t1.Tx(), t1.Ty(), t1.C22(), t1.C32(), t1.C33()) =
-    Smooth2D(t1.Tx(), t1.Ty(), t1.C22(), t1.C32(), t1.C33(), t2.Tx(), t2.Ty(), t2.C22(), t2.C32(), t2.C33());
-
-  std::tie(t1.Qp(), t1.C44()) = Smooth1D(t1.Qp(), t1.C44(), t2.Qp(), t2.C44());
-
-  std::tie(t1.Time(), t1.Vi(), t1.C55(), t1.C65(), t1.C66()) =
-    Smooth2D(t1.Time(), t1.Vi(), t1.C55(), t1.C65(), t1.C66(), t2.Time(), t2.Vi(), t2.C55(), t2.C65(), t2.C66());
-
-  t1.C20() = 0.;
-  t1.C21() = 0.;
-  t1.C30() = 0.;
-  t1.C31() = 0.;
-  t1.C40() = 0.;
-  t1.C41() = 0.;
-  t1.C42() = 0.;
-  t1.C43() = 0.;
-  t1.C50() = 0.;
-  t1.C51() = 0.;
-  t1.C52() = 0.;
-  t1.C53() = 0.;
-  t1.C54() = 0.;
-  t1.C60() = 0.;
-  t1.C61() = 0.;
-  t1.C62() = 0.;
-  t1.C63() = 0.;
-  t1.C64() = 0.;
-}
+  // TODO: move to the CaTrackFit class
 
-std::tuple<ca::fvec, ca::fvec> CbmKfTrackFitter::Smooth1D(ca::fvec x1, ca::fvec Cxx1, ca::fvec x2, ca::fvec Cxx2)
-{
-  // combine two 1D values
-  ca::fvec x   = (x1 * Cxx1 + x2 * Cxx2) / (Cxx1 + Cxx2);
-  ca::fvec Cxx = Cxx1 * Cxx2 / (Cxx1 + Cxx2);
-  return std::tuple(x, Cxx);
-}
+  constexpr int nPar = ca::TrackParamV::kNtrackParam;
+  constexpr int nCov = ca::TrackParamV::kNcovParam;
 
-std::tuple<ca::fvec, ca::fvec, ca::fvec, ca::fvec, ca::fvec>
-CbmKfTrackFitter::Smooth2D(ca::fvec x1, ca::fvec y1, ca::fvec Cxx1, ca::fvec /*Cxy1*/, ca::fvec Cyy1, ca::fvec x2,
-                           ca::fvec y2, ca::fvec Cxx2, ca::fvec /*Cxy2*/, ca::fvec Cyy2)
-{
-  // combine two 2D values
-  // TODO: do it right
-  auto [x, Cxx] = Smooth1D(x1, Cxx1, x2, Cxx2);
-  auto [y, Cyy] = Smooth1D(y1, Cyy1, y2, Cyy2);
-  return std::tuple(x, y, Cxx, ca::fvec::Zero(), Cyy);
+  double r[nPar] = {t1.X()[0], t1.Y()[0], t1.Tx()[0], t1.Ty()[0], t1.Qp()[0], t1.Time()[0], t1.Vi()[0]};
+  double m[nPar] = {t2.X()[0], t2.Y()[0], t2.Tx()[0], t2.Ty()[0], t2.Qp()[0], t2.Time()[0], t2.Vi()[0]};
+
+  double S[nCov], S1[nCov], Tmp[nCov];
+  for (int i = 0; i < nCov; i++) {
+    S[i] = (t1.GetCovMatrix()[i] + t2.GetCovMatrix()[i])[0];
+  }
+
+  int nullty = 0;
+  int ifault = 0;
+  cbm::algo::ca::utils::math::SymInv(S, nPar, S1, Tmp, &nullty, &ifault);
+
+  assert(0 == nullty);
+  assert(0 == ifault);
+
+  double dzeta[nPar];
+
+  for (int i = 0; i < nPar; i++) {
+    dzeta[i] = m[i] - r[i];
+  }
+
+  double C[nPar][nPar];
+  double Si[nPar][nPar];
+  for (int i = 0, k = 0; i < nPar; i++) {
+    for (int j = 0; j <= i; j++, k++) {
+      C[i][j]  = t1.GetCovMatrix()[k][0];
+      Si[i][j] = S1[k];
+      C[j][i]  = C[i][j];
+      Si[j][i] = Si[i][j];
+    }
+  }
+
+  // K = C * S^{-1}
+  double K[nPar][nPar];
+  for (int i = 0; i < nPar; i++) {
+    for (int j = 0; j < nPar; j++) {
+      K[i][j] = 0.;
+      for (int k = 0; k < nPar; k++) {
+        K[i][j] += C[i][k] * Si[k][j];
+      }
+    }
+  }
+
+  for (int i = 0, k = 0; i < nPar; i++) {
+    for (int j = 0; j <= i; j++, k++) {
+      double kc = 0.;  // K*C[i][j]
+      for (int l = 0; l < nPar; l++) {
+        kc += K[i][l] * C[l][j];
+      }
+      t1.CovMatrix()[k] = t1.CovMatrix()[k][0] - kc;
+    }
+  }
+
+  for (int i = 0; i < nPar; i++) {
+    double kd = 0.;
+    for (int j = 0; j < nPar; j++) {
+      kd += K[i][j] * dzeta[j];
+    }
+    r[i] += kd;
+  }
+  t1.X()    = r[0];
+  t1.Y()    = r[1];
+  t1.Tx()   = r[2];
+  t1.Ty()   = r[3];
+  t1.Qp()   = r[4];
+  t1.Time() = r[5];
+  t1.Vi()   = r[6];
+
+  double chi2     = 0.;
+  double chi2Time = 0.;
+  for (int i = 0; i < nPar; i++) {
+    double SiDzeta = 0.;
+    for (int j = 0; j < nPar; j++) {
+      SiDzeta += Si[i][j] * dzeta[j];
+    }
+    if (i < 5) {
+      chi2 += dzeta[i] * SiDzeta;
+    }
+    else {
+      chi2Time += dzeta[i] * SiDzeta;
+    }
+  }
+  t1.SetChiSq(chi2 + t1.GetChiSq()[0] + t2.GetChiSq()[0]);
+  t1.SetChiSqTime(chi2Time + t1.GetChiSqTime()[0] + t2.GetChiSqTime()[0]);
+  t1.SetNdf(5 + t1.GetNdf()[0] + t2.GetNdf()[0]);
+  t1.SetNdfTime(2 + t1.GetNdfTime()[0] + t2.GetNdfTime()[0]);
 }

reco/KF/CbmKfTrackFitter.h

@@ -126,12 +126,6 @@ class CbmKfTrackFitter {
   // combine two tracks
   void Smooth(ca::TrackParamV& t1, const ca::TrackParamV& t2);
 
-  std::tuple<ca::fvec, ca::fvec> Smooth1D(ca::fvec x1, ca::fvec Cxx1, ca::fvec x2, ca::fvec Cxx2);
-  std::tuple<ca::fvec, ca::fvec, ca::fvec, ca::fvec, ca::fvec> Smooth2D(ca::fvec x1, ca::fvec y1, ca::fvec Cxx1,
-                                                                        ca::fvec Cxy1, ca::fvec Cyy1, ca::fvec x2,
-                                                                        ca::fvec y2, ca::fvec Cxx2, ca::fvec Cxy2,
-                                                                        ca::fvec Cyy2);
-
  private:
   // input data arrays
   TClonesArray* fInputMvdHits{nullptr};