diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 725814fa7..73dcf9298 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -18,84 +18,74 @@ #pragma once -#include -#include #include #include +#include #include #include #include - #include -namespace gtsam::utils -{ +namespace gtsam::utils { - /*****************************************************************************/ - /* sort the container and return permutation index with default comparator */ - inline std::vector sortedIndices(const std::vector &src) - { - const size_t n = src.size(); - std::vector> tmp; - tmp.reserve(n); - for (size_t i = 0; i < n; i++) - tmp.emplace_back(i, src[i]); +/*****************************************************************************/ +/* sort the container and return permutation index with default comparator */ +inline std::vector sortedIndices(const std::vector &src) { + const size_t n = src.size(); + std::vector> 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()); + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()); - /* copy back */ - std::vector idx; - idx.reserve(n); - for (size_t i = 0; i < n; i++) - { - idx.push_back(tmp[i].first); - } - return idx; + /* copy back */ + std::vector idx; + idx.reserve(n); + for (size_t i = 0; i < n; i++) { + idx.push_back(tmp[i].first); + } + return idx; +} + +/****************************************************************/ +template +std::vector kruskal(const Graph &fg, + const FastMap &ordering, + const std::vector &weights) { + // Create an index from variables to factor indices. + const VariableIndex variableIndex(fg); + + // 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); + + // 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 factor = fg[index]; + + // Ignore non-binary edges. + if (factor->size() != 2) continue; + + 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); + if (++count == n - 1) break; } + } + return treeIndices; +} - /****************************************************************/ - template - std::vector kruskal(const Graph &fg, - const FastMap &ordering, - const std::vector &weights) - { - // Create an index from variables to factor indices. - const VariableIndex variableIndex(fg); - - // 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); - - // 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 factor = fg[index]; - - // Ignore non-binary edges. - if (factor->size() != 2) - continue; - - 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); - if (++count == n - 1) - break; - } - } - return treeIndices; - } - -} // namespace gtsam::utils +} // namespace gtsam::utils diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h index c89b491a4..f75ed52c3 100644 --- a/gtsam/base/kruskal.h +++ b/gtsam/base/kruskal.h @@ -22,12 +22,11 @@ #include -namespace gtsam::utils -{ - template - std::vector kruskal(const FactorGraph &fg, - const FastMap &ordering, - const std::vector &weights); +namespace gtsam::utils { +template +std::vector kruskal(const FactorGraph &fg, + const FastMap &ordering, + const std::vector &weights); } #include \ No newline at end of file diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index b88fb5301..5fe261d34 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -18,92 +18,86 @@ #include #include #include - +#include +#include +#include #include #include #include -#include -#include -#include -#include #include #include +#include -gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() -{ - using namespace gtsam; - using namespace symbol_shorthand; +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); + 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; + return gfg; } -gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() -{ - using namespace gtsam; - using namespace symbol_shorthand; +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); + 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; + return nfg; } /* ************************************************************************* */ -TEST(kruskal, GaussianFactorGraph) -{ - using namespace gtsam; +TEST(kruskal, GaussianFactorGraph) { + using namespace gtsam; - const auto g = makeTestGaussianFactorGraph(); + const auto g = makeTestGaussianFactorGraph(); - const FastMap forward_ordering = Ordering::Natural(g).invert(); - const auto weights = std::vector(g.size(), 1.0); + const FastMap forward_ordering = Ordering::Natural(g).invert(); + const auto weights = std::vector(g.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); + const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); - EXPECT(mstEdgeIndices[0] == 0); - EXPECT(mstEdgeIndices[1] == 1); - EXPECT(mstEdgeIndices[2] == 2); + EXPECT(mstEdgeIndices[0] == 0); + EXPECT(mstEdgeIndices[1] == 1); + EXPECT(mstEdgeIndices[2] == 2); } /* ************************************************************************* */ -TEST(kruskal, NonlinearFactorGraph) -{ - using namespace gtsam; +TEST(kruskal, NonlinearFactorGraph) { + using namespace gtsam; - const auto g = makeTestNonlinearFactorGraph(); + const auto g = makeTestNonlinearFactorGraph(); - const FastMap forward_ordering = Ordering::Natural(g).invert(); - const auto weights = std::vector(g.size(), 1.0); + const FastMap forward_ordering = Ordering::Natural(g).invert(); + const auto weights = std::vector(g.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); + const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); - EXPECT(mstEdgeIndices[0] == 0); - EXPECT(mstEdgeIndices[1] == 1); - EXPECT(mstEdgeIndices[2] == 2); + EXPECT(mstEdgeIndices[0] == 0); + EXPECT(mstEdgeIndices[1] == 1); + EXPECT(mstEdgeIndices[2] == 2); } /* ************************************************************************* */ -int main() -{ - TestResult tr; - return TestRegistry::runAllTests(tr); +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index bfb2a4051..b2524b187 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -108,30 +108,41 @@ TEST( Lago, checkSTandChords ) { } /* *************************************************************************** */ -TEST( Lago, orientationsOverSpanningTree ) { +TEST(Lago, orientationsOverSpanningTree) { NonlinearFactorGraph g = simpleLago::graph(); - PredecessorMap tree = findMinimumSpanningTree >(g); + auto gPlus = initialize::buildPoseGraph(g); + PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); // check the tree structure - EXPECT_LONGS_EQUAL(x0, tree[x0]); + using initialize::kAnchorKey; + + EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]); EXPECT_LONGS_EQUAL(x0, tree[x1]); - EXPECT_LONGS_EQUAL(x0, tree[x2]); - EXPECT_LONGS_EQUAL(x0, tree[x3]); + EXPECT_LONGS_EQUAL(x1, tree[x2]); + EXPECT_LONGS_EQUAL(x2, tree[x3]); lago::key2doubleMap expected; - expected[x0]= 0; - expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) - expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) - expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) + expected[x0] = 0; + expected[x1] = M_PI / 2; // edges traversed: x0->x1 + expected[x2] = M_PI; // edges traversed: x0->x1->x2 + expected[x3] = 3 * M_PI / 2; // edges traversed: x0->x1->x2->x3 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); + 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, + gPlus); 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); @@ -141,24 +152,24 @@ TEST( Lago, orientationsOverSpanningTree ) { /* *************************************************************************** */ TEST( Lago, regularizedMeasurements ) { 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); + 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 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)); }