From c8e178b542ace14e4c4ecf237dbcf0cc977fe729 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 12:20:07 -0400 Subject: [PATCH] included priors --- examples/tests/testPlanarSLAMExample_lago.cpp | 150 +++++++++++++----- 1 file changed, 109 insertions(+), 41 deletions(-) diff --git a/examples/tests/testPlanarSLAMExample_lago.cpp b/examples/tests/testPlanarSLAMExample_lago.cpp index 6bf49d6df..8aaa35352 100644 --- a/examples/tests/testPlanarSLAMExample_lago.cpp +++ b/examples/tests/testPlanarSLAMExample_lago.cpp @@ -77,6 +77,7 @@ static const double PI = boost::math::constants::pi(); * summing up the (directed) rotation measurements. The root is assumed to have orientation zero */ typedef map key2doubleMap; +const Key keyAnchor = symbol('A',0); double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, key2doubleMap& thetaFromRootMap) { @@ -125,12 +126,12 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, /* * Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g, - * and stores the factor slots corresponding to edges in the tree and to chords wrt this tree + * and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree * 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( - /*OUTPUTS*/ vector& spanningTree, vector& chords, key2doubleMap& deltaThetaMap, +void getSymbolicGraph( + /*OUTPUTS*/ vector& spanningTreeIds, vector& chordsIds, key2doubleMap& deltaThetaMap, /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ // Get keys for which you want the orientation @@ -158,11 +159,11 @@ void getSymbolicSubgraph( inTree = true; } - // store factor slot, distinguishing spanning tree edges from chords + // store factor slot, distinguishing spanning tree edges from chordsIds if(inTree == true) - spanningTree.push_back(id); + spanningTreeIds.push_back(id); else // it's a chord! - chords.push_back(id); + chordsIds.push_back(id); } id++; } @@ -191,7 +192,7 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, /* * Linear factor graph with regularized orientation measurements */ -GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, const vector& chords, +GaussianFactorGraph buildOrientationGraph(const vector& spanningTreeIds, const vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ GaussianFactorGraph lagoGraph; @@ -200,14 +201,14 @@ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, co Matrix I = eye(1); // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTree){ + BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); } - // put regularized measurements in the chords - BOOST_FOREACH(const size_t& factorId, chords){ + // put regularized measurements in the chordsIds + BOOST_FOREACH(const size_t& factorId, chordsIds){ const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); @@ -223,27 +224,63 @@ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, co return lagoGraph; } +/* ************************************************************************* */ +// Selects the subgraph composed by between factors and transforms priors into between wrt a fictitious node +NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ + NonlinearFactorGraph pose2Graph; + + BOOST_FOREACH(const boost::shared_ptr& factor, graph){ + + // recast to a between on Pose2 + boost::shared_ptr< BetweenFactor > pose2Between = + boost::dynamic_pointer_cast< BetweenFactor >(factor); + if (pose2Between) + pose2Graph.add(pose2Between); + + // recast to a between on Rot2 + boost::shared_ptr< BetweenFactor > rot2Between = + boost::dynamic_pointer_cast< BetweenFactor >(factor); + if (rot2Between) + pose2Graph.add(rot2Between); + + // recast to a prior on Pose2 + boost::shared_ptr< PriorFactor > pose2Prior = + boost::dynamic_pointer_cast< PriorFactor >(factor); + if (pose2Prior) + pose2Graph.add(BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); + + // recast to a prior on Rot2 + boost::shared_ptr< PriorFactor > rot2Prior = + boost::dynamic_pointer_cast< PriorFactor >(factor); + if (rot2Prior) + pose2Graph.add(BetweenFactor(keyAnchor, rot2Prior->keys()[0], + rot2Prior->prior(), rot2Prior->get_noiseModel())); + } + return pose2Graph; +} /* ************************************************************************* */ // returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor VectorValues initializeLago(const NonlinearFactorGraph& graph) { + + // 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 = buildPose2graph(graph); + // Find a minimum spanning tree - - //buildPose2graph - - PredecessorMap tree = findMinimumSpanningTree >(graph); + PredecessorMap tree = findMinimumSpanningTree >(pose2Graph); // Create a linear factor graph (LFG) of scalars 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(spanningTree, chords, deltaThetaMap, tree, graph); + 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 = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues estimateLago = lagoGraph.optimize(); @@ -252,26 +289,30 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor +// Only correct the orientation part in initialGuess Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO VectorValues orientations = initializeLago(graph); - VectorValues::const_iterator it; // for all nodes in the tree - for(it = orientations.begin(); it != orientations.end(); ++it ) - { + for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ Key key = it->first; - Pose2 pose = initialGuess.at(key); - Vector orientation = orientations.at(key); - Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); - initialGuessLago.insert(key, poseLago); + if (key != keyAnchor){ + Pose2 pose = initialGuess.at(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: @@ -297,6 +338,7 @@ NonlinearFactorGraph graph() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); + g.add(PriorFactor(x0, pose0, model)); return g; } } @@ -308,13 +350,13 @@ TEST( Lago, checkSTandChords ) { BetweenFactor >(g); 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(spanningTree, chords, 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 + getSymbolicGraph(spanningTreeIds, chordsIds, 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) - DOUBLES_EQUAL(spanningTree[2], 4, 1e-6); // factor 4 is the third in the ST(0->3) + 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) } @@ -337,9 +379,9 @@ TEST( Lago, orientationsOverSpanningTree ) { expected[x3]= -PI/2; // edge x0->x3 (consistent with edge (x0,x3)) 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(spanningTree, chords, 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 + getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); key2doubleMap actual; actual = computeThetasToRoot(deltaThetaMap, tree); @@ -356,19 +398,19 @@ TEST( Lago, regularizedMeasurements ) { BetweenFactor >(g); 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(spanningTree, chords, 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 + getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); - GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTreeIds, chordsIds, g, 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)); // 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 chords) + // Expected regularized measurements (same for the spanning tree, corrected for the chordsIds) Vector expected = (Vector(5) << PI/2, PI, -PI/2, PI/2 - 2*PI , PI/2); EXPECT(assert_equal(expected, actual, 1e-6)); @@ -386,6 +428,32 @@ TEST( Lago, smallGraphVectorValues ) { EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( Lago, multiplePosePriors ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initialGuessLago = initializeLago(g); + + // comparison is up to PI, that's why we add some multiples of 2*PI + EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << PI - 2*PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePoseAndRotPriors ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initialGuessLago = initializeLago(g); + + // comparison is up to PI, that's why we add some multiples of 2*PI + EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << PI - 2*PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( Lago, smallGraphValues ) {