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

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

@ -18,29 +18,24 @@
#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)
{
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]);
for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
/* sort */
std::stable_sort(tmp.begin(), tmp.end());
@ -48,8 +43,7 @@ namespace gtsam::utils
/* copy back */
std::vector<size_t> idx;
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);
}
return idx;
@ -59,13 +53,13 @@ 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)
{
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);
const std::vector<size_t> sortedIndices =
gtsam::utils::sortedIndices(weights);
// Create a vector to hold MST indices.
const size_t n = variableIndex.size();
@ -77,22 +71,18 @@ namespace gtsam::utils
// Loop over all edges in order of increasing weight.
size_t count = 0;
for (const size_t index : sortedIndices)
{
for (const size_t index : sortedIndices) {
const auto factor = fg[index];
// Ignore non-binary edges.
if (factor->size() != 2)
continue;
if (factor->size() != 2) continue;
auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
auto loop = (u == v);
if (!loop)
{
if (!loop) {
dsf.merge(u, v);
treeIndices.push_back(index);
if (++count == n - 1)
break;
if (++count == n - 1) break;
}
}
return treeIndices;

3
gtsam/base/kruskal.h
View File

@ -22,8 +22,7 @@
#include <vector>
namespace gtsam::utils
{
namespace gtsam::utils {
template <class FactorGraph>
std::vector<size_t> kruskal(const FactorGraph &fg,
const FastMap<Key, size_t> &ordering,

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

@ -18,20 +18,18 @@
#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()
{
gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
using namespace gtsam;
using namespace symbol_shorthand;
@ -49,8 +47,7 @@ gtsam::GaussianFactorGraph makeTestGaussianFactorGraph()
return gfg;
}
gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph()
{
gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
using namespace gtsam;
using namespace symbol_shorthand;
@ -67,8 +64,7 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph()
}
/* ************************************************************************* */
TEST(kruskal, GaussianFactorGraph)
{
TEST(kruskal, GaussianFactorGraph) {
using namespace gtsam;
const auto g = makeTestGaussianFactorGraph();
@ -84,8 +80,7 @@ TEST(kruskal, GaussianFactorGraph)
}
/* ************************************************************************* */
TEST(kruskal, NonlinearFactorGraph)
{
TEST(kruskal, NonlinearFactorGraph) {
using namespace gtsam;
const auto g = makeTestNonlinearFactorGraph();
@ -101,8 +96,7 @@ TEST(kruskal, NonlinearFactorGraph)
}
/* ************************************************************************* */
int main()
{
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

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

@ -110,28 +110,39 @@ TEST( Lago, checkSTandChords ) {
/* *************************************************************************** */
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[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));
}