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

Merge pull request #1440 from ankur-rc/no_boost_lago

release/4.3a0
Frank Dellaert 2023-02-05 13:28:58 -08:00 committed by GitHub
parent 130edcd1ec fce0e32738
commit e8bce8da23
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 401 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* 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 kruskal-inl.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Yong-Dian Jian
* @author Ankur Roy Chowdhury
*/
#pragma once
#include <gtsam/base/DSFMap.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/VariableIndex.h>
#include <algorithm>
#include <numeric>
#include <vector>
namespace gtsam::utils {
/*****************************************************************************/
/* Sort the 'weights' in increasing order and return the sorted order of its
* indices. */
inline std::vector<size_t> sortedIndices(const std::vector<double> &weights) {
// Get the number of elements in the 'weights' vector.
const size_t n = weights.size();
// Create a vector of 'indices'.
std::vector<size_t> indices(n);
std::iota(indices.begin(), indices.end(), 0);
// Sort the 'indices' based on the values in 'weights'.
std::stable_sort(indices.begin(), indices.end(),
[&weights](const size_t i0, const size_t i1) {
return weights[i0] < weights[i1];
});
return indices;
}
/****************************************************************/
template <class FACTOR>
std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
const std::vector<double> &weights) {
if (fg.size() != weights.size()) {
throw std::runtime_error(
"kruskal() failure: fg.size() != weights.size(), all factors need to "
"assigned a weight");
}
// 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 Keys.
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;
// Find the representatives of the sets for both the Keys in the binary
// factor.
const auto u = dsf.find(factor->front()), v = dsf.find(factor->back());
// Check if there is a potential loop.
const bool loop = (u == v);
if (!loop) {
// Merge the sets.
dsf.merge(u, v);
// Add the current index to the tree.
treeIndices.push_back(index);
// Break if all the Keys have been added to the tree.
if (++count == n - 1) break;
}
}
return treeIndices;
}
} // namespace gtsam::utils

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

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* 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 kruskal.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Yong-Dian Jian
* @author Ankur Roy Chowdhury
*/
#pragma once
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/FactorGraph.h>
#include <vector>
namespace gtsam::utils {
/**
* Compute the minimum spanning tree (MST) using Kruskal's algorithm
* @param fg Factor graph
* @param weights Weights of the edges/factors in the factor graph
* @return Edge/factor indices spanning the MST
* @note Only binary factors are considered while constructing the spanning tree
* @note The indices of 'weights' should match the indices of the edges in the factor graph
*/
template <class FACTOR>
std::vector<size_t> kruskal(const FactorGraph<FACTOR> &fg,
const std::vector<double> &weights);
} // namespace gtsam::utils
#include <gtsam/base/kruskal-inl.h>

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

@ -0,0 +1,105 @@
/* ----------------------------------------------------------------------------
* 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/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 <list>
#include <memory>
#include <vector>
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;
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;
// Create factor graph.
const auto g = makeTestGaussianFactorGraph();
// Assign weights to all the edges in the graph.
const auto weights = std::vector<double>(g.size(), 1.0);
const auto mstEdgeIndices = utils::kruskal(g, weights);
EXPECT(mstEdgeIndices[0] == 0);
EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2);
}
/* ************************************************************************* */
TEST(kruskal, NonlinearFactorGraph) {
using namespace gtsam;
// Create factor graph.
const auto g = makeTestNonlinearFactorGraph();
// Assign weights to all the edges in the graph.
const auto weights = std::vector<double>(g.size(), 1.0);
const auto mstEdgeIndices = utils::kruskal(g, weights);
EXPECT(mstEdgeIndices[0] == 0);
EXPECT(mstEdgeIndices[1] == 1);
EXPECT(mstEdgeIndices[2] == 2);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

56
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());
@ -240,7 +219,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(
/****************************************************************/
vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeletonType) {
@ -251,7 +229,7 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, weights);
return kruskal(gfg, weights);
break;
default:
std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
@ -327,33 +305,8 @@ vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
/****************************************************************/
vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const vector<double> &weights) const {
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;
return utils::kruskal(gfg, weights);
}
/****************************************************************/
@ -382,7 +335,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
vector<double> weights = this->weights(gfg);
// Build spanning tree.
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
const vector<size_t> tree = buildTree(gfg, weights);
if (tree.size() != n - 1) {
throw std::runtime_error(
"SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
@ -406,6 +359,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(
const GaussianFactorGraph &gfg) const {
const size_t m = gfg.size();
Weights weight;
weight.reserve(m);

2
gtsam/linear/SubgraphBuilder.h
View File

@ -163,13 +163,11 @@ class GTSAM_EXPORT SubgraphBuilder {
private:
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) const;
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights) const;
std::vector<size_t> sample(const std::vector<double> &weights,
const size_t t) const;

102
gtsam/slam/lago.cpp
View File

@ -24,14 +24,20 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/kruskal.h>
#include <boost/math/special_functions.hpp>
#include <iostream>
#include <stack>
using namespace std;
namespace gtsam {
namespace lago {
using initialize::kAnchorKey;
static const Matrix I = I_1x1;
static const Matrix I3 = I_3x3;
@ -49,7 +55,7 @@ static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
* The root is assumed to have orientation zero.
*/
static double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const PredecessorMap& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0;
@ -75,20 +81,19 @@ static double computeThetaToRoot(const Key nodeKey,
/* ************************************************************************* */
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) {
const PredecessorMap& tree) {
key2doubleMap thetaToRootMap;
// Orientation of the roo
thetaToRootMap.insert(pair<Key, double>(initialize::kAnchorKey, 0.0));
// Orientation of the root
thetaToRootMap.emplace(kAnchorKey, 0.0);
// for all nodes in the tree
for(const key2doubleMap::value_type& it: deltaThetaMap) {
for(const auto& [nodeKey, _]: deltaThetaMap) {
// compute the orientation wrt root
Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
const double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap);
thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
thetaToRootMap.emplace(nodeKey, nodeTheta);
}
return thetaToRootMap;
}
@ -97,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
void getSymbolicGraph(
/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
/*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) {
// Get keys for which you want the orientation
size_t id = 0;
@ -161,7 +166,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
GaussianFactorGraph buildLinearOrientationGraph(
const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
const PredecessorMap<Key>& tree) {
const PredecessorMap& tree) {
GaussianFactorGraph lagoGraph;
Vector deltaTheta;
@ -174,7 +179,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
}
// put regularized measurements in the chordsIds
// put regularized measurements in the chords
for(const size_t& factorId: chordsIds) {
const KeyVector& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
@ -190,16 +195,16 @@ GaussianFactorGraph buildLinearOrientationGraph(
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
}
// prior on the anchor orientation
lagoGraph.add(initialize::kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
lagoGraph.add(kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
return lagoGraph;
}
/* ************************************************************************* */
static PredecessorMap<Key> findOdometricPath(
static PredecessorMap findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree;
Key minKey = initialize::kAnchorKey; // this initialization does not matter
PredecessorMap tree;
Key minKey = kAnchorKey; // this initialization does not matter
bool minUnassigned = true;
for(const std::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
@ -211,42 +216,77 @@ static PredecessorMap<Key> findOdometricPath(
minUnassigned = false;
}
if (key2 - key1 == 1) { // consecutive keys
tree.insert(key2, key1);
tree.emplace(key2, key1);
if (key1 < minKey)
minKey = key1;
}
}
tree.insert(minKey, initialize::kAnchorKey);
tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
tree.emplace(minKey, kAnchorKey);
tree.emplace(kAnchorKey, kAnchorKey); // root
return tree;
}
/*****************************************************************************/
PredecessorMap findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph) {
// Compute the minimum spanning tree
const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0);
const auto mstEdgeIndices =
utils::kruskal(pose2Graph, edgeWeights);
// Create a PredecessorMap 'predecessorMap' such that:
// predecessorMap[key2] = key1, where key1 is the 'parent' node for key2 in
// the spanning tree
PredecessorMap predecessorMap;
std::map<Key, bool> visitationMap;
std::stack<std::pair<Key, Key>> stack;
stack.push({kAnchorKey, kAnchorKey});
while (!stack.empty()) {
auto [u, parent] = stack.top();
stack.pop();
if (visitationMap[u]) continue;
visitationMap[u] = true;
predecessorMap[u] = parent;
for (const auto& edgeIdx : mstEdgeIndices) {
const auto v = pose2Graph[edgeIdx]->front();
const auto w = pose2Graph[edgeIdx]->back();
if ((v == u || w == u) && !visitationMap[v == u ? w : v]) {
stack.push({v == u ? w : v, u});
}
}
}
return predecessorMap;
}
/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) {
bool useOdometricPath) {
gttic(lago_computeOrientations);
PredecessorMap tree;
// Find a minimum spanning tree
PredecessorMap<Key> tree;
if (useOdometricPath)
tree = findOdometricPath(pose2Graph);
else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(pose2Graph);
tree = findMinimumSpanningTree(pose2Graph);
// Create a linear factor graph (LFG) of scalars
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
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
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
// temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds,
chordsIds, pose2Graph, orientationsToRoot, tree);
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(
spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
// Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize();
@ -256,8 +296,7 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
/* ************************************************************************* */
VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
bool useOdometricPath) {
bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
@ -314,8 +353,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
}
}
// add prior
linearPose2graph.add(initialize::kAnchorKey, I3, Vector3(0.0, 0.0, 0.0),
priorPose2Noise);
linearPose2graph.add(kAnchorKey, I3, Vector3(0.0, 0.0, 0.0), priorPose2Noise);
// optimize
VectorValues posesLago = linearPose2graph.optimize();
@ -324,7 +362,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
Values initialGuessLago;
for(const VectorValues::value_type& it: posesLago) {
Key key = it.first;
if (key != initialize::kAnchorKey) {
if (key != kAnchorKey) {
const Vector& poseVector = it.second;
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
orientationsLago.at(key)(0) + poseVector(2));
@ -361,7 +399,7 @@ Values initialize(const NonlinearFactorGraph& graph,
// for all nodes in the tree
for(const VectorValues::value_type& it: orientations) {
Key key = it.first;
if (key != initialize::kAnchorKey) {
if (key != kAnchorKey) {
const Pose2& pose = initialGuess.at<Pose2>(key);
const Vector& orientation = it.second;
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));

15
gtsam/slam/lago.h
View File

@ -37,19 +37,19 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
namespace gtsam {
namespace lago {
typedef std::map<Key, double> key2doubleMap;
typedef std::map<Key, Key> PredecessorMap;
/**
* Compute the cumulative orientations (without wrapping)
* for all nodes wrt the root (root has zero orientation).
*/
GTSAM_EXPORT key2doubleMap computeThetasToRoot(
const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
const key2doubleMap& deltaThetaMap, const PredecessorMap& tree);
/**
* Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
@ -62,13 +62,20 @@ GTSAM_EXPORT key2doubleMap computeThetasToRoot(
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g);
/** Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
const key2doubleMap& orientationsToRoot, const PredecessorMap& tree);
/** Given a "pose2" factor graph, find its minimum spanning tree.
* Note: All 'Pose2' Between factors are given equal weightage.
* Note: Assumes all the edges (factors) are Between factors.
*/
GTSAM_EXPORT PredecessorMap findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph);
/** LAGO: Return the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientations(

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

@ -20,6 +20,7 @@
*/
#include <gtsam/slam/lago.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
@ -64,48 +65,79 @@ NonlinearFactorGraph graph() {
}
}
/*******************************************************************************/
TEST(Lago, findMinimumSpanningTree) {
NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g);
lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
// We should recover the following spanning tree:
//
// x2
// / \
// / \
// x3 x1
// /
// /
// x0
// |
// a
using initialize::kAnchorKey;
EXPECT_LONGS_EQUAL(kAnchorKey, tree[kAnchorKey]);
EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]);
EXPECT_LONGS_EQUAL(x0, tree[x1]);
EXPECT_LONGS_EQUAL(x1, tree[x2]);
EXPECT_LONGS_EQUAL(x2, tree[x3]);
}
/* *************************************************************************** */
TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
auto gPlus = initialize::buildPoseGraph<Pose2>(g);
lago::PredecessorMap 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);
DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3)
EXPECT_LONGS_EQUAL(0, spanningTreeIds[0]); // factor 0 is the first in the ST(0->1)
EXPECT_LONGS_EQUAL(1, spanningTreeIds[1]); // factor 1 is the second in the ST(1->2)
EXPECT_LONGS_EQUAL(2, spanningTreeIds[2]); // factor 2 is the third in the ST(2->3)
}
/* *************************************************************************** */
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);
lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus);
// check the tree structure
EXPECT_LONGS_EQUAL(tree[x0], x0);
EXPECT_LONGS_EQUAL(tree[x1], x0);
EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0);
using initialize::kAnchorKey;
EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]);
EXPECT_LONGS_EQUAL(x0, tree[x1]);
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);
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@ -115,24 +147,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);
lago::PredecessorMap 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));
}
@ -145,8 +177,8 @@ TEST( Lago, smallGraphVectorValues ) {
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -171,8 +203,8 @@ TEST( Lago, multiplePosePriors ) {
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -194,12 +226,12 @@ TEST( Lago, multiplePoseAndRotPriors ) {
NonlinearFactorGraph g = simpleLago::graph();
g.addPrior(x1, simpleLago::pose1.theta(), model);
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0).finished(), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI).finished(), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI).finished(), initial.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI).finished(), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI).finished(), initial.at(x3), 1e-6));
}
/* *************************************************************************** */