diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h new file mode 100644 index 000000000..a0210b8e0 --- /dev/null +++ b/gtsam/base/kruskal-inl.h @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * 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 SubgraphBuilder-inl.h + * @date Dec 31, 2009 + * @date Jan 23, 2023 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace gtsam::utils +{ + + /*****************************************************************************/ + /* sort the container and return permutation index with default comparator */ + template + static std::vector sort_idx(const Container &src) + { + typedef typename Container::value_type T; + 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()); + + /* 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) + { + const VariableIndex variableIndex(fg); + const size_t n = variableIndex.size(); + const std::vector sortedIndices = sort_idx(weights); + + /* initialize buffer */ + std::vector treeIndices; + treeIndices.reserve(n - 1); + + // container for acsendingly sorted edges + DSFVector dsf(n); + + size_t count = 0; + for (const size_t index : sortedIndices) + { + const auto &f = *fg[index]; + const auto keys = f.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; + } + +} // namespace gtsam::utils diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h new file mode 100644 index 000000000..c89b491a4 --- /dev/null +++ b/gtsam/base/kruskal.h @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * 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 SubgraphBuilder-inl.h + * @date Dec 31, 2009 + * @date Jan 23, 2023 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#pragma once + +#include + +#include + +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 new file mode 100644 index 000000000..b189e29f1 --- /dev/null +++ b/gtsam/base/tests/testKruskal.cpp @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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; + Matrix I = I_2x2; + Vector2 b(0, 0); + 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; + + const auto g = makeTestGaussianFactorGraph(); + + 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); + + // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices) + // { + // std::cout << "MST Edge indices are: \n"; + // for (const auto &edge : mst_edge_indices) + // { + // std::cout << edge << " : "; + // for (const auto &key : graph[edge]->keys()) + // { + // std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", "; + // } + // std::cout << "\n"; + // } + // }; + + // PrintMst(g, mstEdgeIndices); + + EXPECT(mstEdgeIndices[0] == 0); + EXPECT(mstEdgeIndices[1] == 1); + EXPECT(mstEdgeIndices[2] == 2); +} + +/* ************************************************************************* */ +TEST(kruskal, NonlinearFactorGraph) +{ + using namespace gtsam; + + const auto g = makeTestNonlinearFactorGraph(); + + 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); + + // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices) + // { + // std::cout << "MST Edge indices are: \n"; + // for (const auto &edge : mst_edge_indices) + // { + // std::cout << edge << " : "; + // for (const auto &key : graph[edge]->keys()) + // { + // std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", "; + // } + // std::cout << "\n"; + // } + // }; + + // PrintMst(g, mstEdgeIndices); + + EXPECT(mstEdgeIndices[0] == 0); + EXPECT(mstEdgeIndices[1] == 1); + EXPECT(mstEdgeIndices[2] == 2); +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index ceae2e3ab..49668fc59 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -234,7 +234,7 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { // Convert to a graph that boost understands SDGraph g = gtsam::toBoostGraph(fg); - // find minimum spanning tree + // // find minimum spanning tree std::vector::Vertex> p_map(boost::num_vertices(g)); prim_minimum_spanning_tree(g, &p_map[0]); diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 083cd72c3..0d899cb11 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -52,28 +53,6 @@ using std::vector; namespace gtsam { -/*****************************************************************************/ -/* sort the container and return permutation index with default comparator */ -template -static vector sort_idx(const Container &src) { - typedef typename Container::value_type T; - const size_t n = src.size(); - 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()); - - /* copy back */ - vector idx; - idx.reserve(n); - for (size_t i = 0; i < n; i++) { - idx.push_back(tmp[i].first); - } - return idx; -} - /****************************************************************************/ Subgraph::Subgraph(const vector &indices) { edges_.reserve(indices.size()); @@ -238,101 +217,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator( return "UNKNOWN"; } -/****************************************************************/ -std::vector utils::assignWeights(const GaussianFactorGraph &gfg, const SubgraphBuilderParameters::SkeletonWeight &skeletonWeight) -{ - using Weights = std::vector; - - const size_t m = gfg.size(); - Weights weights; - weights.reserve(m); - - for (const GaussianFactor::shared_ptr &gf : gfg) - { - switch (skeletonWeight) - { - case SubgraphBuilderParameters::EQUAL: - weights.push_back(1.0); - break; - case SubgraphBuilderParameters::RHS_2NORM: - { - if (JacobianFactor::shared_ptr jf = - std::dynamic_pointer_cast(gf)) - { - weights.push_back(jf->getb().norm()); - } - else if (HessianFactor::shared_ptr hf = - std::dynamic_pointer_cast(gf)) - { - weights.push_back(hf->linearTerm().norm()); - } - } - break; - case SubgraphBuilderParameters::LHS_FNORM: - { - if (JacobianFactor::shared_ptr jf = - std::dynamic_pointer_cast(gf)) - { - weights.push_back(std::sqrt(jf->getA().squaredNorm())); - } - else if (HessianFactor::shared_ptr hf = - std::dynamic_pointer_cast(gf)) - { - weights.push_back(std::sqrt(hf->information().squaredNorm())); - } - } - break; - - case SubgraphBuilderParameters::RANDOM: - weights.push_back(std::rand() % 100 + 1.0); - break; - - default: - throw std::invalid_argument( - "utils::assign_weights: undefined weight scheme "); - break; - } - } - return weights; -} - - -/****************************************************************/ -std::vector utils::kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, - const std::vector &weights) -{ - const VariableIndex variableIndex(gfg); - const size_t n = variableIndex.size(); - const vector sortedIndices = sort_idx(weights); - - /* initialize buffer */ - vector 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; -} - /****************************************************************/ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, @@ -477,7 +361,59 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { /****************************************************************/ SubgraphBuilder::Weights SubgraphBuilder::weights( const GaussianFactorGraph &gfg) const { - return utils::assignWeights(gfg, parameters_.skeletonWeight); + using Weights = std::vector; + + const size_t m = gfg.size(); + Weights weights; + weights.reserve(m); + + for (const GaussianFactor::shared_ptr &gf : gfg) + { + switch (parameters_.skeletonWeight) + { + case SubgraphBuilderParameters::EQUAL: + weights.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: + { + if (JacobianFactor::shared_ptr jf = + std::dynamic_pointer_cast(gf)) + { + weights.push_back(jf->getb().norm()); + } + else if (HessianFactor::shared_ptr hf = + std::dynamic_pointer_cast(gf)) + { + weights.push_back(hf->linearTerm().norm()); + } + } + break; + case SubgraphBuilderParameters::LHS_FNORM: + { + if (JacobianFactor::shared_ptr jf = + std::dynamic_pointer_cast(gf)) + { + weights.push_back(std::sqrt(jf->getA().squaredNorm())); + } + else if (HessianFactor::shared_ptr hf = + std::dynamic_pointer_cast(gf)) + { + weights.push_back(std::sqrt(hf->information().squaredNorm())); + } + } + break; + + case SubgraphBuilderParameters::RANDOM: + weights.push_back(std::rand() % 100 + 1.0); + break; + + default: + throw std::invalid_argument( + "utils::assign_weights: undefined weight scheme "); + break; + } + } + return weights; } /*****************************************************************************/ diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index cd369fc5f..34b397cc9 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -149,16 +149,6 @@ struct GTSAM_EXPORT SubgraphBuilderParameters { static std::string augmentationWeightTranslator(AugmentationWeight w); }; -namespace utils -{ - - std::vector assignWeights(const GaussianFactorGraph &gfg, - const SubgraphBuilderParameters::SkeletonWeight &skeleton_weight); - std::vector kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, - const std::vector &weights); -} - /*****************************************************************************/ class GTSAM_EXPORT SubgraphBuilder { public: @@ -171,8 +161,6 @@ class GTSAM_EXPORT SubgraphBuilder { virtual ~SubgraphBuilder() {} virtual Subgraph operator()(const GaussianFactorGraph &jfg) const; -public: - private: std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, @@ -200,4 +188,4 @@ GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, std::pair splitFactorGraph( const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 9a6f2c76b..616c77062 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -129,55 +129,6 @@ TEST( SubgraphSolver, constructor3 ) DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } -/* ************************************************************************* */ -TEST(SubgraphBuilder, utilsAssignWeights) -{ - const auto [g, _] = example::planarGraph(N); // A*x-b - const auto weights = utils::assignWeights(g, gtsam::SubgraphBuilderParameters::SkeletonWeight::EQUAL); - - EXPECT(weights.size() == g.size()); - for (const auto &i : weights) - { - EXPECT_DOUBLES_EQUAL(weights[i], 1.0, 1e-12); - } -} - -/* ************************************************************************* */ -TEST(SubgraphBuilder, utilsKruskal) -{ - - const auto [g, _] = example::planarGraph(N); // A*x-b - - const FastMap forward_ordering = Ordering::Natural(g).invert(); - const auto weights = utils::assignWeights(g, gtsam::SubgraphBuilderParameters::SkeletonWeight::EQUAL); - - const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); - - // auto PrintMst = [](const auto &graph, const auto &mst_edge_indices) - // { - // std::cout << "MST Edge indices are: \n"; - // for (const auto &edge : mst_edge_indices) - // { - // std::cout << edge << " : "; - // for (const auto &key : graph[edge]->keys()) - // { - // std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << ", "; - // } - // std::cout << "\n"; - // } - // }; - - // PrintMst(g, mstEdgeIndices); - - EXPECT(mstEdgeIndices[0] == 1); - EXPECT(mstEdgeIndices[1] == 2); - EXPECT(mstEdgeIndices[2] == 3); - EXPECT(mstEdgeIndices[3] == 4); - EXPECT(mstEdgeIndices[4] == 5); - EXPECT(mstEdgeIndices[5] == 6); - EXPECT(mstEdgeIndices[6] == 7); - EXPECT(mstEdgeIndices[7] == 8); -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }