replaces all instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version
Ankur Roy Chowdhury
parent
b83261e2b1
commit
a0ca68a5b7
|
|
@ -18,84 +18,74 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/DSFMap.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam::utils
|
||||
{
|
||||
namespace gtsam::utils {
|
||||
|
||||
/*****************************************************************************/
|
||||
/* sort the container and return permutation index with default comparator */
|
||||
inline std::vector<size_t> sortedIndices(const std::vector<double> &src)
|
||||
{
|
||||
const size_t n = src.size();
|
||||
std::vector<std::pair<size_t, double>> 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<size_t> sortedIndices(const std::vector<double> &src) {
|
||||
const size_t n = src.size();
|
||||
std::vector<std::pair<size_t, double>> 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<size_t> idx;
|
||||
idx.reserve(n);
|
||||
for (size_t i = 0; i < n; i++)
|
||||
{
|
||||
idx.push_back(tmp[i].first);
|
||||
}
|
||||
return idx;
|
||||
/* 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) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
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
|
||||
} // namespace gtsam::utils
|
||||
|
|
|
|||
|
|
@ -22,12 +22,11 @@
|
|||
|
||||
#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);
|
||||
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>
|
||||
|
|
@ -18,92 +18,86 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.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/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>
|
||||
#include <vector>
|
||||
|
||||
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<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
|
||||
const auto weights = std::vector<double>(g.size(), 1.0);
|
||||
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);
|
||||
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<Key, size_t> forward_ordering = Ordering::Natural(g).invert();
|
||||
const auto weights = std::vector<double>(g.size(), 1.0);
|
||||
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);
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -108,30 +108,41 @@ TEST( Lago, checkSTandChords ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, orientationsOverSpanningTree ) {
|
||||
TEST(Lago, orientationsOverSpanningTree) {
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
auto gPlus = initialize::buildPoseGraph<Pose2>(g);
|
||||
PredecessorMap<Key> 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<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
|
||||
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
|
||||
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
|
||||
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<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
auto gPlus = initialize::buildPoseGraph<Pose2>(g);
|
||||
PredecessorMap<Key> tree = lago::findMinimumSpanningTree(gPlus);
|
||||
|
||||
lago::key2doubleMap deltaThetaMap;
|
||||
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
|
||||
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<Matrix,Vector> 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));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue