diff --git a/gtsam/base/kruskal-inl.h b/gtsam/base/kruskal-inl.h new file mode 100644 index 000000000..d610541a0 --- /dev/null +++ b/gtsam/base/kruskal-inl.h @@ -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 +#include +#include +#include + +#include +#include +#include + +namespace gtsam::utils { + +/*****************************************************************************/ +/* Sort the 'weights' in increasing order and return the sorted order of its + * indices. */ +inline std::vector sortedIndices(const std::vector &weights) { + // Get the number of elements in the 'weights' vector. + const size_t n = weights.size(); + + // Create a vector of 'indices'. + std::vector 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 +std::vector kruskal(const FactorGraph &fg, + const std::vector &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 sortedIndices = + gtsam::utils::sortedIndices(weights); + + // Create a vector to hold MST indices. + const size_t n = variableIndex.size(); + std::vector treeIndices; + treeIndices.reserve(n - 1); + + // Initialize disjoint-set forest to keep track of merged Keys. + DSFMap 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 diff --git a/gtsam/base/kruskal.h b/gtsam/base/kruskal.h new file mode 100644 index 000000000..058c22435 --- /dev/null +++ b/gtsam/base/kruskal.h @@ -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 +#include + +#include + +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 +std::vector kruskal(const FactorGraph &fg, + const std::vector &weights); +} // namespace gtsam::utils + +#include diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp new file mode 100644 index 000000000..9d808670a --- /dev/null +++ b/gtsam/base/tests/testKruskal.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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(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(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); +} +/* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index a0edd1a18..8c4d3437e 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -52,28 +53,6 @@ using std::vector; namespace gtsam { -/*****************************************************************************/ -/* sort the container and return permutation index with default comparator */ -template -static vector sort_idx(const Container &src) { - typedef typename Container::value_type T; - const size_t n = src.size(); - vector > 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 idx; - idx.reserve(n); - for (size_t i = 0; i < n; i++) { - idx.push_back(tmp[i].first); - } - return idx; -} - /****************************************************************************/ Subgraph::Subgraph(const vector &indices) { edges_.reserve(indices.size()); @@ -240,7 +219,6 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator( /****************************************************************/ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, - const FastMap &ordering, const vector &weights) const { const SubgraphBuilderParameters &p = parameters_; switch (p.skeletonType) { @@ -251,7 +229,7 @@ vector 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 SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /****************************************************************/ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, const vector &weights) const { - const VariableIndex variableIndex(gfg); - const size_t n = variableIndex.size(); - const vector sortedIndices = sort_idx(weights); - - /* initialize buffer */ - vector 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 weights = this->weights(gfg); // Build spanning tree. - const vector tree = buildTree(gfg, forward_ordering, weights); + const vector 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); diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index fe8f704dc..aafba9306 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -163,13 +163,11 @@ class GTSAM_EXPORT SubgraphBuilder { private: std::vector buildTree(const GaussianFactorGraph &gfg, - const FastMap &ordering, const std::vector &weights) const; std::vector unary(const GaussianFactorGraph &gfg) const; std::vector natural_chain(const GaussianFactorGraph &gfg) const; std::vector bfs(const GaussianFactorGraph &gfg) const; std::vector kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, const std::vector &weights) const; std::vector sample(const std::vector &weights, const size_t t) const; diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index cb9d98218..5407e60a2 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -24,14 +24,20 @@ #include #include #include +#include #include +#include +#include + 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& 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& tree) { + const PredecessorMap& tree) { key2doubleMap thetaToRootMap; - // Orientation of the roo - thetaToRootMap.insert(pair(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(nodeKey, nodeTheta)); + thetaToRootMap.emplace(nodeKey, nodeTheta); } return thetaToRootMap; } @@ -97,7 +102,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, void getSymbolicGraph( /*OUTPUTS*/vector& spanningTreeIds, vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/const PredecessorMap& 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& spanningTreeIds, const vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, - const PredecessorMap& 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 findOdometricPath( +static PredecessorMap findOdometricPath( const NonlinearFactorGraph& pose2Graph) { - PredecessorMap 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& factor: pose2Graph) { @@ -211,42 +216,77 @@ static PredecessorMap 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(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 visitationMap; + std::stack> 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 static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, - bool useOdometricPath) { + bool useOdometricPath) { gttic(lago_computeOrientations); + PredecessorMap tree; // Find a minimum spanning tree - PredecessorMap tree; if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + tree = findMinimumSpanningTree(pose2Graph); // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; - vector spanningTreeIds; // ids of between factors forming the spanning tree T - vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + vector + spanningTreeIds; // ids of between factors forming the spanning tree T + vector + 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(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(key); const Vector& orientation = it.second; Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); diff --git a/gtsam/slam/lago.h b/gtsam/slam/lago.h index 0df80ac42..5661a7cba 100644 --- a/gtsam/slam/lago.h +++ b/gtsam/slam/lago.h @@ -37,19 +37,19 @@ #include #include #include -#include namespace gtsam { namespace lago { typedef std::map key2doubleMap; +typedef std::map 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& 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& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/const PredecessorMap& 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& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, - const key2doubleMap& orientationsToRoot, const PredecessorMap& 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( diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 18b1df84c..7dc7d1ac6 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -20,6 +20,7 @@ */ #include +#include #include #include #include @@ -64,48 +65,79 @@ NonlinearFactorGraph graph() { } } +/*******************************************************************************/ +TEST(Lago, findMinimumSpanningTree) { + NonlinearFactorGraph g = simpleLago::graph(); + auto gPlus = initialize::buildPoseGraph(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 tree = findMinimumSpanningTree >(g); + auto gPlus = initialize::buildPoseGraph(g); + lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector 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 tree = findMinimumSpanningTree >(g); + auto gPlus = initialize::buildPoseGraph(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 spanningTreeIds; // ids of between factors forming the spanning tree T - vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + vector + spanningTreeIds; // ids of between factors forming the spanning tree T + vector + 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 tree = findMinimumSpanningTree >(g); + auto gPlus = initialize::buildPoseGraph(g); + lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector 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 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)); } /* *************************************************************************** */