From 558bce010dd8712bb9a95cf064d98b2474986f6e Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 11:27:05 -0400 Subject: [PATCH] got rid of useless vector keysInBinary --- examples/tests/testPlanarSLAMExample_lago.cpp | 80 +++++++++---------- 1 file changed, 36 insertions(+), 44 deletions(-) diff --git a/examples/tests/testPlanarSLAMExample_lago.cpp b/examples/tests/testPlanarSLAMExample_lago.cpp index 736914784..6bf49d6df 100644 --- a/examples/tests/testPlanarSLAMExample_lago.cpp +++ b/examples/tests/testPlanarSLAMExample_lago.cpp @@ -76,8 +76,10 @@ static const double PI = boost::math::constants::pi(); * for a node (without wrapping). The function starts at the nodes and moves towards the root * summing up the (directed) rotation measurements. The root is assumed to have orientation zero */ +typedef map key2doubleMap; + double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const map& deltaThetaMap, map& thetaFromRootMap) { + const key2doubleMap& deltaThetaMap, key2doubleMap& thetaFromRootMap) { double nodeTheta = 0; Key key_child = nodeKey; // the node @@ -104,13 +106,16 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, * This function computes the cumulative orientation (without wrapping) * for all node wrt the root (root has zero orientation) */ -map computeThetasToRoot(const vector& keysInBinary, - const map& deltaThetaMap, const PredecessorMap& tree) { +key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, + const PredecessorMap& tree) { - map thetaToRootMap; + key2doubleMap thetaToRootMap; + key2doubleMap::const_iterator it; // for all nodes in the tree - BOOST_FOREACH(const Key& nodeKey, keysInBinary) { + for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ) + { // compute the orientation wrt root + Key nodeKey = it->first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); @@ -124,8 +129,8 @@ map computeThetasToRoot(const vector& keysInBinary, * Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: * for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] */ -void getSymbolicSubgraph(vector& keysInBinary, - /*OUTPUTS*/ vector& spanningTree, vector& chords, map& deltaThetaMap, +void getSymbolicSubgraph( + /*OUTPUTS*/ vector& spanningTree, vector& chords, key2doubleMap& deltaThetaMap, /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ // Get keys for which you want the orientation @@ -140,12 +145,6 @@ void getSymbolicSubgraph(vector& keysInBinary, boost::shared_ptr< BetweenFactor > pose2Between = boost::dynamic_pointer_cast< BetweenFactor >(factor); if (!pose2Between) continue; - // store the keys: these are the orientations we are going to estimate - 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); - // get the orientation - measured().theta(); double deltaTheta = pose2Between->measured().theta(); @@ -193,7 +192,7 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, * Linear factor graph with regularized orientation measurements */ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, const vector& chords, - const NonlinearFactorGraph& g, const map& orientationsToRoot, const PredecessorMap& tree){ + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ GaussianFactorGraph lagoGraph; Vector deltaTheta; @@ -226,19 +225,22 @@ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, co /* ************************************************************************* */ // returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor -VectorValues initializeLago(const NonlinearFactorGraph& graph, vector& keysInBinary) { +VectorValues initializeLago(const NonlinearFactorGraph& graph) { // Find a minimum spanning tree + + //buildPose2graph + PredecessorMap tree = findMinimumSpanningTree >(graph); // Create a linear factor graph (LFG) of scalars - map deltaThetaMap; + key2doubleMap deltaThetaMap; vector spanningTree; // ids of between factors forming the spanning tree T vector chords; // ids of between factors corresponding to chords wrt T - getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, graph); + getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, graph); // temporary structure to correct wraparounds along loops - map orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); + key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot, tree); @@ -249,26 +251,19 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph, vector& keys 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); + VectorValues orientations = initializeLago(graph); - // plug the orientations in the initialGuess - for(size_t i=0; ifirst; Pose2 pose = initialGuess.at(key); Vector orientation = orientations.at(key); Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); @@ -312,11 +307,10 @@ TEST( Lago, checkSTandChords ) { PredecessorMap tree = findMinimumSpanningTree >(g); - vector keysInBinary; - map deltaThetaMap; + key2doubleMap deltaThetaMap; vector spanningTree; // ids of between factors forming the spanning tree T vector chords; // ids of between factors corresponding to chords wrt T - getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); + getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g); DOUBLES_EQUAL(spanningTree[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTree[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) @@ -336,20 +330,19 @@ TEST( Lago, orientationsOverSpanningTree ) { EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x3], x0); - map expected; + key2doubleMap expected; expected[x0]= 0; expected[x1]= PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x2]= -PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x3]= -PI/2; // edge x0->x3 (consistent with edge (x0,x3)) - vector keysInBinary; - map deltaThetaMap; + key2doubleMap deltaThetaMap; vector spanningTree; // ids of between factors forming the spanning tree T vector chords; // ids of between factors corresponding to chords wrt T - getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); + getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g); - map actual; - actual = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); + key2doubleMap actual; + actual = 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); @@ -362,13 +355,12 @@ TEST( Lago, regularizedMeasurements ) { PredecessorMap tree = findMinimumSpanningTree >(g); - vector keysInBinary; - map deltaThetaMap; + key2doubleMap deltaThetaMap; vector spanningTree; // ids of between factors forming the spanning tree T vector chords; // ids of between factors corresponding to chords wrt T - getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); + getSymbolicSubgraph(spanningTree, chords, deltaThetaMap, tree, g); - map orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); + key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot, tree); std::pair actualAb = lagoGraph.jacobian();