Factors out 'kruskal' into it's own header and adds tests

gtsam/base/kruskal-inl.h

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SubgraphBuilder-inl.h
+ * @date Dec 31, 2009
+ * @date Jan 23, 2023
+ * @author Frank Dellaert, Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/types.h>
+#include <gtsam/base/DSFVector.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/VariableIndex.h>
+
+#include <memory>
+
+#include <vector>
+
+namespace gtsam::utils
+{
+
+    /*****************************************************************************/
+    /* sort the container and return permutation index with default comparator */
+    template <typename Container>
+    static std::vector<size_t> sort_idx(const Container &src)
+    {
+        typedef typename Container::value_type T;
+        const size_t n = src.size();
+        std::vector<std::pair<size_t, T>> tmp;
+        tmp.reserve(n);
+        for (size_t i = 0; i < n; i++)
+            tmp.emplace_back(i, src[i]);
+
+        /* sort */
+        std::stable_sort(tmp.begin(), tmp.end());
+
+        /* copy back */
+        std::vector<size_t> idx;
+        idx.reserve(n);
+        for (size_t i = 0; i < n; i++)
+        {
+            idx.push_back(tmp[i].first);
+        }
+        return idx;
+    }
+
+    /****************************************************************/
+    template <class Graph>
+    std::vector<size_t> kruskal(const Graph &fg,
+                                const FastMap<Key, size_t> &ordering,
+                                const std::vector<double> &weights)
+    {
+        const VariableIndex variableIndex(fg);
+        const size_t n = variableIndex.size();
+        const std::vector<size_t> sortedIndices = sort_idx(weights);
+
+        /* initialize buffer */
+        std::vector<size_t> treeIndices;
+        treeIndices.reserve(n - 1);
+
+        // container for acsendingly sorted edges
+        DSFVector dsf(n);
+
+        size_t count = 0;
+        for (const size_t index : sortedIndices)
+        {
+            const auto &f = *fg[index];
+            const auto keys = f.keys();
+            if (keys.size() != 2)
+                continue;
+            const size_t u = ordering.find(keys[0])->second,
+                         v = ordering.find(keys[1])->second;
+            if (dsf.find(u) != dsf.find(v))
+            {
+                dsf.merge(u, v);
+                treeIndices.push_back(index);
+                if (++count == n - 1)
+                    break;
+            }
+        }
+        return treeIndices;
+    }
+
+} // namespace gtsam::utils

gtsam/base/kruskal.h

@@ -0,0 +1,33 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SubgraphBuilder-inl.h
+ * @date Dec 31, 2009
+ * @date Jan 23, 2023
+ * @author Frank Dellaert, Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <gtsam/base/FastMap.h>
+
+#include <vector>
+
+namespace gtsam::utils
+{
+    template <class FactorGraph>
+    std::vector<size_t> kruskal(const FactorGraph &fg,
+                                const FastMap<Key, size_t> &ordering,
+                                const std::vector<double> &weights);
+}
+
+#include <gtsam/base/kruskal-inl.h>

gtsam/base/tests/testKruskal.cpp

@@ -0,0 +1,143 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testKruskal
+ * @brief Unit tests for Kruskal's minimum spanning tree algorithm
+ * @author Ankur Roy Chowdhury
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/kruskal.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/Ordering.h>
+
+#include <vector>
+#include <list>
+#include <memory>
+
+gtsam::GaussianFactorGraph makeTestGaussianFactorGraph()
+{
+    using namespace gtsam;
+    using namespace symbol_shorthand;
+
+    GaussianFactorGraph gfg;
+    Matrix I = I_2x2;
+    Vector2 b(0, 0);
+    const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
+    gfg += JacobianFactor(X(1), I, X(2), I, b, model);
+    gfg += JacobianFactor(X(1), I, X(3), I, b, model);
+    gfg += JacobianFactor(X(1), I, X(4), I, b, model);
+    gfg += JacobianFactor(X(2), I, X(3), I, b, model);
+    gfg += JacobianFactor(X(2), I, X(4), I, b, model);
+    gfg += JacobianFactor(X(3), I, X(4), I, b, model);
+
+    return gfg;
+}
+
+gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph()
+{
+    using namespace gtsam;
+    using namespace symbol_shorthand;
+
+    NonlinearFactorGraph nfg;
+    Matrix I = I_2x2;
+    Vector2 b(0, 0);
+    const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
+    nfg += BetweenFactor(X(1), X(2), Rot3(), model);
+    nfg += BetweenFactor(X(1), X(3), Rot3(), model);
+    nfg += BetweenFactor(X(1), X(4), Rot3(), model);
+    nfg += BetweenFactor(X(2), X(3), Rot3(), model);
+    nfg += BetweenFactor(X(2), X(4), Rot3(), model);
+    nfg += BetweenFactor(X(3), X(4), Rot3(), model);
+
+    return nfg;
+}
+
+/* ************************************************************************* */
+TEST(kruskal, GaussianFactorGraph)
+{
+    using namespace gtsam;
+
+    const auto g = makeTestGaussianFactorGraph();
+
+    const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
+    const auto weights = std::vector<double>(g.size(), 1.0);
+
+    const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights);
+
+    // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices)
+    // {
+    //     std::cout << "MST Edge indices are: \n";
+    //     for (const auto &edge : mst_edge_indices)
+    //     {
+    //         std::cout << edge << " : ";
+    //         for (const auto &key : graph[edge]->keys())
+    //         {
+    //             std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", ";
+    //         }
+    //         std::cout << "\n";
+    //     }
+    // };
+
+    // PrintMst(g, mstEdgeIndices);
+
+    EXPECT(mstEdgeIndices[0] == 0);
+    EXPECT(mstEdgeIndices[1] == 1);
+    EXPECT(mstEdgeIndices[2] == 2);
+}
+
+/* ************************************************************************* */
+TEST(kruskal, NonlinearFactorGraph)
+{
+    using namespace gtsam;
+
+    const auto g = makeTestNonlinearFactorGraph();
+
+    const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
+    const auto weights = std::vector<double>(g.size(), 1.0);
+
+    const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights);
+
+    // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices)
+    // {
+    //     std::cout << "MST Edge indices are: \n";
+    //     for (const auto &edge : mst_edge_indices)
+    //     {
+    //         std::cout << edge << " : ";
+    //         for (const auto &key : graph[edge]->keys())
+    //         {
+    //             std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", ";
+    //         }
+    //         std::cout << "\n";
+    //     }
+    // };
+
+    // PrintMst(g, mstEdgeIndices);
+
+    EXPECT(mstEdgeIndices[0] == 0);
+    EXPECT(mstEdgeIndices[1] == 1);
+    EXPECT(mstEdgeIndices[2] == 2);
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/inference/graph-inl.h

@@ -234,7 +234,7 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
   // Convert to a graph that boost understands
   SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
 
-  // find minimum spanning tree
+  // // find minimum spanning tree
   std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
   prim_minimum_spanning_tree(g, &p_map[0]);
 

gtsam/linear/SubgraphBuilder.cpp

@@ -23,6 +23,7 @@
 #include <gtsam/linear/Errors.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphBuilder.h>
+#include <gtsam/base/kruskal.h>
 
 #include <boost/algorithm/string.hpp>
 #include <boost/archive/text_iarchive.hpp>
@@ -52,28 +53,6 @@ using std::vector;
 
 namespace gtsam {
 
-/*****************************************************************************/
-/* sort the container and return permutation index with default comparator */
-template <typename Container>
-static vector<size_t> sort_idx(const Container &src) {
-  typedef typename Container::value_type T;
-  const size_t n = src.size();
-  vector<std::pair<size_t, T> > tmp;
-  tmp.reserve(n);
-  for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
-
-  /* sort */
-  std::stable_sort(tmp.begin(), tmp.end());
-
-  /* copy back */
-  vector<size_t> idx;
-  idx.reserve(n);
-  for (size_t i = 0; i < n; i++) {
-    idx.push_back(tmp[i].first);
-  }
-  return idx;
-}
-
 /****************************************************************************/
 Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
@@ -238,101 +217,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(
     return "UNKNOWN";
 }
 
-/****************************************************************/
-std::vector<double> utils::assignWeights(const GaussianFactorGraph &gfg, const SubgraphBuilderParameters::SkeletonWeight &skeletonWeight)
-{
-  using Weights = std::vector<double>;
-
-  const size_t m = gfg.size();
-  Weights weights;
-  weights.reserve(m);
-
-  for (const GaussianFactor::shared_ptr &gf : gfg)
-  {
-    switch (skeletonWeight)
-    {
-    case SubgraphBuilderParameters::EQUAL:
-      weights.push_back(1.0);
-      break;
-    case SubgraphBuilderParameters::RHS_2NORM:
-    {
-      if (JacobianFactor::shared_ptr jf =
-              std::dynamic_pointer_cast<JacobianFactor>(gf))
-      {
-        weights.push_back(jf->getb().norm());
-      }
-      else if (HessianFactor::shared_ptr hf =
-                   std::dynamic_pointer_cast<HessianFactor>(gf))
-      {
-        weights.push_back(hf->linearTerm().norm());
-      }
-    }
-    break;
-    case SubgraphBuilderParameters::LHS_FNORM:
-    {
-      if (JacobianFactor::shared_ptr jf =
-              std::dynamic_pointer_cast<JacobianFactor>(gf))
-      {
-        weights.push_back(std::sqrt(jf->getA().squaredNorm()));
-      }
-      else if (HessianFactor::shared_ptr hf =
-                   std::dynamic_pointer_cast<HessianFactor>(gf))
-      {
-        weights.push_back(std::sqrt(hf->information().squaredNorm()));
-      }
-    }
-    break;
-
-    case SubgraphBuilderParameters::RANDOM:
-      weights.push_back(std::rand() % 100 + 1.0);
-      break;
-
-    default:
-      throw std::invalid_argument(
-          "utils::assign_weights: undefined weight scheme ");
-      break;
-    }
-  }
-  return weights;
-}
-
-
-/****************************************************************/
-std::vector<size_t> utils::kruskal(const GaussianFactorGraph &gfg,
-                            const FastMap<Key, size_t> &ordering,
-                            const std::vector<double> &weights)
-{
-  const VariableIndex variableIndex(gfg);
-  const size_t n = variableIndex.size();
-  const vector<size_t> sortedIndices = sort_idx(weights);
-
-  /* initialize buffer */
-  vector<size_t> treeIndices;
-  treeIndices.reserve(n - 1);
-
-  // container for acsendingly sorted edges
-  DSFVector dsf(n);
-
-  size_t count = 0;
-  for (const size_t index : sortedIndices)
-  {
-    const GaussianFactor &gf = *gfg[index];
-    const auto keys = gf.keys();
-    if (keys.size() != 2)
-      continue;
-    const size_t u = ordering.find(keys[0])->second,
-                 v = ordering.find(keys[1])->second;
-    if (dsf.find(u) != dsf.find(v))
-    {
-      dsf.merge(u, v);
-      treeIndices.push_back(index);
-      if (++count == n - 1)
-        break;
-    }
-  }
-  return treeIndices;
-}
-
 /****************************************************************/
 vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
                                           const FastMap<Key, size_t> &ordering,
@@ -477,7 +361,59 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
 /****************************************************************/
 SubgraphBuilder::Weights SubgraphBuilder::weights(
     const GaussianFactorGraph &gfg) const {
-    return utils::assignWeights(gfg, parameters_.skeletonWeight);
+  using Weights = std::vector<double>;
+
+  const size_t m = gfg.size();
+  Weights weights;
+  weights.reserve(m);
+
+  for (const GaussianFactor::shared_ptr &gf : gfg)
+  {
+    switch (parameters_.skeletonWeight)
+    {
+    case SubgraphBuilderParameters::EQUAL:
+      weights.push_back(1.0);
+      break;
+    case SubgraphBuilderParameters::RHS_2NORM:
+    {
+      if (JacobianFactor::shared_ptr jf =
+              std::dynamic_pointer_cast<JacobianFactor>(gf))
+      {
+        weights.push_back(jf->getb().norm());
+      }
+      else if (HessianFactor::shared_ptr hf =
+                   std::dynamic_pointer_cast<HessianFactor>(gf))
+      {
+        weights.push_back(hf->linearTerm().norm());
+      }
+    }
+    break;
+    case SubgraphBuilderParameters::LHS_FNORM:
+    {
+      if (JacobianFactor::shared_ptr jf =
+              std::dynamic_pointer_cast<JacobianFactor>(gf))
+      {
+        weights.push_back(std::sqrt(jf->getA().squaredNorm()));
+      }
+      else if (HessianFactor::shared_ptr hf =
+                   std::dynamic_pointer_cast<HessianFactor>(gf))
+      {
+        weights.push_back(std::sqrt(hf->information().squaredNorm()));
+      }
+    }
+    break;
+
+    case SubgraphBuilderParameters::RANDOM:
+      weights.push_back(std::rand() % 100 + 1.0);
+      break;
+
+    default:
+      throw std::invalid_argument(
+          "utils::assign_weights: undefined weight scheme ");
+      break;
+    }
+  }
+  return weights;
 }
 
 /*****************************************************************************/

gtsam/linear/SubgraphBuilder.h

@@ -149,16 +149,6 @@ struct GTSAM_EXPORT SubgraphBuilderParameters {
   static std::string augmentationWeightTranslator(AugmentationWeight w);
 };
 
-namespace utils
-{
-
-  std::vector<double> assignWeights(const GaussianFactorGraph &gfg,
-                                    const SubgraphBuilderParameters::SkeletonWeight &skeleton_weight);
-  std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
-                              const FastMap<Key, size_t> &ordering,
-                              const std::vector<double> &weights);
-}
-
 /*****************************************************************************/
 class GTSAM_EXPORT SubgraphBuilder {
  public:
@@ -171,8 +161,6 @@ class GTSAM_EXPORT SubgraphBuilder {
   virtual ~SubgraphBuilder() {}
   virtual Subgraph operator()(const GaussianFactorGraph &jfg) const;
 
-public:
-
  private:
   std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
                                 const FastMap<Key, size_t> &ordering,

tests/testSubgraphSolver.cpp

@@ -129,55 +129,6 @@ TEST( SubgraphSolver, constructor3 )
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }
 
-/* ************************************************************************* */
-TEST(SubgraphBuilder, utilsAssignWeights)
-{
-  const auto [g, _] = example::planarGraph(N); // A*x-b
-  const auto weights = utils::assignWeights(g, gtsam::SubgraphBuilderParameters::SkeletonWeight::EQUAL);
-
-  EXPECT(weights.size() == g.size());
-  for (const auto &i : weights)
-  {
-    EXPECT_DOUBLES_EQUAL(weights[i], 1.0, 1e-12);
-  }
-}
-
-/* ************************************************************************* */
-TEST(SubgraphBuilder, utilsKruskal)
-{
-
-  const auto [g, _] = example::planarGraph(N); // A*x-b
-
-  const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
-  const auto weights = utils::assignWeights(g, gtsam::SubgraphBuilderParameters::SkeletonWeight::EQUAL);
-
-  const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights);
-
-  // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices)
-  // {
-  //   std::cout << "MST Edge indices are: \n";
-  //   for (const auto &edge : mst_edge_indices)
-  //   {
-  //     std::cout << edge << " : ";
-  //     for (const auto &key : graph[edge]->keys())
-  //     {
-  //       std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", ";
-  //     }
-  //     std::cout << "\n";
-  //   }
-  // };
-
-  // PrintMst(g, mstEdgeIndices);
-
-  EXPECT(mstEdgeIndices[0] == 1);
-  EXPECT(mstEdgeIndices[1] == 2);
-  EXPECT(mstEdgeIndices[2] == 3);
-  EXPECT(mstEdgeIndices[3] == 4);
-  EXPECT(mstEdgeIndices[4] == 5);
-  EXPECT(mstEdgeIndices[5] == 6);
-  EXPECT(mstEdgeIndices[6] == 7);
-  EXPECT(mstEdgeIndices[7] == 8);
-}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }