diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h index f75ed52c3..f8f648018 100644 --- a/gtsam/base/kruskal.h +++ b/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 */ diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 90909a43f..ab8c7bea2 100644 --- a/gtsam/slam/lago.cpp +++ b/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& 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& tree) { + const PredecessorMap& tree) { key2doubleMap thetaToRootMap; @@ -102,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, void getSymbolicGraph( /*OUTPUTS*/vector& spanningTreeIds, vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/const PredecessorMap& 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& spanningTreeIds, const vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, - const PredecessorMap& tree) { + const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; @@ -200,10 +200,10 @@ GaussianFactorGraph buildLinearOrientationGraph( } /* ************************************************************************* */ -static PredecessorMap findOdometricPath( +static PredecessorMap findOdometricPath( const NonlinearFactorGraph& pose2Graph) { - PredecessorMap tree; + PredecessorMap tree; Key minKey = kAnchorKey; // this initialization does not matter bool minUnassigned = true; @@ -216,18 +216,18 @@ static PredecessorMap 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 findMinimumSpanningTree( +PredecessorMap findMinimumSpanningTree( const NonlinearFactorGraph& pose2Graph) { // Compute the minimum spanning tree const FastMap forwardOrdering = @@ -239,7 +239,7 @@ PredecessorMap findMinimumSpanningTree( // Create a PredecessorMap 'predecessorMap' such that: // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in // the spanning tree - PredecessorMap predecessorMap; + PredecessorMap predecessorMap; std::map visitationMap; std::stack> stack; @@ -268,7 +268,7 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { gttic(lago_computeOrientations); - PredecessorMap tree; + PredecessorMap tree; // Find a minimum spanning tree if (useOdometricPath) tree = findOdometricPath(pose2Graph); diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index 39212ed1b..0dfccdd58 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -37,19 +37,19 @@ #include #include #include -#include namespace gtsam { namespace lago { typedef std::map key2doubleMap; +typedef std::map 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& 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& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/const PredecessorMap& 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& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, - const key2doubleMap& orientationsToRoot, const PredecessorMap& 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 findMinimumSpanningTree( +GTSAM_EXPORT PredecessorMap findMinimumSpanningTree( const NonlinearFactorGraph& pose2Graph); /** LAGO: Return the orientations of the Pose2 in a generic factor graph */ diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index b2524b187..e762b067f 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -69,7 +69,7 @@ NonlinearFactorGraph graph() { TEST(Lago, findMinimumSpanningTree) { NonlinearFactorGraph g = simpleLago::graph(); auto gPlus = initialize::buildPoseGraph(g); - PredecessorMap 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(g); - PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); + lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); lago::key2doubleMap deltaThetaMap; vector 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(g); - PredecessorMap 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(g); - PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); + lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T