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

WIP: replace instances of calling the graph-inl version of 'findMinimumSpanningTree' with the lago version

release/4.3a0
Ankur Roy Chowdhury 2023-02-04 19:04:31 -08:00
parent 64267a3d8d
commit b83261e2b1
4 changed files with 156 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,7 +20,7 @@
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/DSFMap.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
@ -34,12 +34,10 @@ namespace gtsam::utils
/*****************************************************************************/
/* sort the container and return permutation index with default comparator */
template <typename Container>
static std::vector<size_t> sort_idx(const Container &src)
inline std::vector<size_t> sortedIndices(const std::vector<double> &src)
{
typedef typename Container::value_type T;
const size_t n = src.size();
std::vector<std::pair<size_t, T>> tmp;
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]);
@ -63,27 +61,33 @@ namespace gtsam::utils
const FastMap<Key, size_t> &ordering,
const std::vector<double> &weights)
{
// Create an index from variables to factor indices.
const VariableIndex variableIndex(fg);
const size_t n = variableIndex.size();
const std::vector<size_t> sortedIndices = sort_idx(weights);
/* initialize buffer */
// 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);
// container for acsendingly sorted edges
DSFVector dsf(n);
// 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 &f = *fg[index];
const auto keys = f.keys();
if (keys.size() != 2)
const auto factor = fg[index];
// Ignore non-binary edges.
if (factor->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))
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);

106
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;
@ -79,16 +85,15 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
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;
}
@ -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,7 +195,7 @@ 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;
}
@ -199,7 +204,7 @@ static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree;
Key minKey = initialize::kAnchorKey; // this initialization does not matter
Key minKey = kAnchorKey; // this initialization does not matter
bool minUnassigned = true;
for(const std::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
@ -216,11 +221,51 @@ static PredecessorMap<Key> findOdometricPath(
minKey = key1;
}
}
tree.insert(minKey, initialize::kAnchorKey);
tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
tree.insert(minKey, kAnchorKey);
tree.insert(kAnchorKey, kAnchorKey); // root
return tree;
}
/*****************************************************************************/
PredecessorMap<Key> findMinimumSpanningTree(const NonlinearFactorGraph& pose2Graph){
// Compute the minimum spanning tree
const FastMap<Key, size_t> forwardOrdering = Ordering::Natural(pose2Graph).invert();
const auto edgeWeights = std::vector<double>(pose2Graph.size(), 1.0);
const auto mstEdgeIndices = utils::kruskal(pose2Graph, forwardOrdering, edgeWeights);
// std::cout << "MST Edge Indices:\n";
// for(const auto& i: mstEdgeIndices){
// std::cout << i << ", ";
// }
// std::cout << "\n";
// Create PredecessorMap
PredecessorMap<Key> predecessorMap;
std::map<Key, bool> visitationMap;
std::stack<std::pair<Key, Key>> stack;
// const auto rootKey = pose2Graph[mstEdgeIndices.front()]->front();
// stack.push({rootKey, rootKey});
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,
@ -232,8 +277,15 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
if (useOdometricPath)
tree = findOdometricPath(pose2Graph);
else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(pose2Graph);
{
tree = findMinimumSpanningTree(pose2Graph);
// tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2>>(pose2Graph);
std::cout << "computeOrientations:: Spanning Tree: \n";
for(const auto&[k, v]: tree){
std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(v)) << " -> " << gtsam::DefaultKeyFormatter(gtsam::Symbol(k)) << "\n";
}
}
// Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap;
@ -241,6 +293,18 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
// std::cout << "Spanning Tree Edge Ids:\n";
// for(const auto& i: spanningTreeIds){
// std::cout << i << ", ";
// }
// std::cout << "\n";
// std::cout << "Chord Edge Ids:\n";
// for(const auto& i: chordsIds){
// std::cout << i << ", ";
// }
// std::cout << "\n";
// temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
@ -263,7 +327,14 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
// Get orientations from relative orientation measurements
return computeOrientations(pose2Graph, useOdometricPath);
auto initial = computeOrientations(pose2Graph, useOdometricPath);
std::cout << "Lago::initializeOrientations: Values: \n";
for(const auto& [key, val]: initial){
std::cout << gtsam::DefaultKeyFormatter(gtsam::Symbol(key)) << " -> " << val << "\n";
}
std::cout << "\n";
return initial;
}
/* ************************************************************************* */
@ -314,8 +385,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 +394,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 +431,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));

6
gtsam/slam/lago.h
View File

@ -70,6 +70,12 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/** Given a "pose2" factor graph, find it's minimum spanning tree.
* Note: all 'Pose2' factors are given equal weightage.
*/
GTSAM_EXPORT PredecessorMap<Key> findMinimumSpanningTree(
const NonlinearFactorGraph& pose2Graph);
/** LAGO: Return the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientations(
const NonlinearFactorGraph& graph, bool useOdometricPath = true);

58
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,20 +65,45 @@ NonlinearFactorGraph graph() {
}
}
/*******************************************************************************/
TEST(Lago, findMinimumSpanningTree) {
NonlinearFactorGraph g = simpleLago::graph();
auto gPlus = initialize::buildPoseGraph<Pose2>(g);
PredecessorMap<Key> 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);
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);
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)
}
@ -88,10 +114,10 @@ TEST( Lago, orientationsOverSpanningTree ) {
BetweenFactor<Pose2> >(g);
// 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);
EXPECT_LONGS_EQUAL(x0, tree[x0]);
EXPECT_LONGS_EQUAL(x0, tree[x1]);
EXPECT_LONGS_EQUAL(x0, tree[x2]);
EXPECT_LONGS_EQUAL(x0, tree[x3]);
lago::key2doubleMap expected;
expected[x0]= 0;
@ -145,8 +171,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 +197,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 +220,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));
}
/* *************************************************************************** */