GSF: KL Divergence / component merge

Tracking/TrkFitter/TrkGaussianSumFilter/CMakeLists.txt

@@ -61,7 +61,8 @@ atlas_add_executable( GSF_testFindMinimumIndex
 atlas_add_executable( GSF_testAlignedDynArray
 	test/testAlignedDynArray.cxx)
 
-
+atlas_add_executable( GSF_testMergeComponents
+	test/testMergeComponents.cxx src/KLGaussianMixtureReduction.cxx)
 
 atlas_add_test(ut_GSF_testFindMinimumIndex
 	SCRIPT GSF_testFindMinimumIndex)

Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/KLGaussianMixtureReduction.h

@@ -14,7 +14,8 @@
 
 #include "CxxUtils/restrict.h"
 #include "CxxUtils/features.h"
-#include <utility>  
+#include <vector>
+#include <utility>
 #include <cstdint>
 
 namespace GSFUtils {
@@ -48,6 +49,7 @@ struct Component1D{
   double mean=0.;
   double cov=0.;
   double invCov=1e10; 
+  double weight=0.;
 };
 
 /**
@@ -96,54 +98,29 @@ typedef Component1D* ATH_RESTRICT componentPtrRestrict;
 #if HAVE_FUNCTION_MULTIVERSIONING
 #if defined(__x86_64__)
 __attribute__((target("avx2")))
-int32_t
+std::pair<int32_t,float> 
 findMinimumIndex(const floatPtrRestrict distancesIn, const int32_t n);
 __attribute__((target("sse4.2,sse2"))) 
-int32_t
+std::pair<int32_t,float> 
 findMinimumIndex(const floatPtrRestrict distancesIn, const int32_t n);
 #endif //x86_64 specific targets
 __attribute__((target("default")))
 #endif// function multiversioning
-int32_t
+std::pair<int32_t,float> 
 findMinimumIndex(const floatPtrRestrict distancesIn, const int32_t n);
 
-
 /**
- * Extra methods for finding the index of the minimum
- * pairwise distance
+ * Merge the componentsIn and return 
+ * which componets got merged
  */
-int32_t
-findMinimumIndexSTL(const floatPtrRestrict distancesIn, const int n);
-//find a pair of the  indices of the 2 smaller values
-std::pair<int32_t, int32_t>
-findMinimumIndexPair(const floatPtrRestrict distancesIn, const int32_t n);
+std::vector<std::pair<int32_t, int32_t>>
+findMerges(componentPtrRestrict componentsIn, 
+           const int32_t inputSize, 
+           const int32_t reducedSize);
 
-/**
- * Recalculate the distances given a merged input 
- * and return the index of the minimum pair
- */
-int32_t
-recalculateDistances(const componentPtrRestrict componentsIn,
-                     floatPtrRestrict distancesIn,
-                     const int32_t mini,
-                     const int32_t n);
-
-/** 
- * Calculate the distances for all component pairs
- */
-void
-calculateAllDistances(const componentPtrRestrict componentsIn,
-                      floatPtrRestrict distancesIn,
-                      const int32_t n);
+} // namespace KLGaussianMixtureReduction
 
-/*
- * Reset the distances wrt to a mini index
- */
-void
-resetDistances(floatPtrRestrict distancesIn,
-               const int32_t mini,
-               const int32_t n);
+      
 
-} // namespace KLGaussianMixtureReduction
 
 #endif

Tracking/TrkFitter/TrkGaussianSumFilter/share/ut_GSF_testFindMinimumIndex.ref

 FindMinimumIndex Index = 51 with value = 1.02537
-FindMinimumIndexSTL Index = 51 with value = 1.02537
-FindMinimumIndexPair 1st = 51 with value 1.02537 ,2nd = 15 with value 1.04305

Tracking/TrkFitter/TrkGaussianSumFilter/src/KLGaussianMixtureReduction.cxx

@@ -2,6 +2,7 @@
   Copyright (C) 2002-2020 CERN for the benefit of the ATLAS collaboration
 */
 #include "TrkGaussianSumFilter/KLGaussianMixtureReduction.h"
+#include "TrkGaussianSumFilter/AlignedDynArray.h"
 #include "CxxUtils/features.h"
 #include "CxxUtils/vectorize.h"
 #include <algorithm>
@@ -18,6 +19,184 @@
 #endif
 
 ATH_ENABLE_VECTORIZATION;
+namespace{
+/* 
+ * Component Merging helper methods 
+ */
+
+using namespace GSFUtils;
+
+/**
+ * Based on
+ * https://arxiv.org/pdf/2001.00727.pdf
+ * equation (10) 
+ * but not accounting for weights
+ */  
+[[maybe_unused]]
+float
+symmetricKL(const Component1D& componentI,
+            const Component1D& componentJ)
+{
+  const double meanDifference = componentI.mean - componentJ.mean;
+  const double covDifference = componentI.cov - componentJ.cov;
+  const double invertCovDiff = componentI.invCov - componentJ.invCov;
+  const double inverCovSum = componentI.invCov + componentJ.invCov;
+  return covDifference * invertCovDiff +
+         meanDifference * inverCovSum * meanDifference;
+}
+/**
+ * https://arxiv.org/pdf/2001.00727.pdf
+ * equation (10) 
+ */ 
+[[maybe_unused]]
+float
+weightedSymmetricKL(const Component1D& componentI,
+                    const Component1D& componentJ)
+{
+  const double meanDifference = componentI.mean - componentJ.mean;
+  const double covDifference = componentI.cov - componentJ.cov;
+  const double invertCovDiff = componentI.invCov - componentJ.invCov;
+  const double inverCovSum = componentI.invCov + componentJ.invCov;
+  const double weightMul = componentI.weight * componentJ.weight;
+  const double symmetricDis = covDifference * invertCovDiff +
+                              meanDifference * inverCovSum * meanDifference;
+  return weightMul * symmetricDis;
+}
+
+/*
+ * Moment-preserving merge of two 1D components
+ * for example see
+ * Runnalls, Andrew R.(2007) 
+ * Kullback-Leibler approach to Gaussian mixture reduction
+ * equations (2),(3),(4)
+ */
+void
+combine(GSFUtils::Component1D& updated, 
+        GSFUtils::Component1D& removed)
+{
+
+  const double sumWeight = updated.weight + removed.weight;
+
+  const double invSumWeight = 1. / sumWeight;
+  const double weightI_IJ = updated.weight * invSumWeight;
+  const double weightJ_IJ = removed.weight * invSumWeight;
+  const double meanDiff = (updated.mean - removed.mean);
+
+  const double sumMean = weightI_IJ * updated.mean + weightJ_IJ * removed.mean;
+
+  const double sumVariance = weightI_IJ * updated.cov +
+                             weightJ_IJ * removed.cov +
+                             weightI_IJ * weightJ_IJ * meanDiff * meanDiff;
+
+  updated.mean = sumMean;
+  updated.cov = sumVariance;
+  updated.invCov = 1. / sumVariance;
+  updated.weight = sumWeight;
+
+  removed.mean = 0.;
+  removed.cov = 0.;
+  removed.invCov = 1e10;
+  removed.weight = 0.;
+}
+
+/**
+ * Recalculate the distances given a merged input 
+ * and return the index of the minimum pair
+ */
+std::pair<int32_t, float>
+recalculateDistances(const componentPtrRestrict componentsIn,
+                     floatPtrRestrict distancesIn,
+                     const int32_t mini,
+                     const int32_t n)
+{
+  const Component1D* components =
+    static_cast<const Component1D*>(__builtin_assume_aligned(componentsIn, alignment));
+  float* distances = static_cast<float*>(__builtin_assume_aligned(distancesIn, alignment));
+
+  const int32_t j = mini;
+  const int32_t indexConst = (j + 1) * j / 2;
+
+  int32_t minIndex = 0;
+  float minDistance = std::numeric_limits<float>::max();
+
+  const Component1D componentJ = components[j];
+
+  for (int32_t i = 0; i < j; ++i) {
+    const Component1D componentI = components[i];
+    const int32_t index = indexConst + i;
+    if (componentI.cov == 0) {
+      distances[index] = std::numeric_limits<float>::max();
+      continue;
+    }
+    distances[index] = symmetricKL(componentI,componentJ);
+    if (distances[index] < minDistance) {
+      minIndex = index;
+      minDistance = distances[index];
+    }
+  }
+  for (int32_t i = j + 1; i < n; ++i) {
+    const int32_t index = (i + 1) * i / 2 + j;
+    const Component1D componentI = components[i];
+    if (componentI.cov == 0) {
+      distances[index] = std::numeric_limits<float>::max();
+      continue;
+    }
+    distances[index] = symmetricKL(componentI,componentJ);
+    if (distances[index] < minDistance) {
+      minIndex = index;
+      minDistance = distances[index];
+    }
+  }
+  return {minIndex,minDistance};
+}
+
+/** 
+ * Calculate the distances for all component pairs
+ */
+void
+calculateAllDistances(const componentPtrRestrict componentsIn,
+                      floatPtrRestrict distancesIn,
+                      const int32_t n)
+{
+
+  const Component1D* components =
+  static_cast<const Component1D*>(__builtin_assume_aligned(componentsIn, alignment));
+  float* distances = static_cast<float*>(__builtin_assume_aligned(distancesIn, alignment));
+
+  for (int32_t i = 0; i < n; ++i) {
+    const int32_t indexConst = (i + 1) * i / 2;
+    const Component1D componentI = components[i];
+    for (int32_t j = 0; j < i; ++j) {
+      const Component1D componentJ = components[j];
+      distances[indexConst + j] = symmetricKL(componentI,componentJ);
+    }
+  }
+}
+
+/*
+ * Reset the distances wrt to a mini index
+ */
+void
+resetDistances(floatPtrRestrict distancesIn,
+               const int32_t mini,
+               const int32_t n)
+{
+
+  float* distances = (float*)__builtin_assume_aligned(distancesIn, alignment);
+  const int32_t j = mini;
+  const int32_t indexConst = (j + 1) * j / 2;
+  for (int32_t i = 0; i < j; ++i) {
+    distances[indexConst + i] = std::numeric_limits<float>::max();
+  }
+
+  for (int32_t i = j; i < n; ++i) {
+    const int32_t index = (i + 1) * i / 2 + j;
+    distances[index] = std::numeric_limits<float>::max();
+  }
+}
+
+}
+
 
 namespace GSFUtils {
 /*
@@ -74,7 +253,7 @@ namespace GSFUtils {
  * in dst.
  */
 __attribute__((target("avx2"))) 
-int32_t
+std::pair<int32_t,float>
 findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
 {
   float* array = (float*)__builtin_assume_aligned(distancesIn, alignment);
@@ -109,7 +288,7 @@ findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
       minDistance = distances[i];
     }
   }
-  return minIndex;
+  return {minIndex,minDistance};
 }
 /*
  * SSE
@@ -159,7 +338,7 @@ static const auto mm_blendv_epi8 = SSE2_mm_blendv_epi8;
  * latency.
  */
 __attribute__((target("sse4.2,sse2"))) 
-int32_t
+std::pair<int32_t,float>
 findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
 {
   float* array = (float*)__builtin_assume_aligned(distancesIn, alignment);
@@ -206,7 +385,7 @@ findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
       minDistance = distances[i];
     }
   }
-  return minIndex;
+  return {minIndex,minDistance};
 }
 #endif // end of x86_64 versions
 
@@ -214,162 +393,96 @@ findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
 __attribute__((target("default")))
 #endif // HAVE_FUNCTION_MULTIVERSIONING
 
-int32_t
+std::pair<int32_t,float>
 findMinimumIndex(const floatPtrRestrict distancesIn, const int n)
 {
   float* array = (float*)__builtin_assume_aligned(distancesIn, alignment);
-  float minvalue = array[0];
+  float minDistance = array[0];
   size_t minIndex = 0;
   for (int i = 0; i < n; ++i) {
     const float value = array[i];
-    if (value < minvalue) {
+    if (value < minDistance) {
       minIndex = i;
-      minvalue = value;
+      minDistance = value;
     }
   }
-  return minIndex;
+  return {minIndex,minDistance}; 
 }
 
 /*
- * Find Minimum Index STL style.
- * This works quite well in Clang8 but not in gcc8
- * https://its.cern.ch/jira/projects/ATLASRECTS/issues/ATLASRECTS-5244
+ * Merge the componentsIn and return 
+ * which componets got merged
  */
-int32_t
-findMinimumIndexSTL(const floatPtrRestrict distancesIn, const int n)
+std::vector<std::pair<int32_t, int32_t>>
+findMerges(componentPtrRestrict componentsIn,
+           const int32_t inputSize,
+           const int32_t reducedSize)
 {
-  float* array = (float*)__builtin_assume_aligned(distancesIn, alignment);
-  return std::distance(array, std::min_element(array, array + n));
-}
 
-/*
- * Find the index of the 2 smaller values
- */
-std::pair<int32_t, int32_t>
-findMinimumIndexPair(const floatPtrRestrict distancesIn, const int n)
-{
+  Component1D* components =
+    static_cast<Component1D*>(__builtin_assume_aligned(componentsIn, alignment));
+  //Based on the inputSize allocate enough space for the pairwise distances
+  const int32_t n = inputSize;
+  const int32_t nn = (n + 1) * n/2;
+  const int32_t nn2 =
+    (nn & 7) == 0 ? nn
+                  : nn + (8 - (nn & 7)); // make sure it is a multiplet of 8
 
-  float* distances = (float*)__builtin_assume_aligned(distancesIn, alignment);
-  int32_t mini = 0;
-  int32_t mini2 = -1;
-  float minDistance = distances[0];
-  float minDistance2 = std::numeric_limits<float>::max();
+  AlignedDynArray<float, alignment> distances(nn2, std::numeric_limits<float>::max());
 
-  for (int i = 1; i < n; ++i) {
-    if (distances[i] < minDistance) {
-      mini2 = mini;
-      minDistance2 = minDistance;
-      mini = i;
-      minDistance = distances[i];
-    } else if (distances[i] < minDistance2) {
-      mini2 = i;
-      minDistance2 = distances[i];
+  // Create a trianular mapping for the pairwise distances
+  std::vector<triangularToIJ> convert(nn2, { -1, -1 });
+  for (int32_t i = 0; i < n; ++i) {
+    const int indexConst = (i + 1) * i / 2;
+    for (int32_t j = 0; j <= i; ++j) {
+      int32_t index = indexConst + j;
+      convert[index].I = i;
+      convert[index].J = j;
     }
   }
 
-  return std::make_pair(mini, mini2);
-}
-int32_t
-recalculateDistances(const componentPtrRestrict componentsIn,
-                     floatPtrRestrict distancesIn,
-                     const int32_t mini,
-                     const int32_t n)
-{
-  const Component1D* components =
-    static_cast<const Component1D*>(__builtin_assume_aligned(componentsIn, alignment));
-  float* distances = static_cast<float*>(__builtin_assume_aligned(distancesIn, alignment));
-
-  const int32_t j = mini;
-  const int32_t indexConst = (j + 1) * j / 2;
-
-  int32_t minIndex = 0;
-  float minDistance = std::numeric_limits<float>::max();
+  // vector to be returned
+  std::vector<std::pair<int32_t, int32_t>> merges;
+  merges.reserve(inputSize - reducedSize);
 
-  const Component1D componentJ = components[j];
+  // initial distance calculation
+  calculateAllDistances(components, distances, n);
 
-  for (int32_t i = 0; i < j; ++i) {
-    const Component1D componentI = components[i];
-    const int32_t index = indexConst + i;
-    if (componentI.cov == 0) {
-      distances[index] = std::numeric_limits<float>::max();
-      continue;
+  // merge loop
+  int32_t numberOfComponentsLeft = n;
+  int32_t minIndex = -1;
+  float currentMinValue = std::numeric_limits<float>::max();
+  bool foundNext =false;
+  while (numberOfComponentsLeft > reducedSize) {
+    // see if we have the next already
+    if (!foundNext) {
+      std::pair<int32_t,float> minDis = findMinimumIndex(distances, nn2);
+      minIndex = minDis.first;
+      currentMinValue = minDis.second;
     }
-    const double meanDifference = componentI.mean - componentJ.mean;
-    const double covarianceDifference = componentI.cov - componentJ.cov;
-    const double invertCovDiff = componentI.invCov - componentJ.invCov;
-    const double inverCovSum = componentI.invCov + componentJ.invCov;
-    distances[index] = covarianceDifference * invertCovDiff +
-                       meanDifference * inverCovSum * meanDifference;
-    if (distances[index] < minDistance) {
-      minIndex = index;
-      minDistance = distances[index];
+    //always reset 
+    foundNext=false;
+    const int32_t mini = convert[minIndex].I;
+    const int32_t minj = convert[minIndex].J;
+    // Combine the 2 components
+    combine(components[mini], components[minj]);
+    // re-calculate distances wrt the new component at mini
+    // re-calculate distances wrt the new component at mini
+    std::pair<int32_t, float>  possibleNextMin =
+      recalculateDistances(components, distances, mini, n);
+    //We might already got something smaller than the previous minimum
+    if (possibleNextMin.first > 0 && possibleNextMin.second < currentMinValue) {
+      foundNext=true;
+      minIndex= possibleNextMin.first;
+      currentMinValue=possibleNextMin.second;
     }
-  }
-  for (int32_t i = j + 1; i < n; ++i) {
-    const int32_t index = (i + 1) * i / 2 + j;
-    const Component1D componentI = components[i];
-    if (componentI.cov == 0) {
-      distances[index] = std::numeric_limits<float>::max();
-      continue;
-    }
-    const double meanDifference = componentI.mean - componentJ.mean;
-    const double covarianceDifference = componentI.cov - componentJ.cov;
-    const double invertCovDiff = componentI.invCov - componentJ.invCov;
-    const double inverCovSum = componentI.invCov + componentJ.invCov;
-    distances[index] = covarianceDifference * invertCovDiff +
-                       meanDifference * inverCovSum * meanDifference;
-    if (distances[index] < minDistance) {
-      minIndex = index;
-      minDistance = distances[index];
-    }
-  }
-  return minIndex;
-}
-
-// Calculate the distances for all pairs
-void
-calculateAllDistances(const componentPtrRestrict componentsIn,
-                      floatPtrRestrict distancesIn,
-                      const int32_t n)
-{
-
-  const Component1D* components =
-  static_cast<const Component1D*>(__builtin_assume_aligned(componentsIn, alignment));
-  float* distances = static_cast<float*>(__builtin_assume_aligned(distancesIn, alignment));
-
-  for (int32_t i = 0; i < n; ++i) {
-    const int32_t indexConst = (i + 1) * i / 2;
-    const Component1D componentI = components[i];
-    for (int32_t j = 0; j < i; ++j) {
-      const Component1D componentJ = components[j];
-      const double  meanDifference = componentI.mean - componentJ.mean;
-      const double covarianceDifference = componentI.cov - componentJ.cov;
-      const double invertCovDiff = componentI.invCov - componentJ.invCov;
-      const double inverCovSum = componentI.invCov + componentJ.invCov;
-      distances[indexConst + j] = covarianceDifference * invertCovDiff +
-                                  meanDifference * inverCovSum * meanDifference;
-    }
-  }
-}
-
-// Reset the distances for a row
-void
-resetDistances(floatPtrRestrict distancesIn,
-               const int32_t mini,
-               const int32_t n)
-{
-
-  float* distances = (float*)__builtin_assume_aligned(distancesIn, alignment);
-  const int32_t j = mini;
-  const int32_t indexConst = (j + 1) * j / 2;
-  for (int32_t i = 0; i < j; ++i) {
-    distances[indexConst + i] = std::numeric_limits<float>::max();
-  }
-
-  for (int32_t i = j; i < n; ++i) {
-    const int32_t index = (i + 1) * i / 2 + j;
-    distances[index] = std::numeric_limits<float>::max();
-  }
+    // Reset old weights wrt the  minj position
+    resetDistances(distances, minj, n);
+    // keep track and decrement
+    merges.emplace_back(mini,minj);
+    --numberOfComponentsLeft;
+  } // end of merge while
+ return merges;
 }
 
 } // end namespace GSFUtils

Tracking/TrkFitter/TrkGaussianSumFilter/src/QuickCloseComponentsMultiStateMerger.cxx

@@ -90,118 +90,41 @@ Trk::QuickCloseComponentsMultiStateMerger::merge(Trk::MultiComponentState states
 }
 
 Trk::MultiComponentState
-Trk::QuickCloseComponentsMultiStateMerger::mergeFullDistArray(MultiComponentStateAssembler::Cache& cache,
-                                                              Trk::MultiComponentState& statesToMerge) const
+Trk::QuickCloseComponentsMultiStateMerger::mergeFullDistArray(
+  MultiComponentStateAssembler::Cache& cache,
+  Trk::MultiComponentState& statesToMerge) const
 {
-  /*
-   * Allocate, and initialize the needed arrays
-   */
   const int32_t n = statesToMerge.size();
-  const int32_t nn = (n + 1) * n / 2;
-  const int32_t nn2 = (nn & 7) == 0 ? nn : nn + (8 - (nn & 7)); // make sure it is a multiplet of 8
-  
-  // Array to store all of the distances between components
-  AlignedDynArray<float, alignment> distances(nn2,
-                                              std::numeric_limits<float>::max()); 
-  AlignedDynArray<Component1D, alignment> components(n);    // Arrayfor each component
-  // Needed to convert the triangular index to (i,j)
-  std::vector<triangularToIJ> convert(nn2, { -1, -1 });
-  // Calculate indicies
-  for (int32_t i = 0; i < n; ++i) {
-    const int indexConst = (i + 1) * i / 2;
-    for (int32_t j = 0; j <= i; ++j) {
-      int32_t index = indexConst + j;
-      convert[index].I = i;
-      convert[index].J = j;
-    }
-  }
-
-  // Create an array of all components to be merged
+  AlignedDynArray<Component1D, alignment> components(n);
   for (int32_t i = 0; i < n; ++i) {
     const AmgSymMatrix(5)* measuredCov = statesToMerge[i].first->covariance();
     const AmgVector(5)& parameters = statesToMerge[i].first->parameters();
     // Fill in infomation
-    const double cov= measuredCov ? (*measuredCov)(Trk::qOverP, Trk::qOverP) : -1.;
-    components[i].mean= parameters[Trk::qOverP];
+    const double cov = measuredCov ? (*measuredCov)(Trk::qOverP, Trk::qOverP) : -1.;
+    components[i].mean = parameters[Trk::qOverP];
     components[i].cov = cov;
-    components[i].invCov = cov > 0 ? 1./cov : 1e10;
+    components[i].invCov = cov > 0 ? 1. / cov : 1e10;
+    components[i].weight = statesToMerge[i].second;
   }
-  // Calculate distances for all pairs
-  // This loop can be vectorised
-  calculateAllDistances(components, distances, n);
-
-  /*
-   *  Loop over all components until you reach the target amount
-   */
-  unsigned int numberOfComponents = n;
-  int32_t minIndex = -1;
-  int32_t nextMinIndex = -1;
-
-  while (numberOfComponents > m_maximumNumberOfComponents) {
-
-    /* 
-     * Find the minimum index 
-     * Do it only if we do not have a good new guess
-     */
-    if(nextMinIndex<1){
-      minIndex = findMinimumIndex(distances, nn2);
-    } else {
-      minIndex = nextMinIndex;
-    }
-    /* reset the nextMinindex*/
-    nextMinIndex=-1;
-    /*Keep track of the current minimum value*/
-    float currentMinValue=distances[minIndex];
-
-    /*convert the index in an (i,j) pair*/
-    int32_t mini = convert[minIndex].I;
-    int32_t minj = convert[minIndex].J;
-    /*
-     * Combine the components to be merged
-     * statesToMerge[mini] becomes the merged
-     * statesToMerge[minj] is set to dummy values
-     */
+
+  // Gather the merges
+  const std::vector<std::pair<int32_t, int32_t>> merges =
+    findMerges(components, n, m_maximumNumberOfComponents);
+
+  // Do the full 5D calculations of the merge
+  for (const auto& mergePair : merges) {
+    const int32_t mini = mergePair.first;
+    const int32_t minj = mergePair.second;
     MultiComponentStateCombiner::combineWithWeight(statesToMerge[mini], statesToMerge[minj]);
     statesToMerge[minj].first.reset();
     statesToMerge[minj].second = 0.;
-    /*
-     * set relevant distances
-     */
-    const AmgSymMatrix(5)* measuredCov = statesToMerge[mini].first->covariance();
-    const AmgVector(5)& parameters = statesToMerge[mini].first->parameters();
-    const double cov = (*measuredCov)(Trk::qOverP, Trk::qOverP);
-    
-    components[mini].mean= parameters[Trk::qOverP];
-    components[mini].cov = cov;
-    components[mini].invCov = cov > 0 ? 1./cov : 1e10;
-    
-    components[minj].mean = 0.;
-    components[minj].cov = 0.;
-    components[minj].invCov = 1e10;
-    
-    // re-calculate distances wrt the new component at mini
-    int32_t possibleNextMin = recalculateDistances(components, distances, mini, n);
-    //We might already got something smaller than the previous minimum
-    //we can therefore use the new one directly
-    if (possibleNextMin > 0 && distances[possibleNextMin] < currentMinValue) {
-      nextMinIndex = possibleNextMin;
-    }
-    // Reset old weights wrt the  minj position
-    resetDistances(distances, minj, n);
-    // Decrement the number of components
-    --numberOfComponents;
-  } // end of merge while
-
-  // Build final state containing both merged and unmerged components
+  }
+  // Assemble the final result
   for (auto& state : statesToMerge) {
     // Avoid merge ones
     if (!state.first) {
       continue;
     }
-    /*
-     * Add componets to the state be prepared for assembly
-     * and update the relevant weight
-     */
     cache.multiComponentState.push_back(ComponentParameters(state.first.release(), state.second));
     cache.validWeightSum += state.second;
   }

Tracking/TrkFitter/TrkGaussianSumFilter/test/testFindMinimumIndex.cxx

@@ -14,13 +14,8 @@ int main(){
     6.97245, 2.13307, 2.89188, 1.46095, 1.32797, 4.67858, 5.1219,  5.38812, 8.14577, 9.28787, 8.26778, 7.35197, 1.02537,
     7.39633, 6.79565, 5.1043,  7.96525, 6.16379, 8.89082, 8.27358, 1.15997, 8.39121, 8.38757, 9.46067, 4.714};
 
-    int32_t minIndex=GSFUtils::findMinimumIndex(array,64);
-    std::cout << "FindMinimumIndex Index = " <<  minIndex <<  " with value = " <<array[minIndex] <<'\n'; 
-    minIndex=GSFUtils::findMinimumIndexSTL(array,64);
-    std::cout << "FindMinimumIndexSTL Index = " <<  minIndex << " with value = " <<array[minIndex] <<'\n'; 
-    std::pair<int32_t,int32_t> minpair = GSFUtils::findMinimumIndexPair(array,64);
-    std::cout << "FindMinimumIndexPair 1st = " << minpair.first << " with value " << array[minpair.first]
-              << " ,2nd = " << minpair.second << " with value " << array[minpair.second] << '\n';
+  std::pair<int32_t,float> minIndex=GSFUtils::findMinimumIndex(array,64);
+  std::cout << "FindMinimumIndex Index = " <<  minIndex.first <<  " with value = " <<  minIndex.second <<'\n'; 
 
-    return 0;
+  return 0;
 }

Tracking/TrkFitter/TrkGaussianSumFilter/test/testMergeComponents.cxx

+#include <array>
+/*
+  Copyright (C) 2002-2020 CERN for the benefit of the ATLAS collaboration
+*/
+
+#include "TrkGaussianSumFilter/AlignedDynArray.h"
+#include "TrkGaussianSumFilter/KLGaussianMixtureReduction.h"
+#include <iostream>
+
+using namespace GSFUtils;
+namespace {
+constexpr int32_t n =72;
+constexpr std::array<Component1D,n> input = {
+  { { -4.66462e-06, 1.06618e-11, 9.37928e+10, 0.00608503 },
+    { -2.08263e-05, 7.533e-11, 1.32749e+10, 0.0274963 },
+    { -4.15487e-05, 4.79975e-11, 2.08344e+10, 0.0591673 },
+    { -4.86464e-05, 7.57086e-12, 1.32085e+11, 0.0648575 },
+    { -5.1025e-05, 3.16755e-12, 3.15701e+11, 0.0952169 },
+    { -5.15536e-05, 2.9173e-12, 3.42782e+11, 0.536818 },
+    { -4.71833e-06, 1.1939e-11, 8.3759e+10, 0.000587903 },
+    { -2.10663e-05, 7.81055e-11, 1.28032e+10, 0.0026566 },
+    { -4.20273e-05, 5.01395e-11, 1.99443e+10, 0.00571686 },
+    { -4.92063e-05, 8.77638e-12, 1.13942e+11, 0.00626678 },
+    { -5.16121e-05, 4.27124e-12, 2.34124e+11, 0.00920053 },
+    { -5.21468e-05, 4.01522e-12, 2.49052e+11, 0.0518911 },
+    { -4.58728e-06, 1.12379e-11, 8.89846e+10, 0.000489032 },
+    { -2.0481e-05, 7.37793e-11, 1.35539e+10, 0.00220977 },
+    { -4.08598e-05, 4.73457e-11, 2.11212e+10, 0.00475507 },
+    { -4.78398e-05, 8.24862e-12, 1.21232e+11, 0.00521237 },
+    { -5.01789e-05, 3.99012e-12, 2.50619e+11, 0.00765225 },
+    { -5.06988e-05, 3.74811e-12, 2.66801e+11, 0.0431424 },
+    { -4.78054e-06, 1.18292e-11, 8.45367e+10, 0.000296965 },
+    { -2.1344e-05, 7.97519e-11, 1.25389e+10, 0.00134192 },
+    { -4.25814e-05, 5.10436e-11, 1.95911e+10, 0.00288774 },
+    { -4.9855e-05, 8.5826e-12, 1.16515e+11, 0.00316553 },
+    { -5.22926e-05, 3.9579e-12, 2.52659e+11, 0.00464746 },
+    { -5.28342e-05, 3.69508e-12, 2.7063e+11, 0.0262122 },
+    { -4.46758e-06, 1.74302e-11, 5.73717e+10, 0.000201451 },
+    { -1.99466e-05, 7.67504e-11, 1.30293e+10, 0.000910294 },
+    { -3.97936e-05, 5.16782e-11, 1.93505e+10, 0.00195881 },
+    { -4.65914e-05, 1.45949e-11, 6.85173e+10, 0.0021472 },
+    { -4.88695e-05, 1.05557e-11, 9.47354e+10, 0.0031523 },
+    { -4.93758e-05, 1.03262e-11, 9.68413e+10, 0.017773 },
+    { -3.9244e-06, 1.94312e-11, 5.14636e+10, 2.28683e-05 },
+    { -1.75215e-05, 6.52038e-11, 1.53365e+10, 0.000103336 },
+    { -3.49554e-05, 4.58576e-11, 2.18066e+10, 0.000222367 },
+    { -4.09266e-05, 1.72434e-11, 5.79933e+10, 0.000243755 },
+    { -4.29277e-05, 1.41268e-11, 7.07877e+10, 0.000357862 },
+    { -4.33724e-05, 1.39496e-11, 7.16864e+10, 0.00201799 },
+    { -4.38853e-06, 1.66176e-11, 6.01772e+10, 1.22764e-05 },
+    { -1.95937e-05, 7.38574e-11, 1.35396e+10, 5.54734e-05 },
+    { -3.90896e-05, 4.96645e-11, 2.01351e+10, 0.000119371 },
+    { -4.57671e-05, 1.38817e-11, 7.20373e+10, 0.000130852 },
+    { -4.80049e-05, 9.98424e-12, 1.00158e+11, 0.000192104 },
+    { -4.85022e-05, 9.76274e-12, 1.0243e+11, 0.00108316 },
+    { -3.45701e-06, 5.26509e-11, 1.8993e+10, 8.66949e-06 },
+    { -1.54349e-05, 8.81702e-11, 1.13417e+10, 3.91764e-05 },
+    { -3.07925e-05, 7.31575e-11, 1.36691e+10, 8.43101e-05 },
+    { -3.60521e-05, 5.09531e-11, 1.96259e+10, 9.24221e-05 },
+    { -3.78147e-05, 4.85347e-11, 2.06038e+10, 0.000135694 },
+    { -3.82063e-05, 4.83973e-11, 2.06623e+10, 0.000765623 },
+    { -4.94321e-06, 1.29867e-11, 7.70021e+10, 7.54445e-07 },
+    { -2.20705e-05, 8.56111e-11, 1.16807e+10, 3.40926e-06 },
+    { -4.40305e-05, 5.49154e-11, 1.82098e+10, 7.33698e-06 },
+    { -5.15512e-05, 9.51523e-12, 1.05095e+11, 8.04293e-06 },
+    { -5.40715e-05, 4.57064e-12, 2.18788e+11, 1.18086e-05 },
+    { -5.46316e-05, 4.28965e-12, 2.33119e+11, 6.66306e-05 },
+    { -2.67066e-06, 8.51914e-12, 1.17383e+11, 4.90461e-07 },
+    { -1.1924e-05, 2.97175e-11, 3.36503e+10, 2.21633e-06 },
+    { -2.37883e-05, 2.07577e-11, 4.81749e+10, 4.76964e-06 },
+    { -2.78515e-05, 7.50587e-12, 1.33229e+11, 5.22854e-06 },
+    { -2.92132e-05, 6.06258e-12, 1.64946e+11, 7.67648e-06 },
+    { -2.95158e-05, 5.98056e-12, 1.67208e+11, 4.33103e-05 },
+    { -4.75742e-06, 1.31378e-11, 7.61161e+10, 2.66875e-07 },
+    { -2.1241e-05, 8.04056e-11, 1.24369e+10, 1.20598e-06 },
+    { -4.23756e-05, 5.1974e-11, 1.92404e+10, 2.59534e-06 },
+    { -4.96137e-05, 9.92245e-12, 1.00782e+11, 2.84505e-06 },
+    { -5.20393e-05, 5.34254e-12, 1.87177e+11, 4.17709e-06 },
+    { -5.25783e-05, 5.08227e-12, 1.96762e+11, 2.35683e-05 },
+    { -4.13925e-06, 1.32939e-11, 7.52224e+10, 7.37356e-10 },
+    { -1.84807e-05, 6.42154e-11, 1.55726e+10, 3.33188e-09 },
+    { -3.68691e-05, 4.2693e-11, 2.3423e+10, 7.16968e-09 },
+    { -4.31674e-05, 1.086e-11, 9.20809e+10, 7.85921e-09 },
+    { -4.5278e-05, 7.39274e-12, 1.35268e+11, 1.15381e-08 },
+    { -4.57471e-05, 7.19569e-12, 1.38972e+11, 6.50527e-08 } }
+};
+}
+
+int
+main()
+{
+  AlignedDynArray<Component1D, alignment> components(n);
+  // Create an array of all components to be merged
+  for (int32_t i = 0; i < n; ++i) {
+    components[i].mean = input[i].mean;
+    components[i].cov = input[i].cov;
+    components[i].invCov = input[i].invCov;
+    components[i].weight = input[i].weight;
+  }
+  std::vector<std::pair<int32_t, int32_t>> mergeOrder=findMerges(components,n,12);
+  for (const auto& i : mergeOrder){  
+    std::cout << "[" << i.first << ", " << i.second << "]" << '\n';
+  }
+  return 0;
+}
+