From 981d8e9391dd24571e37218d44944044eddd3d0e Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Mon, 23 Jan 2023 16:25:51 -0800 Subject: [PATCH 01/18] factor out class methods 'weights' and 'kruskal' into free-functions --- gtsam/linear/SubgraphBuilder.cpp | 161 +++++++++++++++++++------------ gtsam/linear/SubgraphBuilder.h | 19 +++- tests/testSubgraphSolver.cpp | 37 +++++++ 3 files changed, 150 insertions(+), 67 deletions(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 97d681547..083cd72c3 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -238,6 +238,101 @@ 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, @@ -329,31 +424,7 @@ vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const vector &weights) const { - 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; + return utils::kruskal(gfg, ordering, weights); } /****************************************************************/ @@ -406,45 +477,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { /****************************************************************/ SubgraphBuilder::Weights SubgraphBuilder::weights( const GaussianFactorGraph &gfg) const { - const size_t m = gfg.size(); - Weights weight; - weight.reserve(m); - - for (const GaussianFactor::shared_ptr &gf : gfg) { - switch (parameters_.skeletonWeight) { - case SubgraphBuilderParameters::EQUAL: - weight.push_back(1.0); - break; - case SubgraphBuilderParameters::RHS_2NORM: { - if (JacobianFactor::shared_ptr jf = - std::dynamic_pointer_cast(gf)) { - weight.push_back(jf->getb().norm()); - } else if (HessianFactor::shared_ptr hf = - std::dynamic_pointer_cast(gf)) { - weight.push_back(hf->linearTerm().norm()); - } - } break; - case SubgraphBuilderParameters::LHS_FNORM: { - if (JacobianFactor::shared_ptr jf = - std::dynamic_pointer_cast(gf)) { - weight.push_back(std::sqrt(jf->getA().squaredNorm())); - } else if (HessianFactor::shared_ptr hf = - std::dynamic_pointer_cast(gf)) { - weight.push_back(std::sqrt(hf->information().squaredNorm())); - } - } break; - - case SubgraphBuilderParameters::RANDOM: - weight.push_back(std::rand() % 100 + 1.0); - break; - - default: - throw std::invalid_argument( - "SubgraphBuilder::weights: undefined weight scheme "); - break; - } - } - return weight; + return utils::assignWeights(gfg, parameters_.skeletonWeight); } /*****************************************************************************/ diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index fe8f704dc..cd369fc5f 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -149,6 +149,16 @@ 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: @@ -161,6 +171,8 @@ class GTSAM_EXPORT SubgraphBuilder { virtual ~SubgraphBuilder() {} virtual Subgraph operator()(const GaussianFactorGraph &jfg) const; +public: + private: std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, @@ -168,12 +180,13 @@ class GTSAM_EXPORT SubgraphBuilder { std::vector unary(const GaussianFactorGraph &gfg) const; std::vector natural_chain(const GaussianFactorGraph &gfg) const; std::vector bfs(const GaussianFactorGraph &gfg) const; + std::vector sample(const std::vector &weights, + const size_t t) const; std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const; - std::vector sample(const std::vector &weights, - const size_t t) const; - Weights weights(const GaussianFactorGraph &gfg) const; + Weights weights(const GaussianFactorGraph &gfg) const ; + SubgraphBuilderParameters parameters_; }; diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 69b5fe5f9..245572896 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -129,6 +129,43 @@ TEST( SubgraphSolver, constructor3 ) DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } +/* ************************************************************************* */ +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); } /* ************************************************************************* */ From cc7ed2d152655365a24a443454800ac5a74ab124 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Mon, 23 Jan 2023 16:30:39 -0800 Subject: [PATCH 02/18] add unit test for 'assignWeights' --- tests/testSubgraphSolver.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 245572896..9a6f2c76b 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -129,6 +129,19 @@ 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) { From a8fa2f4c34cc2c24d1131a2db000ba21d05d1c4d Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Mon, 23 Jan 2023 18:55:56 -0800 Subject: [PATCH 03/18] Factors out 'kruskal' into it's own header and adds tests --- gtsam/base/kruskal-inl.h | 97 +++++++++++++++++ gtsam/base/kruskal.h | 33 ++++++ gtsam/base/tests/testKruskal.cpp | 143 +++++++++++++++++++++++++ gtsam/inference/graph-inl.h | 2 +- gtsam/linear/SubgraphBuilder.cpp | 172 ++++++++++--------------------- gtsam/linear/SubgraphBuilder.h | 14 +-- tests/testSubgraphSolver.cpp | 49 --------- 7 files changed, 329 insertions(+), 181 deletions(-) create mode 100644 gtsam/base/kruskal-inl.h create mode 100644 gtsam/base/kruskal.h create mode 100644 gtsam/base/tests/testKruskal.cpp 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); } From 3b2fe0a585a91e249ae7d0056a7e34c9496eabee Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Tue, 24 Jan 2023 09:50:49 -0800 Subject: [PATCH 04/18] Cleanup unused variables from test --- gtsam/base/tests/testKruskal.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index b189e29f1..18ad77c58 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -55,8 +55,6 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() 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); From 64267a3d8d65f33269b98341dee87f56019eb0db Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Tue, 24 Jan 2023 09:57:12 -0800 Subject: [PATCH 05/18] remove commented code section --- gtsam/base/tests/testKruskal.cpp | 32 -------------------------------- 1 file changed, 32 deletions(-) diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index 18ad77c58..b88fb5301 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -78,22 +78,6 @@ TEST(kruskal, GaussianFactorGraph) 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); @@ -111,22 +95,6 @@ TEST(kruskal, NonlinearFactorGraph) 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); From b83261e2b13fa2781d833d37c4184d72c3db9849 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 19:04:31 -0800 Subject: [PATCH 06/18] WIP: replace instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version --- gtsam/base/kruskal-inl.h | 36 +++++++----- gtsam/slam/lago.cpp | 106 ++++++++++++++++++++++++++++------ gtsam/slam/lago.h | 6 ++ gtsam/slam/tests/testLago.cpp | 58 ++++++++++++++----- 4 files changed, 156 insertions(+), 50 deletions(-) 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)); } /* *************************************************************************** */ From a0ca68a5b707f1b899e8b81e1ef170a47a70b80b Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 19:30:06 -0800 Subject: [PATCH 07/18] replaces all instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version --- gtsam/base/kruskal-inl.h | 126 ++++++++++++++----------------- gtsam/base/kruskal.h | 11 ++- gtsam/base/tests/testKruskal.cpp | 108 +++++++++++++------------- gtsam/slam/tests/testLago.cpp | 47 +++++++----- 4 files changed, 143 insertions(+), 149 deletions(-) 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)); } From 4be4b97b210d6964485eaa13fc88227f974bf042 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 19:37:25 -0800 Subject: [PATCH 08/18] Some cleanup --- gtsam/slam/lago.cpp | 95 ++++++++++++++++----------------------------- gtsam/slam/lago.h | 3 +- 2 files changed, 35 insertions(+), 63 deletions(-) diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 0732c2a32..90909a43f 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -227,90 +227,69 @@ static PredecessorMap findOdometricPath( } /*****************************************************************************/ -PredecessorMap findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){ - +PredecessorMap findMinimumSpanningTree( + const NonlinearFactorGraph& pose2Graph) { // Compute the minimum spanning tree - const FastMap forwardOrdering = Ordering::Natural(pose2Graph).invert(); + const FastMap forwardOrdering = + Ordering::Natural(pose2Graph).invert(); const auto edgeWeights = std::vector(pose2Graph.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(pose2Graph, forwardOrdering, edgeWeights); + 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 + // Create a PredecessorMap 'predecessorMap' such that: + // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in + // the spanning tree 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}); - } + 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, - bool useOdometricPath) { + bool useOdometricPath) { gttic(lago_computeOrientations); - // Find a minimum spanning tree PredecessorMap tree; + // Find a minimum spanning tree if (useOdometricPath) tree = findOdometricPath(pose2Graph); - else - { + else { 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; - vector spanningTreeIds; // ids of between factors forming the spanning tree T - vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + vector + spanningTreeIds; // ids of between factors forming the spanning tree T + 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); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, - chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph( + spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -320,21 +299,13 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, /* ************************************************************************* */ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, - bool useOdometricPath) { - + bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph(graph); // Get orientations from relative orientation measurements - 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; + return computeOrientations(pose2Graph, useOdometricPath); } /* ************************************************************************* */ diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index ca6a2c89b..39212ed1b 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -71,7 +71,8 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( 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. + * Note: all 'Pose2' Between factors are given equal weightage. + * Note: assumes all the edges (factors) are Between factors */ GTSAM_EXPORT PredecessorMap findMinimumSpanningTree( const NonlinearFactorGraph& pose2Graph); From e29a0d35edec3c3c78aa8b06d9f5d87a27fca59b Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 19:49:05 -0800 Subject: [PATCH 09/18] Removes import 'graph.h' inside 'lago.h' --- gtsam/base/kruskal.h | 1 - gtsam/slam/lago.cpp | 24 ++++++++++++------------ gtsam/slam/lago.h | 10 +++++----- gtsam/slam/tests/testLago.cpp | 13 ++++--------- 4 files changed, 21 insertions(+), 27 deletions(-) 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 From 3f65cfb21879d94516afadf0df6555aa9d26d6c2 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 19:58:58 -0800 Subject: [PATCH 10/18] removes unused argument 'ordering' from 'kruskal' --- gtsam/base/kruskal-inl.h | 3 +-- gtsam/base/kruskal.h | 1 - gtsam/linear/SubgraphBuilder.cpp | 5 ++--- gtsam/linear/SubgraphBuilder.h | 1 - gtsam/slam/lago.cpp | 4 +--- 5 files changed, 4 insertions(+), 10 deletions(-) diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 73dcf9298..7f1a30151 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -52,7 +52,6 @@ inline std::vector sortedIndices(const std::vector &src) { /****************************************************************/ 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); @@ -66,7 +65,7 @@ std::vector kruskal(const Graph &fg, std::vector treeIndices; treeIndices.reserve(n - 1); - // Initialize disjoint-set forest to keep track of merged 'blah'. + // Initialize disjoint-set forest to keep track of merged Keys. DSFMap dsf; // Loop over all edges in order of increasing weight. diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h index f8f648018..8f5d75966 100644 --- a/gtsam/base/kruskal.h +++ b/gtsam/base/kruskal.h @@ -24,7 +24,6 @@ namespace gtsam::utils { template std::vector kruskal(const FactorGraph &fg, - const FastMap &ordering, const std::vector &weights); } diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 0d899cb11..b787a87ea 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -230,7 +230,7 @@ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, return bfs(gfg); break; case SubgraphBuilderParameters::KRUSKAL: - return kruskal(gfg, ordering, weights); + return kruskal(gfg, weights); break; default: std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; @@ -306,9 +306,8 @@ vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /****************************************************************/ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, const vector &weights) const { - return utils::kruskal(gfg, ordering, weights); + return utils::kruskal(gfg, weights); } /****************************************************************/ diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index 34b397cc9..65e288c0d 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -171,7 +171,6 @@ class GTSAM_EXPORT SubgraphBuilder { std::vector sample(const std::vector &weights, const size_t t) const; std::vector kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, const std::vector &weights) const; Weights weights(const GaussianFactorGraph &gfg) const ; diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index ab8c7bea2..d63d2b8ea 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -230,11 +230,9 @@ static PredecessorMap findOdometricPath( 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); + utils::kruskal(pose2Graph, edgeWeights); // Create a PredecessorMap 'predecessorMap' such that: // predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in From b5a1e9699a8d76b8736be6adf95d0b4ff11528f4 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:11:19 -0800 Subject: [PATCH 11/18] fix testKruskal --- gtsam/base/tests/testKruskal.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index 5fe261d34..9d808670a 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -67,12 +67,13 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() { TEST(kruskal, GaussianFactorGraph) { using namespace gtsam; + // Create factor graph. const auto g = makeTestGaussianFactorGraph(); - const FastMap forward_ordering = Ordering::Natural(g).invert(); + // Assign weights to all the edges in the graph. const auto weights = std::vector(g.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); + const auto mstEdgeIndices = utils::kruskal(g, weights); EXPECT(mstEdgeIndices[0] == 0); EXPECT(mstEdgeIndices[1] == 1); @@ -83,12 +84,13 @@ TEST(kruskal, GaussianFactorGraph) { TEST(kruskal, NonlinearFactorGraph) { using namespace gtsam; + // Create factor graph. const auto g = makeTestNonlinearFactorGraph(); - const FastMap forward_ordering = Ordering::Natural(g).invert(); + // Assign weights to all the edges in the graph. const auto weights = std::vector(g.size(), 1.0); - const auto mstEdgeIndices = utils::kruskal(g, forward_ordering, weights); + const auto mstEdgeIndices = utils::kruskal(g, weights); EXPECT(mstEdgeIndices[0] == 0); EXPECT(mstEdgeIndices[1] == 1); From 3b745d39e83871bd3a3a8f102fa8755da0ebfbb5 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:23:33 -0800 Subject: [PATCH 12/18] remove extraneous argument 'ordering' from SubGraphBuilder::buildTree --- gtsam/linear/SubgraphBuilder.cpp | 3 +-- gtsam/linear/SubgraphBuilder.h | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index b787a87ea..06d666f45 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -219,7 +219,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator( /****************************************************************/ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, - const FastMap &ordering, const vector &weights) const { const SubgraphBuilderParameters &p = parameters_; switch (p.skeletonType) { @@ -336,7 +335,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { vector weights = this->weights(gfg); // Build spanning tree. - const vector tree = buildTree(gfg, forward_ordering, weights); + const vector tree = buildTree(gfg, weights); if (tree.size() != n - 1) { throw std::runtime_error( "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph"); diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index 65e288c0d..c4f8702ea 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -163,7 +163,6 @@ class GTSAM_EXPORT SubgraphBuilder { private: std::vector buildTree(const GaussianFactorGraph &gfg, - const FastMap &ordering, const std::vector &weights) const; std::vector unary(const GaussianFactorGraph &gfg) const; std::vector natural_chain(const GaussianFactorGraph &gfg) const; From b20827b2dc18cf0174618e75cd34d08134518106 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:36:11 -0800 Subject: [PATCH 13/18] adds docstring --- gtsam/base/kruskal-inl.h | 6 ++++++ gtsam/base/kruskal.h | 16 +++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 7f1a30151..50cf10260 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -53,6 +53,12 @@ inline std::vector sortedIndices(const std::vector &src) { template std::vector kruskal(const Graph &fg, const std::vector &weights) { + if (fg.size() != weights.size()) { + throw std::runtime_error( + "kruskal() failure: fg.size() != weights.size(), all factors need to " + "assigned a weight"); + } + // Create an index from variables to factor indices. const VariableIndex variableIndex(fg); diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h index 8f5d75966..4b7493685 100644 --- a/gtsam/base/kruskal.h +++ b/gtsam/base/kruskal.h @@ -10,9 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file SubgraphBuilder-inl.h + * @file kruskal.h * @date Dec 31, 2009 - * @author Frank Dellaert, Yong-Dian Jian + * @author Frank Dellaert + * @author Yong-Dian Jian + * @author Ankur Roy Chowdhury */ #pragma once @@ -22,9 +24,17 @@ #include namespace gtsam::utils { +/** + * Compute the minimum spanning tree (MST) using Kruskal's algorithm + * @param fg Factor graph + * @param weights Weights of the edges/factors in the factor graph + * @return Edge/factor indices spanning the MST + * @note Only binary factors are considered while constructing the spanning tree + * @note The indices of 'weights' should match the indices of the edges in the factor graph + */ template std::vector kruskal(const FactorGraph &fg, const std::vector &weights); -} +} // namespace gtsam::utils #include \ No newline at end of file From c8124ec944d5aaa471702b281a4013a7a958a193 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:48:30 -0800 Subject: [PATCH 14/18] Fix formatting --- gtsam/linear/SubgraphBuilder.cpp | 72 +++++++++++++------------------- 1 file changed, 30 insertions(+), 42 deletions(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 06d666f45..27eec198b 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -365,50 +365,38 @@ SubgraphBuilder::Weights SubgraphBuilder::weights( 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; + 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; + case SubgraphBuilderParameters::RANDOM: + weights.push_back(std::rand() % 100 + 1.0); + break; - default: - throw std::invalid_argument( - "utils::assign_weights: undefined weight scheme "); - break; + default: + throw std::invalid_argument( + "SubgraphBuilder::weights(): undefined weight scheme "); + break; } } return weights; From d5ec65e320fa074a7234b83ab28b3018ea25d3e3 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:49:34 -0800 Subject: [PATCH 15/18] minor edit --- gtsam/base/kruskal-inl.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 50cf10260..25932301a 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -12,7 +12,6 @@ /** * @file SubgraphBuilder-inl.h * @date Dec 31, 2009 - * @date Jan 23, 2023 * @author Frank Dellaert, Yong-Dian Jian */ From c3045c097d06e260e51436bb08a290134393b894 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:54:08 -0800 Subject: [PATCH 16/18] cleanup some formatting and comments --- gtsam/base/kruskal-inl.h | 2 +- gtsam/inference/graph-inl.h | 2 +- gtsam/linear/SubgraphBuilder.cpp | 20 ++++++++++---------- gtsam/linear/SubgraphBuilder.h | 5 ++--- gtsam/slam/lago.cpp | 3 +-- 5 files changed, 15 insertions(+), 17 deletions(-) diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 25932301a..4b66a905f 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SubgraphBuilder-inl.h + * @file kruskal-inl.h * @date Dec 31, 2009 * @author Frank Dellaert, Yong-Dian Jian */ diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 49668fc59..ceae2e3ab 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 27eec198b..bf9343c81 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -362,44 +362,44 @@ SubgraphBuilder::Weights SubgraphBuilder::weights( using Weights = std::vector; const size_t m = gfg.size(); - Weights weights; - weights.reserve(m); + Weights weight; + weight.reserve(m); for (const GaussianFactor::shared_ptr &gf : gfg) { switch (parameters_.skeletonWeight) { case SubgraphBuilderParameters::EQUAL: - weights.push_back(1.0); + weight.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()); + weight.push_back(jf->getb().norm()); } else if (HessianFactor::shared_ptr hf = std::dynamic_pointer_cast(gf)) { - weights.push_back(hf->linearTerm().norm()); + weight.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())); + weight.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())); + weight.push_back(std::sqrt(hf->information().squaredNorm())); } } break; case SubgraphBuilderParameters::RANDOM: - weights.push_back(std::rand() % 100 + 1.0); + weight.push_back(std::rand() % 100 + 1.0); break; default: throw std::invalid_argument( - "SubgraphBuilder::weights(): undefined weight scheme "); + "SubgraphBuilder::weights: undefined weight scheme "); break; } } - return weights; + return weight; } /*****************************************************************************/ diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index c4f8702ea..606f13e9b 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -167,12 +167,11 @@ class GTSAM_EXPORT SubgraphBuilder { std::vector unary(const GaussianFactorGraph &gfg) const; std::vector natural_chain(const GaussianFactorGraph &gfg) const; std::vector bfs(const GaussianFactorGraph &gfg) const; - std::vector sample(const std::vector &weights, - const size_t t) const; std::vector kruskal(const GaussianFactorGraph &gfg, const std::vector &weights) const; + std::vector sample(const std::vector &weights, + const size_t t) const; Weights weights(const GaussianFactorGraph &gfg) const ; - SubgraphBuilderParameters parameters_; }; diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index d63d2b8ea..5407e60a2 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -270,9 +270,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, // Find a minimum spanning tree if (useOdometricPath) tree = findOdometricPath(pose2Graph); - else { + else tree = findMinimumSpanningTree(pose2Graph); - } // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; From b2fe6faa4c84ac415ac61f3d25296334d0d9599f Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sat, 4 Feb 2023 20:56:01 -0800 Subject: [PATCH 17/18] Removes extra space --- gtsam/linear/SubgraphBuilder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index 606f13e9b..60dfefe24 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -171,7 +171,7 @@ class GTSAM_EXPORT SubgraphBuilder { const std::vector &weights) const; std::vector sample(const std::vector &weights, const size_t t) const; - Weights weights(const GaussianFactorGraph &gfg) const ; + Weights weights(const GaussianFactorGraph &gfg) const; SubgraphBuilderParameters parameters_; }; From fce0e327389891c901a8dc1f85ebcfb6689fa040 Mon Sep 17 00:00:00 2001 From: Ankur Roy Chowdhury Date: Sun, 5 Feb 2023 10:33:24 -0800 Subject: [PATCH 18/18] Address review comments --- gtsam/base/kruskal-inl.h | 57 +++++++++++++++++++------------- gtsam/base/kruskal.h | 7 ++-- gtsam/linear/SubgraphBuilder.cpp | 1 - gtsam/linear/SubgraphBuilder.h | 2 +- gtsam/slam/lago.h | 6 ++-- tests/testSubgraphSolver.cpp | 1 - 6 files changed, 42 insertions(+), 32 deletions(-) diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h index 4b66a905f..d610541a0 100644 --- a/gtsam/base/kruskal-inl.h +++ b/gtsam/base/kruskal-inl.h @@ -12,7 +12,9 @@ /** * @file kruskal-inl.h * @date Dec 31, 2009 - * @author Frank Dellaert, Yong-Dian Jian + * @author Frank Dellaert + * @author Yong-Dian Jian + * @author Ankur Roy Chowdhury */ #pragma once @@ -20,37 +22,37 @@ #include #include #include -#include #include -#include +#include +#include #include 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 'weights' in increasing order and return the sorted order of its + * indices. */ +inline std::vector sortedIndices(const std::vector &weights) { + // Get the number of elements in the 'weights' vector. + const size_t n = weights.size(); - /* sort */ - std::stable_sort(tmp.begin(), tmp.end()); + // Create a vector of 'indices'. + std::vector indices(n); + std::iota(indices.begin(), indices.end(), 0); - /* copy back */ - std::vector idx; - idx.reserve(n); - for (size_t i = 0; i < n; i++) { - idx.push_back(tmp[i].first); - } - return idx; + // Sort the 'indices' based on the values in 'weights'. + std::stable_sort(indices.begin(), indices.end(), + [&weights](const size_t i0, const size_t i1) { + return weights[i0] < weights[i1]; + }); + + return indices; } /****************************************************************/ -template -std::vector kruskal(const Graph &fg, +template +std::vector kruskal(const FactorGraph &fg, const std::vector &weights) { if (fg.size() != weights.size()) { throw std::runtime_error( @@ -61,7 +63,7 @@ std::vector kruskal(const Graph &fg, // Create an index from variables to factor indices. const VariableIndex variableIndex(fg); - // Get indices in sort-order of the weights + // Get indices in sort-order of the weights. const std::vector sortedIndices = gtsam::utils::sortedIndices(weights); @@ -81,11 +83,20 @@ std::vector kruskal(const Graph &fg, // Ignore non-binary edges. if (factor->size() != 2) continue; - auto u = dsf.find(factor->front()), v = dsf.find(factor->back()); - auto loop = (u == v); + // Find the representatives of the sets for both the Keys in the binary + // factor. + const auto u = dsf.find(factor->front()), v = dsf.find(factor->back()); + + // Check if there is a potential loop. + const bool loop = (u == v); if (!loop) { + // Merge the sets. dsf.merge(u, v); + + // Add the current index to the tree. treeIndices.push_back(index); + + // Break if all the Keys have been added to the tree. if (++count == n - 1) break; } } diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h index 4b7493685..058c22435 100644 --- a/gtsam/base/kruskal.h +++ b/gtsam/base/kruskal.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include @@ -32,9 +33,9 @@ namespace gtsam::utils { * @note Only binary factors are considered while constructing the spanning tree * @note The indices of 'weights' should match the indices of the edges in the factor graph */ -template -std::vector kruskal(const FactorGraph &fg, +template +std::vector kruskal(const FactorGraph &fg, const std::vector &weights); } // namespace gtsam::utils -#include \ No newline at end of file +#include diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index bf9343c81..40121d7cc 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -359,7 +359,6 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { /****************************************************************/ SubgraphBuilder::Weights SubgraphBuilder::weights( const GaussianFactorGraph &gfg) const { - using Weights = std::vector; const size_t m = gfg.size(); Weights weight; diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index 60dfefe24..aafba9306 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -185,4 +185,4 @@ GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, std::pair splitFactorGraph( const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index 0dfccdd58..5661a7cba 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -70,9 +70,9 @@ 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' Between factors are given equal weightage. - * Note: assumes all the edges (factors) are Between factors +/** Given a "pose2" factor graph, find its 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( const NonlinearFactorGraph& pose2Graph); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 616c77062..69b5fe5f9 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -129,7 +129,6 @@ TEST( SubgraphSolver, constructor3 ) DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */