/* ---------------------------------------------------------------------------- * 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 testPlanarSLAMExample_lago.cpp * @brief Unit tests for planar SLAM example using the initialization technique * LAGO (Linear Approximation for Graph Optimization) * * @author Luca Carlone * @author Frank Dellaert * @date May 14, 2014 */ // As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent // the robot positions and Point2 variables (x, y) to represent the landmark coordinates. #include #include #include // Each variable in the system (poses and landmarks) must be identified with a unique key. // We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). // Here we will use Symbols #include // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use a RangeBearing factor for the range-bearing measurements to identified // landmarks, and Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. #include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using // are nonlinear factors, we will need a Nonlinear Factor Graph. #include #include #include #include #include using namespace std; using namespace gtsam; using namespace boost::assign; Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); static const double PI = boost::math::constants::pi(); #include /** * @brief Initialization technique for planar pose SLAM using * LAGO (Linear Approximation for Graph Optimization). see papers: * * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate * approximation for planar pose graph optimization, IJRR, 2014. * * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation * for graph-based simultaneous localization and mapping, RSS, 2011. * * @param graph: nonlinear factor graph including between (Pose2) measurements * @return Values: initial guess including orientation estimate from LAGO */ /* * This function computes the cumulative orientation wrt the root (without wrapping) * 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 */ 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 } return nodeTheta; } /* * This function computes the cumulative orientation (without wrapping) * for all node wrt the root (root has zero orientation) */ map computeThetasToRoot(vector& keysInBinary, map& deltaThetaMap, PredecessorMap& tree){ map thetaToRootMap; BOOST_FOREACH(const Key& nodeKey, keysInBinary){ double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); } return thetaToRootMap; } /* * Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belong to the tree and to g, * and stores the factor slots corresponding to edges in the tree and to chords 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(vector& keysInBinary, /*OUTPUTS*/ vector& spanningTree, vector& chords, map& deltaThetaMap, /*INPUTS*/ 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]; // recast to a between 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(); // insert (directed) orientations in the map "deltaThetaMap" 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; } // store factor slot, distinguishing spanning tree edges from chords if(inTree == true) spanningTree.push_back(id); else // it's a chord! chords.push_back(id); } id++; } } void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta){ td::cout << "TODO: improve computation of noise model" << std::endl; boost::shared_ptr< BetweenFactor > pose2Between = boost::dynamic_pointer_cast< BetweenFactor >(factor); if (!pose2Between) throw std::invalid_argument("buildOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve noise model SharedNoiseModel model = pose2Between->get_noiseModel(); boost::shared_ptr< noiseModel::Gaussian > gaussianModel = boost::dynamic_pointer_cast< noiseModel::Gaussian >(model); if (!gaussianModel) throw std::invalid_argument("buildOrientationGraph: invalid noise model!"); Matrix infoMatrix = gaussianModel->R() * gaussianModel->R(); // information matrix Matrix covMatrix = infoMatrix.inverse(); Vector variance_deltaTheta = (Vector(1) << covMatrix(2,2)); model_deltaTheta = noiseModel::Diagonal::Variances(variance_deltaTheta); } /* * Linear factor graph with regularized orientation measurements */ GaussianFactorGraph buildOrientationGraph(const vector& spanningTree, const vector& chords, const NonlinearFactorGraph& g, map orientationsToRoot){ GaussianFactorGraph lagoGraph; Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; Matrix I = eye(1); // put original measurements in the spanning tree BOOST_FOREACH(const size_t& factorId, spanningTree){ Key key1 = g[factorId]->keys()[0]; Key key2 = g[factorId]->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){ Key key1 = g[factorId]->keys()[0]; Key key2 = g[factorId]->keys()[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); double key1_DeltaTheta_key2 = deltaTheta(0); double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot[key1] - orientationsToRoot[key2]; // this coincides to summing up measurements along the cycle induced by the chord double k = round(k2pi_noise/(2*PI)); Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*PI); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); } // prior on first orientation (anchor) noiseModel::Diagonal::shared_ptr model_anchor = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); std::cout << "TODO: fix the right root orientation and key" << std::endl; lagoGraph.add(JacobianFactor(x0, I, (Vector(1) << 0.0), model_anchor)); return lagoGraph; } /* ************************************************************************* */ // 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 map 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); // temporary structure to correct wraparounds along loops map orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, graph, orientationsToRoot); // Solve the LFG VectorValues estimateLago = lagoGraph.optimize(); 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: // symbolic FG // x2 0 1 // / | \ 1 2 // / | \ 2 3 // x3 | x1 2 0 // \ | / 0 3 // \ | / // x0 // Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000); Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796); Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593); Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389); NonlinearFactorGraph graph() { NonlinearFactorGraph g; g.add(BetweenFactor(x0, x1, pose0.between(pose1), model)); g.add(BetweenFactor(x1, x2, pose1.between(pose2), model)); 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)); return g; } } /* *************************************************************************** */ TEST( Lago, checkSTandChords ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); 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 getSymbolicSubgraph(keysInBinary, 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) DOUBLES_EQUAL(spanningTree[2], 4, 1e-6); // factor 4 is the third in the ST(0->3) } /* *************************************************************************** */ TEST( Lago, orientationsOverSpanningTree ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(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); map 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; 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); map actual; actual = computeThetasToRoot(keysInBinary, 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); DOUBLES_EQUAL(expected[x3], actual[x3], 1e-6); } /* *************************************************************************** */ TEST( Lago, regularizedMeasurements ) { NonlinearFactorGraph g = simple::graph(); PredecessorMap tree = findMinimumSpanningTree >(g); 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 getSymbolicSubgraph(keysInBinary, spanningTree, chords, deltaThetaMap, tree, g); map orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot); 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) Vector expected = (Vector(5) << PI/2, PI, -PI/2, PI/2 - 2*PI , PI/2); EXPECT(assert_equal(expected, actual, 1e-6)); } /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { VectorValues initialGuessLago = initializeLago(simple::graph()); // 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 ) { // 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; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */