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

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

@ -18,29 +18,24 @@
#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++) for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
tmp.emplace_back(i, src[i]);
/* sort */ /* sort */
std::stable_sort(tmp.begin(), tmp.end()); std::stable_sort(tmp.begin(), tmp.end());
@ -48,24 +43,23 @@ namespace gtsam::utils
/* 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> template <class Graph>
std::vector<size_t> kruskal(const Graph &fg, std::vector<size_t> kruskal(const Graph &fg,
const FastMap<Key, size_t> &ordering, const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) const std::vector<double> &weights) {
{
// Create an index from variables to factor indices. // Create an index from variables to factor indices.
const VariableIndex variableIndex(fg); const VariableIndex variableIndex(fg);
// Get indices in sort-order of the weights // 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. // Create a vector to hold MST indices.
const size_t n = variableIndex.size(); const size_t n = variableIndex.size();
@ -77,25 +71,21 @@ namespace gtsam::utils
// Loop over all edges in order of increasing weight. // Loop over all edges in order of increasing weight.
size_t count = 0; size_t count = 0;
for (const size_t index : sortedIndices) for (const size_t index : sortedIndices) {
{
const auto factor = fg[index]; const auto factor = fg[index];
// Ignore non-binary edges. // Ignore non-binary edges.
if (factor->size() != 2) if (factor->size() != 2) continue;
continue;
auto u = dsf.find(factor->front()), v = dsf.find(factor->back()); auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
auto loop = (u == v); auto loop = (u == v);
if (!loop) if (!loop) {
{
dsf.merge(u, v); dsf.merge(u, v);
treeIndices.push_back(index); treeIndices.push_back(index);
if (++count == n - 1) if (++count == n - 1) break;
break;
} }
} }
return treeIndices; return treeIndices;
} }
} // namespace gtsam::utils } // namespace gtsam::utils

7
gtsam/base/kruskal.h
View File

@ -22,10 +22,9 @@
#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);
} }

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

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