WIP: replace instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version

gtsam/base/kruskal-inl.h

@@ -20,7 +20,7 @@
 
 #include <gtsam/base/FastMap.h>
 #include <gtsam/base/types.h>
-#include <gtsam/base/DSFVector.h>
+#include <gtsam/base/DSFMap.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/VariableIndex.h>
@@ -34,12 +34,10 @@ namespace gtsam::utils
 
     /*****************************************************************************/
     /* sort the container and return permutation index with default comparator */
-    template <typename Container>
+    inline std::vector<size_t> sortedIndices(const std::vector<double> &src)
-    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;
+        std::vector<std::pair<size_t, double>> tmp;
         tmp.reserve(n);
         for (size_t i = 0; i < n; i++)
             tmp.emplace_back(i, src[i]);
@@ -63,27 +61,33 @@ namespace gtsam::utils
                                 const FastMap<Key, size_t> &ordering,
                                 const std::vector<double> &weights)
     {
+        // Create an index from variables to factor indices.
         const VariableIndex variableIndex(fg);
-        const size_t n = variableIndex.size();
-        const std::vector<size_t> sortedIndices = sort_idx(weights);
 
-        /* initialize buffer */
+        // Get indices in sort-order of the weights
+        const std::vector<size_t> sortedIndices = gtsam::utils::sortedIndices(weights);
+
+        // Create a vector to hold MST indices.
+        const size_t n = variableIndex.size();
         std::vector<size_t> treeIndices;
         treeIndices.reserve(n - 1);
 
-        // container for acsendingly sorted edges
+        // Initialize disjoint-set forest to keep track of merged 'blah'.
-        DSFVector dsf(n);
+        DSFMap<Key> dsf;
 
+        // Loop over all edges in order of increasing weight.
         size_t count = 0;
         for (const size_t index : sortedIndices)
         {
-            const auto &f = *fg[index];
+            const auto factor = fg[index];
-            const auto keys = f.keys();
+
-            if (keys.size() != 2)
+            // Ignore non-binary edges.
+            if (factor->size() != 2)
                 continue;
-            const size_t u = ordering.find(keys[0])->second,
+
-                         v = ordering.find(keys[1])->second;
+            auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
-            if (dsf.find(u) != dsf.find(v))
+            auto loop = (u == v);
+            if (!loop)
             {
                 dsf.merge(u, v);
                 treeIndices.push_back(index);

gtsam/slam/lago.cpp

@@ -24,14 +24,20 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/base/kruskal.h>
 
 #include <boost/math/special_functions.hpp>
 
+#include <iostream>
+#include <stack>
+
 using namespace std;
 
 namespace gtsam {
 namespace lago {
 
+using initialize::kAnchorKey;
+
 static const Matrix I = I_1x1;
 static const Matrix I3 = I_3x3;
 
@@ -79,16 +85,15 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
 
   key2doubleMap thetaToRootMap;
 
-  // Orientation of the roo
+  // Orientation of the root
-  thetaToRootMap.insert(pair<Key, double>(initialize::kAnchorKey, 0.0));
+  thetaToRootMap.emplace(kAnchorKey, 0.0);
 
   // for all nodes in the tree
-  for(const key2doubleMap::value_type& it: deltaThetaMap) {
+  for(const auto& [nodeKey, _]: deltaThetaMap) {
     // compute the orientation wrt root
-    Key nodeKey = it.first;
+    const double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
-    double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
         thetaToRootMap);
-    thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
+    thetaToRootMap.emplace(nodeKey, nodeTheta);
   }
   return thetaToRootMap;
 }
@@ -174,7 +179,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
   }
-  // put regularized measurements in the chordsIds
+  // put regularized measurements in the chords
   for(const size_t& factorId: chordsIds) {
     const KeyVector& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
@@ -190,7 +195,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
     lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
   }
   // prior on the anchor orientation
-  lagoGraph.add(initialize::kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
+  lagoGraph.add(kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
   return lagoGraph;
 }
 
@@ -199,7 +204,7 @@ static PredecessorMap<Key> findOdometricPath(
     const NonlinearFactorGraph& pose2Graph) {
 
   PredecessorMap<Key> tree;
-  Key minKey = initialize::kAnchorKey; // this initialization does not matter
+  Key minKey = kAnchorKey; // this initialization does not matter
   bool minUnassigned = true;
 
   for(const std::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
@@ -216,11 +221,51 @@ static PredecessorMap<Key> findOdometricPath(
         minKey = key1;
     }
   }
-  tree.insert(minKey, initialize::kAnchorKey);
+  tree.insert(minKey, kAnchorKey);
-  tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
+  tree.insert(kAnchorKey, kAnchorKey); // root
   return tree;
 }
 
+/*****************************************************************************/
+PredecessorMap<Key> findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){
+
+  // Compute the minimum spanning tree
+  const FastMap<Key, size_t> forwardOrdering = Ordering::Natural(pose2Graph).invert();
+  const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0);
+  const auto mstEdgeIndices = utils::kruskal(pose2Graph, forwardOrdering, edgeWeights);
+
+  // std::cout << "MST Edge Indices:\n";
+  // for(const auto& i: mstEdgeIndices){
+  //   std::cout << i << ", ";
+  // }
+  // std::cout << "\n";
+
+  // Create PredecessorMap
+  PredecessorMap<Key> predecessorMap;
+  std::map<Key, bool> visitationMap;
+  std::stack<std::pair<Key, Key>> stack;
+
+  // const auto rootKey = pose2Graph[mstEdgeIndices.front()]->front();
+  // stack.push({rootKey, rootKey});
+  stack.push({kAnchorKey, kAnchorKey});
+  while (!stack.empty()) {
+      auto [u, parent] = stack.top();
+      stack.pop();
+      if (visitationMap[u]) continue;
+      visitationMap[u] = true;
+      predecessorMap[u] = parent;
+      for (const auto& edgeIdx: mstEdgeIndices) {
+          const auto v = pose2Graph[edgeIdx]->front();
+          const auto w = pose2Graph[edgeIdx]->back();
+          if((v == u || w == u) && !visitationMap[v == u ? w : v]) {
+              stack.push({v == u ? w : v, u});
+          }
+      }
+   }
+  
+  return predecessorMap;
+}
+
 /* ************************************************************************* */
 // Return the orientations of a graph including only BetweenFactors<Pose2>
 static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
@@ -232,8 +277,15 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
   if (useOdometricPath)
     tree = findOdometricPath(pose2Graph);
   else
-    tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+  {
-        BetweenFactor<Pose2> >(pose2Graph);
+    tree = findMinimumSpanningTree(pose2Graph);
+    // tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2>>(pose2Graph);
+
+    std::cout << "computeOrientations:: Spanning Tree: \n";
+    for(const auto&[k, v]: tree){
+      std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(v)) << " -> " << gtsam::DefaultKeyFormatter(gtsam::Symbol(k)) << "\n";
+    }
+  }
 
   // Create a linear factor graph (LFG) of scalars
   key2doubleMap deltaThetaMap;
@@ -241,6 +293,18 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
   getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
 
+  // std::cout << "Spanning Tree Edge Ids:\n";
+  // for(const auto& i: spanningTreeIds){
+  //   std::cout << i << ", ";
+  // }
+  // std::cout << "\n";
+
+  // std::cout << "Chord Edge Ids:\n";
+  // for(const auto& i: chordsIds){
+  //   std::cout << i << ", ";
+  // }
+  // std::cout << "\n";
+
   // temporary structure to correct wraparounds along loops
   key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
 
@@ -263,7 +327,14 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
   NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
 
   // Get orientations from relative orientation measurements
-  return computeOrientations(pose2Graph, useOdometricPath);
+  auto initial = computeOrientations(pose2Graph, useOdometricPath);
+  std::cout << "Lago::initializeOrientations: Values: \n";
+  for(const auto& [key, val]: initial){
+    std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << " -> " << val << "\n";
+  }
+  std::cout << "\n";
+
+  return initial;
 }
 
 /* ************************************************************************* */
@@ -314,8 +385,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
     }
   }
   // add prior
-  linearPose2graph.add(initialize::kAnchorKey, I3, Vector3(0.0, 0.0, 0.0),
+  linearPose2graph.add(kAnchorKey, I3, Vector3(0.0, 0.0, 0.0), priorPose2Noise);
-      priorPose2Noise);
 
   // optimize
   VectorValues posesLago = linearPose2graph.optimize();
@@ -324,7 +394,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
   Values initialGuessLago;
   for(const VectorValues::value_type& it: posesLago) {
     Key key = it.first;
-    if (key != initialize::kAnchorKey) {
+    if (key != kAnchorKey) {
       const Vector& poseVector = it.second;
       Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
           orientationsLago.at(key)(0) + poseVector(2));
@@ -361,7 +431,7 @@ Values initialize(const NonlinearFactorGraph& graph,
   // for all nodes in the tree
   for(const VectorValues::value_type& it: orientations) {
     Key key = it.first;
-    if (key != initialize::kAnchorKey) {
+    if (key != kAnchorKey) {
       const Pose2& pose = initialGuess.at<Pose2>(key);
       const Vector& orientation = it.second;
       Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));

gtsam/slam/lago.h

@@ -70,6 +70,12 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
     const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
     const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
 
+/** Given a "pose2" factor graph, find it's minimum spanning tree.
+ * Note: all 'Pose2' factors are given equal weightage.
+ */
+GTSAM_EXPORT PredecessorMap<Key> findMinimumSpanningTree(
+    const NonlinearFactorGraph& pose2Graph);
+
 /** LAGO: Return the orientations of the Pose2 in a generic factor graph */
 GTSAM_EXPORT VectorValues initializeOrientations(
     const NonlinearFactorGraph& graph, bool useOdometricPath = true);

gtsam/slam/tests/testLago.cpp

@@ -20,6 +20,7 @@
  */
 
 #include <gtsam/slam/lago.h>
+#include <gtsam/slam/InitializePose.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
@@ -64,20 +65,45 @@ NonlinearFactorGraph graph() {
 }
 }
 
+/*******************************************************************************/
+TEST(Lago, findMinimumSpanningTree) {
+  NonlinearFactorGraph g = simpleLago::graph();
+  auto gPlus = initialize::buildPoseGraph<Pose2>(g);
+  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
+
+  // We should recover the following spanning tree:
+  //
+  //              x2
+  //             /  \               
+  //            /    \              
+  //          x3     x1
+  //                 /
+  //                /
+  //              x0
+  //               |
+  //               a
+  using initialize::kAnchorKey;
+  EXPECT_LONGS_EQUAL(kAnchorKey, tree[kAnchorKey]);
+  EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]);
+  EXPECT_LONGS_EQUAL(x0, tree[x1]);
+  EXPECT_LONGS_EQUAL(x1, tree[x2]);
+  EXPECT_LONGS_EQUAL(x2, tree[x3]);
+}
+
 /* *************************************************************************** */
 TEST( Lago, checkSTandChords ) {
   NonlinearFactorGraph g = simpleLago::graph();
-  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+  auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-      BetweenFactor<Pose2> >(g);
+  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
 
   lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
   lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
 
-  DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
+  EXPECT_LONGS_EQUAL(0, spanningTreeIds[0]); // factor 0 is the first in the ST(0->1)
-  DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
+  EXPECT_LONGS_EQUAL(1, spanningTreeIds[1]); // factor 1 is the second in the ST(1->2)
-  DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the  ST(0->3)
+  EXPECT_LONGS_EQUAL(2, spanningTreeIds[2]); // factor 2 is the third in the  ST(2->3)
 
 }
 
@@ -88,10 +114,10 @@ TEST( Lago, orientationsOverSpanningTree ) {
       BetweenFactor<Pose2> >(g);
 
   // check the tree structure
-  EXPECT_LONGS_EQUAL(tree[x0], x0);
+  EXPECT_LONGS_EQUAL(x0, tree[x0]);
-  EXPECT_LONGS_EQUAL(tree[x1], x0);
+  EXPECT_LONGS_EQUAL(x0, tree[x1]);
-  EXPECT_LONGS_EQUAL(tree[x2], x0);
+  EXPECT_LONGS_EQUAL(x0, tree[x2]);
-  EXPECT_LONGS_EQUAL(tree[x3], x0);
+  EXPECT_LONGS_EQUAL(x0, tree[x3]);
 
   lago::key2doubleMap expected;
   expected[x0]=  0;
@@ -145,8 +171,8 @@ TEST( Lago, smallGraphVectorValues ) {
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
   EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
@@ -171,8 +197,8 @@ TEST( Lago, multiplePosePriors ) {
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
   EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
@@ -198,8 +224,8 @@ TEST( Lago, multiplePoseAndRotPriors ) {
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
   EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */