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

replaces all instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version

release/4.3a0
Ankur Roy Chowdhury 2023-02-04 19:30:06 -08:00
parent b83261e2b1
commit a0ca68a5b7
4 changed files with 143 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
gtsam/base/kruskal-inl.h
View File

@ -18,84 +18,74 @@
#pragma once #pragma once
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/DSFMap.h> #include <gtsam/base/DSFMap.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <memory> #include <memory>
#include <vector> #include <vector>
namespace gtsam::utils namespace gtsam::utils {
{
/*****************************************************************************/ /*****************************************************************************/
/* sort the container and return permutation index with default comparator */ /* sort the container and return permutation index with default comparator */
inline std::vector<size_t> sortedIndices(const std::vector<double> &src) inline std::vector<size_t> sortedIndices(const std::vector<double> &src) {
{ const size_t n = src.size();
const size_t n = src.size(); std::vector<std::pair<size_t, double>> tmp;
std::vector<std::pair<size_t, double>> tmp; tmp.reserve(n);
tmp.reserve(n); for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
for (size_t i = 0; i < n; i++)
tmp.emplace_back(i, src[i]);
/* sort */ /* sort */
std::stable_sort(tmp.begin(), tmp.end()); std::stable_sort(tmp.begin(), tmp.end());
/* copy back */ /* copy back */
std::vector<size_t> idx; std::vector<size_t> idx;
idx.reserve(n); idx.reserve(n);
for (size_t i = 0; i < n; i++) for (size_t i = 0; i < n; i++) {
{ idx.push_back(tmp[i].first);
idx.push_back(tmp[i].first); }
} return idx;
return idx; }
/****************************************************************/
template <class Graph>
std::vector<size_t> kruskal(const Graph &fg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) {
// Create an index from variables to factor indices.
const VariableIndex variableIndex(fg);
// Get indices in sort-order of the weights
const std::vector<size_t> sortedIndices =
gtsam::utils::sortedIndices(weights);
// Create a vector to hold MST indices.
const size_t n = variableIndex.size();
std::vector<size_t> treeIndices;
treeIndices.reserve(n - 1);
// Initialize disjoint-set forest to keep track of merged 'blah'.
DSFMap<Key> 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
template <class Graph>
std::vector<size_t> kruskal(const Graph &fg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights)
{
// Create an index from variables to factor indices.
const VariableIndex variableIndex(fg);
// Get indices in sort-order of the weights
const std::vector<size_t> sortedIndices = gtsam::utils::sortedIndices(weights);
// Create a vector to hold MST indices.
const size_t n = variableIndex.size();
std::vector<size_t> treeIndices;
treeIndices.reserve(n - 1);
// Initialize disjoint-set forest to keep track of merged 'blah'.
DSFMap<Key> 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

11
gtsam/base/kruskal.h
View File

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

108
gtsam/base/tests/testKruskal.cpp
View File

@ -18,92 +18,86 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/kruskal.h> #include <gtsam/base/kruskal.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.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 <list>
#include <memory> #include <memory>
#include <vector>
gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
{ using namespace gtsam;
using namespace gtsam; using namespace symbol_shorthand;
using namespace symbol_shorthand;
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
Matrix I = I_2x2; Matrix I = I_2x2;
Vector2 b(0, 0); Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); 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(2), I, b, model);
gfg += JacobianFactor(X(1), I, X(3), 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(1), I, X(4), I, b, model);
gfg += JacobianFactor(X(2), I, X(3), 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(2), I, X(4), I, b, model);
gfg += JacobianFactor(X(3), I, X(4), I, b, model); gfg += JacobianFactor(X(3), I, X(4), I, b, model);
return gfg; return gfg;
} }
gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
{ using namespace gtsam;
using namespace gtsam; using namespace symbol_shorthand;
using namespace symbol_shorthand;
NonlinearFactorGraph nfg; NonlinearFactorGraph nfg;
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
nfg += BetweenFactor(X(1), X(2), Rot3(), model); nfg += BetweenFactor(X(1), X(2), Rot3(), model);
nfg += BetweenFactor(X(1), X(3), Rot3(), model); nfg += BetweenFactor(X(1), X(3), Rot3(), model);
nfg += BetweenFactor(X(1), X(4), Rot3(), model); nfg += BetweenFactor(X(1), X(4), Rot3(), model);
nfg += BetweenFactor(X(2), X(3), Rot3(), model); nfg += BetweenFactor(X(2), X(3), Rot3(), model);
nfg += BetweenFactor(X(2), X(4), Rot3(), model); nfg += BetweenFactor(X(2), X(4), Rot3(), model);
nfg += BetweenFactor(X(3), X(4), Rot3(), model); nfg += BetweenFactor(X(3), X(4), Rot3(), model);
return nfg; return nfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(kruskal, GaussianFactorGraph) TEST(kruskal, GaussianFactorGraph) {
{ using namespace gtsam;
using namespace gtsam;
const auto g = makeTestGaussianFactorGraph(); const auto g = makeTestGaussianFactorGraph();
const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert(); const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
const auto weights = std::vector<double>(g.size(), 1.0); const auto weights = std::vector<double>(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[0] == 0);
EXPECT(mstEdgeIndices[1] == 1); EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2); EXPECT(mstEdgeIndices[2] == 2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(kruskal, NonlinearFactorGraph) TEST(kruskal, NonlinearFactorGraph) {
{ using namespace gtsam;
using namespace gtsam;
const auto g = makeTestNonlinearFactorGraph(); const auto g = makeTestNonlinearFactorGraph();
const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert(); const FastMap<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
const auto weights = std::vector<double>(g.size(), 1.0); const auto weights = std::vector<double>(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[0] == 0);
EXPECT(mstEdgeIndices[1] == 1); EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2); EXPECT(mstEdgeIndices[2] == 2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() int main() {
{ TestResult tr;
TestResult tr; return TestRegistry::runAllTests(tr);
return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

47
gtsam/slam/tests/testLago.cpp
View File

@ -108,30 +108,41 @@ TEST( Lago, checkSTandChords ) {
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, orientationsOverSpanningTree ) { TEST(Lago, orientationsOverSpanningTree) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, auto gPlus = initialize::buildPoseGraph<Pose2>(g);
BetweenFactor<Pose2> >(g); PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
// check the tree structure // 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[x1]);
EXPECT_LONGS_EQUAL(x0, tree[x2]); EXPECT_LONGS_EQUAL(x1, tree[x2]);
EXPECT_LONGS_EQUAL(x0, tree[x3]); EXPECT_LONGS_EQUAL(x2, tree[x3]);
lago::key2doubleMap expected; lago::key2doubleMap expected;
expected[x0]= 0; expected[x0] = 0;
expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x1] = M_PI / 2; // edges traversed: x0->x1
expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x2] = M_PI; // edges traversed: x0->x1->x2
expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) expected[x3] = 3 * M_PI / 2; // edges traversed: x0->x1->x2->x3
lago::key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t>
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T spanningTreeIds; // ids of between factors forming the spanning tree T
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); vector<size_t>
chordsIds; // ids of between factors corresponding to chordsIds wrt T
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree,
gPlus);
lago::key2doubleMap actual; lago::key2doubleMap actual;
actual = lago::computeThetasToRoot(deltaThetaMap, tree); 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[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@ -141,24 +152,24 @@ TEST( Lago, orientationsOverSpanningTree ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, regularizedMeasurements ) { TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simpleLago::graph(); NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, auto gPlus = initialize::buildPoseGraph<Pose2>(g);
BetweenFactor<Pose2> >(g); PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
lago::key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T vector<size_t> 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); 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<Matrix,Vector> actualAb = lagoGraph.jacobian(); std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) // 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(); 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 // this is the whitened error, so we multiply by the std to unwhiten
actual = 0.1 * actual; actual = 0.1 * actual;
// Expected regularized measurements (same for the spanning tree, corrected for the chordsIds) // 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)); EXPECT(assert_equal(expected, actual, 1e-6));
} }