diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index a0210b8e0..725814fa7 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -34,12 +34,10 @@ namespace gtsam::utils /*****************************************************************************/ /* sort the container and return permutation index with default comparator */ - template - static std::vector sort_idx(const Container &src) + inline std::vector sortedIndices(const std::vector &src) { - typedef typename Container::value_type T; const size_t n = src.size(); - std::vector> tmp; + std::vector> 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 &ordering, const std::vector &weights) { + // Create an index from variables to factor indices. const VariableIndex variableIndex(fg); - const size_t n = variableIndex.size(); - const std::vector sortedIndices = sort_idx(weights); - /* initialize buffer */ + // Get indices in sort-order of the weights + const std::vector sortedIndices = gtsam::utils::sortedIndices(weights); + + // Create a vector to hold MST indices. + const size_t n = variableIndex.size(); std::vector treeIndices; treeIndices.reserve(n - 1); - // container for acsendingly sorted edges - DSFVector dsf(n); + // Initialize disjoint-set forest to keep track of merged 'blah'. + DSFMap 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 keys = f.keys(); - if (keys.size() != 2) + const auto factor = fg[index]; + + // Ignore non-binary edges. + if (factor->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)) + + auto u = dsf.find(factor->front()), v = dsf.find(factor->back()); + auto loop = (u == v); + if (!loop) { dsf.merge(u, v); treeIndices.push_back(index); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index cb9d98218..0732c2a32 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -24,14 +24,20 @@ #include #include #include +#include #include +#include +#include + 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 - thetaToRootMap.insert(pair(initialize::kAnchorKey, 0.0)); + // Orientation of the root + 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; - double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, + const double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); - thetaToRootMap.insert(pair(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 findOdometricPath( const NonlinearFactorGraph& pose2Graph) { 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& factor: pose2Graph) { @@ -216,11 +221,51 @@ static PredecessorMap findOdometricPath( minKey = key1; } } - tree.insert(minKey, initialize::kAnchorKey); - tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root + tree.insert(minKey, kAnchorKey); + tree.insert(kAnchorKey, kAnchorKey); // root return tree; } +/*****************************************************************************/ +PredecessorMap findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){ + + // Compute the minimum spanning tree + const FastMap forwardOrdering = Ordering::Natural(pose2Graph).invert(); + const auto edgeWeights = std::vector(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 predecessorMap; + std::map visitationMap; + std::stack> 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 static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, @@ -232,8 +277,15 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + { + tree = findMinimumSpanningTree(pose2Graph); + // tree = findMinimumSpanningTree>(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 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(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), - priorPose2Noise); + linearPose2graph.add(kAnchorKey, I3, Vector3(0.0, 0.0, 0.0), 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(key); const Vector& orientation = it.second; Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index 0df80ac42..ca6a2c89b 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -70,6 +70,12 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( const std::vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); +/** Given a "pose2" factor graph, find it's minimum spanning tree. + * Note: all 'Pose2' factors are given equal weightage. + */ +GTSAM_EXPORT PredecessorMap 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); diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 43049994b..bfb2a4051 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -20,6 +20,7 @@ */ #include +#include #include #include #include @@ -64,20 +65,45 @@ NonlinearFactorGraph graph() { } } +/*******************************************************************************/ +TEST(Lago, findMinimumSpanningTree) { + NonlinearFactorGraph g = simpleLago::graph(); + auto gPlus = initialize::buildPoseGraph(g); + 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 tree = findMinimumSpanningTree >(g); + auto gPlus = initialize::buildPoseGraph(g); + PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector 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) - DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) - DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3) + EXPECT_LONGS_EQUAL(0, spanningTreeIds[0]); // factor 0 is the first in the ST(0->1) + EXPECT_LONGS_EQUAL(1, spanningTreeIds[1]); // factor 1 is the second in the ST(1->2) + EXPECT_LONGS_EQUAL(2, spanningTreeIds[2]); // factor 2 is the third in the ST(2->3) } @@ -88,10 +114,10 @@ TEST( Lago, orientationsOverSpanningTree ) { BetweenFactor >(g); // check the tree structure - EXPECT_LONGS_EQUAL(tree[x0], x0); - EXPECT_LONGS_EQUAL(tree[x1], x0); - EXPECT_LONGS_EQUAL(tree[x2], x0); - EXPECT_LONGS_EQUAL(tree[x3], x0); + EXPECT_LONGS_EQUAL(x0, tree[x0]); + EXPECT_LONGS_EQUAL(x0, tree[x1]); + EXPECT_LONGS_EQUAL(x0, tree[x2]); + 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) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 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) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -194,12 +220,12 @@ TEST( Lago, multiplePoseAndRotPriors ) { NonlinearFactorGraph g = simpleLago::graph(); g.addPrior(x1, simpleLago::pose1.theta(), model); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); - + // 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) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6)); } /* *************************************************************************** */