From 11db29b1d8a476535f69f30ceb019b90e47ba165 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 16 May 2014 15:27:05 -0400 Subject: [PATCH] completed lago example --- examples/tests/testPlanarSLAMExample_lago.cpp | 155 ++++++++++++------ 1 file changed, 103 insertions(+), 52 deletions(-) diff --git a/examples/tests/testPlanarSLAMExample_lago.cpp b/examples/tests/testPlanarSLAMExample_lago.cpp index b3eb115f0..d717efe2c 100644 --- a/examples/tests/testPlanarSLAMExample_lago.cpp +++ b/examples/tests/testPlanarSLAMExample_lago.cpp @@ -78,64 +78,64 @@ static const double PI = boost::math::constants::pi(); double computeThetaToRoot(const Key nodeKey, PredecessorMap& tree, map& deltaThetaMap, map& thetaFromRootMap) { - double nodeTheta = 0; - Key key_child = nodeKey; // the node - Key key_parent = 0; // the initialization does not matter - while(1){ - // We check if we reached the root - if(tree[key_child]==key_child) // if we reached the root - break; - // we sum the delta theta corresponding to the edge parent->child - nodeTheta += deltaThetaMap[key_child]; - // we get the parent - key_parent = tree[key_child]; // the parent - // we check if we connected to some part of the tree we know - if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ - nodeTheta += thetaFromRootMap[key_parent]; - break; - } - key_child = key_parent; // we move upwards in the tree + double nodeTheta = 0; + Key key_child = nodeKey; // the node + Key key_parent = 0; // the initialization does not matter + while(1){ + // We check if we reached the root + if(tree[key_child]==key_child) // if we reached the root + break; + // we sum the delta theta corresponding to the edge parent->child + nodeTheta += deltaThetaMap[key_child]; + // we get the parent + key_parent = tree[key_child]; // the parent + // we check if we connected to some part of the tree we know + if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ + nodeTheta += thetaFromRootMap[key_parent]; + break; } - return nodeTheta; + key_child = key_parent; // we move upwards in the tree + } + return nodeTheta; } void getSymbolicSubgraph(vector& keysInBinary, vector& spanningTree, vector& chords, map& deltaThetaMap, PredecessorMap& tree, const NonlinearFactorGraph& g){ // Get keys for which you want the orientation size_t id=0; - // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g){ - if (factor->keys().size() == 2){ - Key key1 = factor->keys()[0]; - Key key2 = factor->keys()[1]; - if(std::find(keysInBinary.begin(), keysInBinary.end(), key1)==keysInBinary.end()) // did not find key1, we add it - keysInBinary.push_back(key1); - if(std::find(keysInBinary.begin(), keysInBinary.end(), key2)==keysInBinary.end()) // did not find key2, we add it - keysInBinary.push_back(key2); + // Loop over the factors + BOOST_FOREACH(const boost::shared_ptr& factor, g){ + if (factor->keys().size() == 2){ + Key key1 = factor->keys()[0]; + Key key2 = factor->keys()[1]; + if(std::find(keysInBinary.begin(), keysInBinary.end(), key1)==keysInBinary.end()) // did not find key1, we add it + keysInBinary.push_back(key1); + if(std::find(keysInBinary.begin(), keysInBinary.end(), key2)==keysInBinary.end()) // did not find key2, we add it + keysInBinary.push_back(key2); - // recast to a between - boost::shared_ptr< BetweenFactor > pose2Between = boost::dynamic_pointer_cast< BetweenFactor >(factor); - if (!pose2Between) continue; + // recast to a between + boost::shared_ptr< BetweenFactor > pose2Between = boost::dynamic_pointer_cast< BetweenFactor >(factor); + if (!pose2Between) continue; - // get the orientation - measured().theta(); - double deltaTheta = pose2Between->measured().theta(); + // get the orientation - measured().theta(); + double deltaTheta = pose2Between->measured().theta(); - bool inTree=false; - if(tree[key1]==key2){ - deltaThetaMap.insert(std::pair(key1, -deltaTheta)); - inTree = true; - } - if(tree[key2]==key1){ - deltaThetaMap.insert(std::pair(key2, deltaTheta)); - inTree = true; - } - if(inTree == true) - spanningTree.push_back(id); - else // it's a chord! - chords.push_back(id); + bool inTree=false; + if(tree[key1]==key2){ + deltaThetaMap.insert(std::pair(key1, -deltaTheta)); + inTree = true; } - id++; + if(tree[key2]==key1){ + deltaThetaMap.insert(std::pair(key2, deltaTheta)); + inTree = true; + } + if(inTree == true) + spanningTree.push_back(id); + else // it's a chord! + chords.push_back(id); } + id++; + } } /* @@ -202,13 +202,13 @@ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, co } /* ************************************************************************* */ -VectorValues initializeLago(const NonlinearFactorGraph& graph) { +// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor +VectorValues initializeLago(const NonlinearFactorGraph& graph, vector& keysInBinary) { // Find a minimum spanning tree PredecessorMap tree = findMinimumSpanningTree >(graph); // Create a linear factor graph (LFG) of scalars - vector keysInBinary; map deltaThetaMap; vector spanningTree; // ids of between factors forming the spanning tree T vector chords; // ids of between factors corresponding to chords wrt T @@ -226,6 +226,34 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph) { return estimateLago; } +/* ************************************************************************* */ +// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor +VectorValues initializeLago(const NonlinearFactorGraph& graph) { + + vector keysInBinary; + return initializeLago(graph, keysInBinary); +} + +/* ************************************************************************* */ +// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor +Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { + Values initialGuessLago; + + // get the orientation estimates from LAGO + vector keysInBinary; + VectorValues orientations = initializeLago(graph, keysInBinary); + + // plug the orientations in the initialGuess + for(size_t i=0; i(key); + Vector orientation = orientations.at(key); + Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); + initialGuessLago.insert(key, poseLago); + } + return initialGuessLago; +} + namespace simple { // We consider a small graph: @@ -259,7 +287,7 @@ NonlinearFactorGraph graph() { TEST( Lago, checkSTandChords ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); + BetweenFactor >(g); vector keysInBinary; map deltaThetaMap; @@ -277,7 +305,7 @@ TEST( Lago, checkSTandChords ) { TEST( Lago, orientationsOverSpanningTree ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); + BetweenFactor >(g); // check the tree structure EXPECT_LONGS_EQUAL(tree[x0], x0); @@ -309,7 +337,7 @@ TEST( Lago, orientationsOverSpanningTree ) { TEST( Lago, regularizedMeasurements ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); + BetweenFactor >(g); vector keysInBinary; map deltaThetaMap; @@ -332,7 +360,7 @@ TEST( Lago, regularizedMeasurements ) { } /* *************************************************************************** */ -TEST( Lago, smallGraph_GTmeasurements ) { +TEST( Lago, smallGraphVectorValues ) { VectorValues initialGuessLago = initializeLago(simple::graph()); @@ -343,6 +371,29 @@ TEST( Lago, smallGraph_GTmeasurements ) { EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( Lago, smallGraphValues ) { + + // we set the orientations in the initial guess to zero + Values initialGuess; + initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0)); + initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0)); + initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0)); + initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); + + // lago does not touch the Cartesian part and only fixed the orientations + Values actual = initializeLago(simple::graph(), initialGuess); + + // we are in a noiseless case + Values expected; + expected.insert(x0,simple::pose0); + expected.insert(x1,simple::pose1); + expected.insert(x2,simple::pose2); + expected.insert(x3,simple::pose3); + + EXPECT(assert_equal(expected, actual, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr;