Removes import 'graph.h' inside 'lago.h'

gtsam/base/kruskal.h

@@ -12,7 +12,6 @@
 /**
  * @file SubgraphBuilder-inl.h
  * @date Dec 31, 2009
- * @date Jan 23, 2023
  * @author Frank Dellaert, Yong-Dian Jian
  */
 

gtsam/slam/lago.cpp

@@ -55,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;
@@ -81,7 +81,7 @@ static double computeThetaToRoot(const Key nodeKey,
 
 /* ************************************************************************* */
 key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
-    const PredecessorMap<Key>& tree) {
+    const PredecessorMap& tree) {
 
   key2doubleMap thetaToRootMap;
 
@@ -102,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;
@@ -166,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;
@@ -200,10 +200,10 @@ GaussianFactorGraph buildLinearOrientationGraph(
 }
 
 /* ************************************************************************* */
-static PredecessorMap<Key> findOdometricPath(
+static PredecessorMap findOdometricPath(
     const NonlinearFactorGraph& pose2Graph) {
 
-  PredecessorMap<Key> tree;
+  PredecessorMap tree;
   Key minKey = kAnchorKey; // this initialization does not matter
   bool minUnassigned = true;
 
@@ -216,18 +216,18 @@ 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, kAnchorKey);
-  tree.insert(kAnchorKey, kAnchorKey); // root
+  tree.emplace(minKey, kAnchorKey);
+  tree.emplace(kAnchorKey, kAnchorKey); // root
   return tree;
 }
 
 /*****************************************************************************/
-PredecessorMap<Key> findMinimumSpanningTree(
+PredecessorMap findMinimumSpanningTree(
     const NonlinearFactorGraph& pose2Graph) {
   // Compute the minimum spanning tree
   const FastMap<Key, size_t> forwardOrdering =
@@ -239,7 +239,7 @@ PredecessorMap<Key> findMinimumSpanningTree(
   // Create a PredecessorMap 'predecessorMap' such that:
   // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
   // the spanning tree
-  PredecessorMap<Key> predecessorMap;
+  PredecessorMap predecessorMap;
   std::map<Key, bool> visitationMap;
   std::stack<std::pair<Key, Key>> stack;
 
@@ -268,7 +268,7 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
                                         bool useOdometricPath) {
   gttic(lago_computeOrientations);
 
-  PredecessorMap<Key> tree;
+  PredecessorMap tree;
   // Find a minimum spanning tree
   if (useOdometricPath)
     tree = findOdometricPath(pose2Graph);

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,19 +62,19 @@ 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 it's minimum spanning tree.
  * Note: all 'Pose2' Between factors are given equal weightage.
  * Note: assumes all the edges (factors) are Between factors
  */
-GTSAM_EXPORT PredecessorMap<Key> findMinimumSpanningTree(
+GTSAM_EXPORT PredecessorMap findMinimumSpanningTree(
     const NonlinearFactorGraph& pose2Graph);
 
 /** LAGO: Return the orientations of the Pose2 in a generic factor graph */

gtsam/slam/tests/testLago.cpp

@@ -69,7 +69,7 @@ NonlinearFactorGraph graph() {
 TEST(Lago, findMinimumSpanningTree) {
   NonlinearFactorGraph g = simpleLago::graph();
   auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
+  lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
 
   // We should recover the following spanning tree:
   //
@@ -94,7 +94,7 @@ TEST(Lago, findMinimumSpanningTree) {
 TEST( Lago, checkSTandChords ) {
   NonlinearFactorGraph g = simpleLago::graph();
   auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
+  lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
 
   lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
@@ -111,7 +111,7 @@ TEST( Lago, checkSTandChords ) {
 TEST(Lago, orientationsOverSpanningTree) {
   NonlinearFactorGraph g = simpleLago::graph();
   auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
+  lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
 
   // check the tree structure
   using initialize::kAnchorKey;
@@ -138,11 +138,6 @@ TEST(Lago, orientationsOverSpanningTree) {
   lago::key2doubleMap actual;
   actual = lago::computeThetasToRoot(deltaThetaMap, tree);
 
-  std::cout << "Thetas to root Map\n";
-  for (const auto& [k, v] : actual) {
-    std::cout << k << ": " << v << "\n";
-  }
-
   DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
   DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
   DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@@ -153,7 +148,7 @@ TEST(Lago, orientationsOverSpanningTree) {
 TEST( Lago, regularizedMeasurements ) {
   NonlinearFactorGraph g = simpleLago::graph();
   auto gPlus = initialize::buildPoseGraph<Pose2>(g);
-  PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
+  lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
 
   lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T