Merge pull request #1440 from ankur-rc/no_boost_lago

gtsam/base/kruskal-inl.h

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 kruskal-inl.h
+ * @date Dec 31, 2009
+ * @author Frank Dellaert
+ * @author Yong-Dian Jian
+ * @author Ankur Roy Chowdhury
+ */
+
+#pragma once
+
+#include <gtsam/base/DSFMap.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/types.h>
+#include <gtsam/inference/VariableIndex.h>
+
+#include <algorithm>
+#include <numeric>
+#include <vector>
+
+namespace gtsam::utils {
+
+/*****************************************************************************/
+/* Sort the 'weights' in increasing order and return the sorted order of its
+ * indices. */
+inline std::vector<size_t> sortedIndices(const std::vector<double> &weights) {
+  // Get the number of elements in the 'weights' vector.
+  const size_t n = weights.size();
+
+  // Create a vector of 'indices'.
+  std::vector<size_t> indices(n);
+  std::iota(indices.begin(), indices.end(), 0);
+
+  // Sort the 'indices' based on the values in 'weights'.
+  std::stable_sort(indices.begin(), indices.end(),
+                   [&weights](const size_t i0, const size_t i1) {
+                     return weights[i0] < weights[i1];
+                   });
+
+  return indices;
+}
+
+/****************************************************************/
+template <class FACTOR>
+std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
+                            const std::vector<double> &weights) {
+  if (fg.size() != weights.size()) {
+    throw std::runtime_error(
+        "kruskal() failure: fg.size() != weights.size(), all factors need to "
+        "assigned a weight");
+  }
+
+  // Create an index from variables to factor indices.
+  const VariableIndex variableIndex(fg);
+
+  // 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);
+
+  // Initialize disjoint-set forest to keep track of merged Keys.
+  DSFMap<Key> dsf;
+
+  // Loop over all edges in order of increasing weight.
+  size_t count = 0;
+  for (const size_t index : sortedIndices) {
+    const auto factor = fg[index];
+
+    // Ignore non-binary edges.
+    if (factor->size() != 2) continue;
+
+    // Find the representatives of the sets for both the Keys in the binary
+    // factor.
+    const auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
+
+    // Check if there is a potential loop.
+    const bool loop = (u == v);
+    if (!loop) {
+      // Merge the sets.
+      dsf.merge(u, v);
+
+      // Add the current index to the tree.
+      treeIndices.push_back(index);
+
+      // Break if all the Keys have been added to the tree.
+      if (++count == n - 1) break;
+    }
+  }
+  return treeIndices;
+}
+
+}  // namespace gtsam::utils

gtsam/base/kruskal.h

@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 kruskal.h
+ * @date Dec 31, 2009
+ * @author Frank Dellaert
+ * @author Yong-Dian Jian
+ * @author Ankur Roy Chowdhury
+ */
+
+#pragma once
+
+#include <gtsam/base/FastMap.h>
+#include <gtsam/inference/FactorGraph.h>
+
+#include <vector>
+
+namespace gtsam::utils {
+/**
+ * Compute the minimum spanning tree (MST) using Kruskal's algorithm
+ * @param fg Factor graph
+ * @param weights Weights of the edges/factors in the factor graph
+ * @return Edge/factor indices spanning the MST
+ * @note Only binary factors are considered while constructing the spanning tree 
+ * @note The indices of 'weights' should match the indices of the edges in the factor graph 
+ */
+template <class FACTOR>
+std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
+                            const std::vector<double> &weights);
+}  // namespace gtsam::utils
+
+#include <gtsam/base/kruskal-inl.h>

gtsam/base/tests/testKruskal.cpp

@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/geometry/Rot3.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+#include <list>
+#include <memory>
+#include <vector>
+
+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;
+  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;
+
+  // Create factor graph.
+  const auto g = makeTestGaussianFactorGraph();
+
+  // Assign weights to all the edges in the graph.
+  const auto weights = std::vector<double>(g.size(), 1.0);
+
+  const auto mstEdgeIndices = utils::kruskal(g, weights);
+
+  EXPECT(mstEdgeIndices[0] == 0);
+  EXPECT(mstEdgeIndices[1] == 1);
+  EXPECT(mstEdgeIndices[2] == 2);
+}
+
+/* ************************************************************************* */
+TEST(kruskal, NonlinearFactorGraph) {
+  using namespace gtsam;
+
+  // Create factor graph.
+  const auto g = makeTestNonlinearFactorGraph();
+
+  // Assign weights to all the edges in the graph.
+  const auto weights = std::vector<double>(g.size(), 1.0);
+
+  const auto mstEdgeIndices = utils::kruskal(g, weights);
+
+  EXPECT(mstEdgeIndices[0] == 0);
+  EXPECT(mstEdgeIndices[1] == 1);
+  EXPECT(mstEdgeIndices[2] == 2);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

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());
@@ -240,7 +219,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(
 
 /****************************************************************/
 vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
-                                          const FastMap<Key, size_t> &ordering,
                                           const vector<double> &weights) const {
   const SubgraphBuilderParameters &p = parameters_;
   switch (p.skeletonType) {
@@ -251,7 +229,7 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
       return bfs(gfg);
       break;
     case SubgraphBuilderParameters::KRUSKAL:
-      return kruskal(gfg, ordering, weights);
+      return kruskal(gfg, weights);
       break;
     default:
       std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
@@ -327,33 +305,8 @@ vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
 
 /****************************************************************/
 vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
-                                        const FastMap<Key, size_t> &ordering,
                                         const vector<double> &weights) const {
-  const VariableIndex variableIndex(gfg);
+  return utils::kruskal(gfg, weights);
-  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;
 }
 
 /****************************************************************/
@@ -382,7 +335,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   vector<double> weights = this->weights(gfg);
 
   // Build spanning tree.
-  const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
+  const vector<size_t> tree = buildTree(gfg, weights);
   if (tree.size() != n - 1) {
     throw std::runtime_error(
         "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
@@ -406,6 +359,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
 /****************************************************************/
 SubgraphBuilder::Weights SubgraphBuilder::weights(
     const GaussianFactorGraph &gfg) const {
+
   const size_t m = gfg.size();
   Weights weight;
   weight.reserve(m);

gtsam/linear/SubgraphBuilder.h

@@ -163,13 +163,11 @@ class GTSAM_EXPORT SubgraphBuilder {
 
  private:
   std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
-                                const FastMap<Key, size_t> &ordering,
                                 const std::vector<double> &weights) const;
   std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
   std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
   std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
   std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
-                              const FastMap<Key, size_t> &ordering,
                               const std::vector<double> &weights) const;
   std::vector<size_t> sample(const std::vector<double> &weights,
                              const size_t t) const;

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;
 
@@ -49,7 +55,7 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
  * The root is assumed to have orientation zero.
  */
 static double computeThetaToRoot(const Key nodeKey,
-    const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
+    const PredecessorMap& tree, const key2doubleMap& deltaThetaMap,
     const key2doubleMap& thetaFromRootMap) {
 
   double nodeTheta = 0;
@@ -75,20 +81,19 @@ static double computeThetaToRoot(const Key nodeKey,
 
 /* ************************************************************************* */
 key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
-    const PredecessorMap<Key>& tree) {
+    const PredecessorMap& tree) {
 
   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;
 }
@@ -97,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
 void getSymbolicGraph(
 /*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
     key2doubleMap& deltaThetaMap,
-    /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
+    /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) {
 
   // Get keys for which you want the orientation
   size_t id = 0;
@@ -161,7 +166,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
 GaussianFactorGraph buildLinearOrientationGraph(
     const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
     const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
-    const PredecessorMap<Key>& tree) {
+    const PredecessorMap& tree) {
 
   GaussianFactorGraph lagoGraph;
   Vector deltaTheta;
@@ -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,16 +195,16 @@ 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;
 }
 
 /* ************************************************************************* */
-static PredecessorMap<Key> findOdometricPath(
+static PredecessorMap findOdometricPath(
     const NonlinearFactorGraph& pose2Graph) {
 
-  PredecessorMap<Key> tree;
+  PredecessorMap 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) {
@@ -211,42 +216,77 @@ static PredecessorMap<Key> findOdometricPath(
       minUnassigned = false;
     }
     if (key2 - key1 == 1) { // consecutive keys
-      tree.insert(key2, key1);
+      tree.emplace(key2, key1);
       if (key1 < minKey)
         minKey = key1;
     }
   }
-  tree.insert(minKey, initialize::kAnchorKey);
+  tree.emplace(minKey, kAnchorKey);
-  tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
+  tree.emplace(kAnchorKey, kAnchorKey); // root
   return tree;
 }
 
+/*****************************************************************************/
+PredecessorMap findMinimumSpanningTree(
+    const NonlinearFactorGraph& pose2Graph) {
+  // Compute the minimum spanning tree
+  const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0);
+  const auto mstEdgeIndices =
+      utils::kruskal(pose2Graph, edgeWeights);
+
+  // Create a PredecessorMap 'predecessorMap' such that:
+  // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
+  // the spanning tree
+  PredecessorMap predecessorMap;
+  std::map<Key, bool> visitationMap;
+  std::stack<std::pair<Key, Key>> stack;
+
+  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,
                                         bool useOdometricPath) {
   gttic(lago_computeOrientations);
 
+  PredecessorMap tree;
   // Find a minimum spanning tree
-  PredecessorMap<Key> tree;
   if (useOdometricPath)
     tree = findOdometricPath(pose2Graph);
   else
-    tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+    tree = findMinimumSpanningTree(pose2Graph);
-        BetweenFactor<Pose2> >(pose2Graph);
 
   // Create a linear factor graph (LFG) of scalars
   key2doubleMap deltaThetaMap;
-  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
+  vector<size_t>
-  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
+      spanningTreeIds;  // ids of between factors forming the spanning tree T
+  vector<size_t>
+      chordsIds;  // ids of between factors corresponding to chordsIds wrt T
   getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
 
   // temporary structure to correct wraparounds along loops
   key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
 
   // regularize measurements and plug everything in a factor graph
-  GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds,
+  GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(
-      chordsIds, pose2Graph, orientationsToRoot, tree);
+      spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
 
   // Solve the LFG
   VectorValues orientationsLago = lagoGraph.optimize();
@@ -257,7 +297,6 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
 /* ************************************************************************* */
 VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
                                     bool useOdometricPath) {
-
   // We "extract" the Pose2 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
@@ -314,8 +353,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 +362,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 +399,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

@@ -37,19 +37,19 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph.h>
 
 namespace gtsam {
 namespace lago {
 
 typedef std::map<Key, double> key2doubleMap;
+typedef std::map<Key, Key> PredecessorMap;
 
 /**
  * Compute the cumulative orientations (without wrapping)
  * for all nodes wrt the root (root has zero orientation).
  */
 GTSAM_EXPORT key2doubleMap computeThetasToRoot(
-    const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
+    const key2doubleMap& deltaThetaMap, const PredecessorMap& tree);
 
 /**
  * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
@@ -62,13 +62,20 @@ GTSAM_EXPORT key2doubleMap computeThetasToRoot(
 GTSAM_EXPORT void getSymbolicGraph(
 /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
     key2doubleMap& deltaThetaMap,
-    /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
+    /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g);
 
 /** Linear factor graph with regularized orientation measurements */
 GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
     const std::vector<size_t>& spanningTreeIds,
     const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
-    const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
+    const key2doubleMap& orientationsToRoot, const PredecessorMap& tree);
+
+/** Given a "pose2" factor graph, find its minimum spanning tree.
+ * Note: All 'Pose2' Between factors are given equal weightage.
+ * Note: Assumes all the edges (factors) are Between factors.
+ */
+GTSAM_EXPORT PredecessorMap findMinimumSpanningTree(
+    const NonlinearFactorGraph& pose2Graph);
 
 /** LAGO: Return the orientations of the Pose2 in a generic factor graph */
 GTSAM_EXPORT VectorValues initializeOrientations(

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,48 +65,79 @@ NonlinearFactorGraph graph() {
 }
 }
 
+/*******************************************************************************/
+TEST(Lago, findMinimumSpanningTree) {
+  NonlinearFactorGraph g = simpleLago::graph();
+  auto gPlus = initialize::buildPoseGraph<Pose2>(g);
+  lago::PredecessorMap 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);
+  lago::PredecessorMap 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)
 
 }
 
 /* *************************************************************************** */
-TEST( Lago, orientationsOverSpanningTree ) {
+TEST(Lago, orientationsOverSpanningTree) {
   NonlinearFactorGraph g = simpleLago::graph();
-  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+  auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-      BetweenFactor<Pose2> >(g);
+  lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
 
   // check the tree structure
-  EXPECT_LONGS_EQUAL(tree[x0], x0);
+  using initialize::kAnchorKey;
-  EXPECT_LONGS_EQUAL(tree[x1], x0);
+
-  EXPECT_LONGS_EQUAL(tree[x2], x0);
+  EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]);
-  EXPECT_LONGS_EQUAL(tree[x3], x0);
+  EXPECT_LONGS_EQUAL(x0, tree[x1]);
+  EXPECT_LONGS_EQUAL(x1, tree[x2]);
+  EXPECT_LONGS_EQUAL(x2, tree[x3]);
 
   lago::key2doubleMap expected;
-  expected[x0]=  0;
+  expected[x0] = 0;
-  expected[x1]=  M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
+  expected[x1] = M_PI / 2;      // edges traversed: x0->x1
-  expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
+  expected[x2] = M_PI;          // edges traversed: x0->x1->x2
-  expected[x3]= -M_PI/2;  // edge x0->x3 (consistent with edge (x0,x3))
+  expected[x3] = 3 * M_PI / 2;  // edges traversed: x0->x1->x2->x3
 
   lago::key2doubleMap deltaThetaMap;
-  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
+  vector<size_t>
-  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
+      spanningTreeIds;  // ids of between factors forming the spanning tree T
-  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
+  vector<size_t>
+      chordsIds;  // ids of between factors corresponding to chordsIds wrt T
+  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree,
+                         gPlus);
 
   lago::key2doubleMap actual;
   actual = lago::computeThetasToRoot(deltaThetaMap, tree);
+
   DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
   DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
   DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@@ -115,24 +147,24 @@ TEST( Lago, orientationsOverSpanningTree ) {
 /* *************************************************************************** */
 TEST( Lago, regularizedMeasurements ) {
   NonlinearFactorGraph g = simpleLago::graph();
-  PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+  auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-      BetweenFactor<Pose2> >(g);
+  lago::PredecessorMap 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);
+  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, gPlus);
 
   lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree);
 
-  GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
+  GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, gPlus, orientationsToRoot, tree);
   std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
   // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
   Vector actual = (Vector(5) <<  actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)).finished();
   // this is the whitened error, so we multiply by the std to unwhiten
   actual = 0.1 * actual;
   // Expected regularized measurements (same for the spanning tree, corrected for the chordsIds)
-  Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2).finished();
+  Vector expected = (Vector(5) << M_PI/2, M_PI/2, M_PI/2, 0 , -M_PI).finished();
 
   EXPECT(assert_equal(expected, actual, 1e-6));
 }
@@ -145,8 +177,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 +203,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 +230,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));
 }
 
 /* *************************************************************************** */