12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Factors out 'kruskal' into it's own header and adds tests

release/4.3a0
Ankur Roy Chowdhury 2023-01-23 18:55:56 -08:00
parent cc7ed2d152
commit a8fa2f4c34
7 changed files with 329 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
gtsam/base/kruskal-inl.h Normal file
View File

@ -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 <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
#include <memory>
#include <vector>
namespace gtsam::utils
{
/*****************************************************************************/
/* sort the container and return permutation index with default comparator */
template <typename Container>
static std::vector<size_t> sort_idx(const Container &src)
{
typedef typename Container::value_type T;
const size_t n = src.size();
std::vector<std::pair<size_t, T>> 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<size_t> idx;
idx.reserve(n);
for (size_t i = 0; i < n; i++)
{
idx.push_back(tmp[i].first);
}
return idx;
}
/****************************************************************/
template <class Graph>
std::vector<size_t> kruskal(const Graph &fg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights)
{
const VariableIndex variableIndex(fg);
const size_t n = variableIndex.size();
const std::vector<size_t> sortedIndices = sort_idx(weights);
/* initialize buffer */
std::vector<size_t> 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

33
gtsam/base/kruskal.h Normal file
View File

@ -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 <gtsam/base/FastMap.h>
#include <vector>
namespace gtsam::utils
{
template <class FactorGraph>
std::vector<size_t> kruskal(const FactorGraph &fg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights);
}
#include <gtsam/base/kruskal-inl.h>

143
gtsam/base/tests/testKruskal.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/kruskal.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <vector>
#include <list>
#include <memory>
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<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
const auto weights = std::vector<double>(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<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
const auto weights = std::vector<double>(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);
}
/* ************************************************************************* */

2
gtsam/inference/graph-inl.h
View File

@ -234,7 +234,7 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// Convert to a graph that boost understands
SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
// find minimum spanning tree
// // find minimum spanning tree
std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
prim_minimum_spanning_tree(g, &p_map[0]);

172
gtsam/linear/SubgraphBuilder.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/base/kruskal.h>
#include <boost/algorithm/string.hpp>
#include <boost/archive/text_iarchive.hpp>
@ -52,28 +53,6 @@ using std::vector;
namespace gtsam {
/*****************************************************************************/
/* sort the container and return permutation index with default comparator */
template <typename Container>
static vector<size_t> sort_idx(const Container &src) {
typedef typename Container::value_type T;
const size_t n = src.size();
vector<std::pair<size_t, T> > 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<size_t> idx;
idx.reserve(n);
for (size_t i = 0; i < n; i++) {
idx.push_back(tmp[i].first);
}
return idx;
}
/****************************************************************************/
Subgraph::Subgraph(const vector<size_t> &indices) {
edges_.reserve(indices.size());
@ -238,101 +217,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(
return "UNKNOWN";
}
/****************************************************************/
std::vector<double> utils::assignWeights(const GaussianFactorGraph &gfg, const SubgraphBuilderParameters::SkeletonWeight &skeletonWeight)
{
using Weights = std::vector<double>;
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<JacobianFactor>(gf))
{
weights.push_back(jf->getb().norm());
}
else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<HessianFactor>(gf))
{
weights.push_back(hf->linearTerm().norm());
}
}
break;
case SubgraphBuilderParameters::LHS_FNORM:
{
if (JacobianFactor::shared_ptr jf =
std::dynamic_pointer_cast<JacobianFactor>(gf))
{
weights.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<HessianFactor>(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<size_t> utils::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights)
{
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> sortedIndices = sort_idx(weights);
/* initialize buffer */
vector<size_t> 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<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &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<double>;
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<JacobianFactor>(gf))
{
weights.push_back(jf->getb().norm());
}
else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<HessianFactor>(gf))
{
weights.push_back(hf->linearTerm().norm());
}
}
break;
case SubgraphBuilderParameters::LHS_FNORM:
{
if (JacobianFactor::shared_ptr jf =
std::dynamic_pointer_cast<JacobianFactor>(gf))
{
weights.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<HessianFactor>(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;
}
/*****************************************************************************/

14
gtsam/linear/SubgraphBuilder.h
View File

@ -149,16 +149,6 @@ struct GTSAM_EXPORT SubgraphBuilderParameters {
static std::string augmentationWeightTranslator(AugmentationWeight w);
};
namespace utils
{
std::vector<double> assignWeights(const GaussianFactorGraph &gfg,
const SubgraphBuilderParameters::SkeletonWeight &skeleton_weight);
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &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<size_t> buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
@ -200,4 +188,4 @@ GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
} // namespace gtsam
} // namespace gtsam

49
tests/testSubgraphSolver.cpp
View File

@ -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<Key, size_t> 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); }