diff --git a/examples/Data/noisyToyGraph.txt b/examples/Data/noisyToyGraph.txt new file mode 100644 index 000000000..747f8da1c --- /dev/null +++ b/examples/Data/noisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 0.774115 1.183389 1.576173 +VERTEX_SE2 2 -0.262420 2.047059 -3.127594 +VERTEX_SE2 3 -1.605649 0.993891 -1.561134 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/optimizedNoisyToyGraph.txt b/examples/Data/optimizedNoisyToyGraph.txt new file mode 100644 index 000000000..217a76fb1 --- /dev/null +++ b/examples/Data/optimizedNoisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 -0.000000 0.000000 +VERTEX_SE2 1 0.955797 1.137643 1.543041 +VERTEX_SE2 2 0.129867 1.989651 3.201259 +VERTEX_SE2 3 -1.047715 0.933789 4.743682 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/orientationsNoisyToyGraph.txt b/examples/Data/orientationsNoisyToyGraph.txt new file mode 100644 index 000000000..ada372a8e --- /dev/null +++ b/examples/Data/orientationsNoisyToyGraph.txt @@ -0,0 +1,9 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 0.000000 0.000000 1.565449 +VERTEX_SE2 2 0.000000 0.000000 3.134143 +VERTEX_SE2 3 0.000000 0.000000 4.710123 +EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 +EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000 diff --git a/examples/Data/pose2example-rewritten.txt b/examples/Data/pose2example-rewritten.txt new file mode 100644 index 000000000..6c8fe38a2 --- /dev/null +++ b/examples/Data/pose2example-rewritten.txt @@ -0,0 +1,23 @@ +VERTEX_SE2 0 0 0 0 +VERTEX_SE2 1 1.03039 0.01135 -0.081596 +VERTEX_SE2 2 2.03614 -0.129733 -0.301887 +VERTEX_SE2 3 3.0151 -0.442395 -0.345514 +VERTEX_SE2 4 3.34395 0.506678 1.21471 +VERTEX_SE2 5 3.68449 1.46405 1.18379 +VERTEX_SE2 6 4.06463 2.41478 1.17633 +VERTEX_SE2 7 4.42978 3.30018 1.25917 +VERTEX_SE2 8 4.12888 2.32148 -1.82539 +VERTEX_SE2 9 3.88465 1.32751 -1.95302 +VERTEX_SE2 10 3.53107 0.388263 -2.14893 +EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017 +EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017 diff --git a/examples/Data/pose2example.txt b/examples/Data/pose2example.txt new file mode 100644 index 000000000..c6c69c881 --- /dev/null +++ b/examples/Data/pose2example.txt @@ -0,0 +1,23 @@ +VERTEX_SE2 0 0.000000 0.000000 0.000000 +VERTEX_SE2 1 1.030390 0.011350 -0.081596 +VERTEX_SE2 2 2.036137 -0.129733 -0.301887 +VERTEX_SE2 3 3.015097 -0.442395 -0.345514 +VERTEX_SE2 4 3.343949 0.506678 1.214715 +VERTEX_SE2 5 3.684491 1.464049 1.183785 +VERTEX_SE2 6 4.064626 2.414783 1.176333 +VERTEX_SE2 7 4.429778 3.300180 1.259169 +VERTEX_SE2 8 4.128877 2.321481 -1.825391 +VERTEX_SE2 9 3.884653 1.327509 -1.953016 +VERTEX_SE2 10 3.531067 0.388263 -2.148934 +EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 +EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699 \ No newline at end of file diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp new file mode 100644 index 000000000..393bba86d --- /dev/null +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample_g2o.cpp + * @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the + * optimization. Output is written on a file, in g2o format + * Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o + * @date May 15, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + + +int main(const int argc, const char *argv[]){ + + if (argc < 2) + std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; + const string g2oFile = argv[1]; + + NonlinearFactorGraph graph; + Values initial; + readG2o(g2oFile, graph, initial); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = graph; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, result); + std::cout << "done! " << std::endl; + + return 0; +} diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp new file mode 100644 index 000000000..d61f1b533 --- /dev/null +++ b/examples/Pose2SLAMExample_lago.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample_lago.cpp + * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem + * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h + * Output is written on a file, in g2o format + * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o + * @date May 15, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + + +int main(const int argc, const char *argv[]){ + + if (argc < 2) + std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; + const string g2oFile = argv[1]; + + NonlinearFactorGraph graph; + Values initial; + readG2o(g2oFile, graph, initial); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = graph; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + std::cout << "Computing LAGO estimate" << std::endl; + Values estimateLago = initializeLago(graphWithPrior); + std::cout << "done!" << std::endl; + + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, estimateLago); + std::cout << "done! " << std::endl; + + return 0; +} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 30a4434fe..85307e322 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -178,6 +178,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, // Calculate delta translation = unrotate(R1, dt); Point2 dt = p2.t() - t_; double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index a5646f647..42b548f5a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -398,7 +398,7 @@ TEST( Pose2, matrix ) TEST( Pose2, compose_matrix ) { Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x + Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT } @@ -412,7 +412,7 @@ TEST( Pose2, between ) // // *--0--*--* Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x + Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x Matrix actualH1,actualH2; Pose2 expected(M_PI/2.0, Point2(2,2)); diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 48ee88255..90c7b32c9 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -22,7 +22,7 @@ #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" -#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" +//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" #endif #include #ifdef __GNUC__ @@ -73,34 +73,41 @@ SDGraph toBoostGraph(const G& graph) { SDGraph g; typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; std::map key2vertex; - BoostVertex v1, v2; typename G::const_iterator itFactor; + // Loop over the factors for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { - if ((*itFactor)->keys().size() > 2) - throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys")); - if ((*itFactor)->keys().size() == 1) + // Ignore factors that are not binary + if ((*itFactor)->keys().size() != 2) continue; + // Cast the factor to the user-specified factor type F boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + // Ignore factors that are not of type F if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + // Retrieve the 2 keys (nodes) the factor (edge) is incident on + KEY key1 = factor->keys()[0]; + KEY key2 = factor->keys()[1]; + BoostVertex v1, v2; + + // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key1) == key2vertex.end()) { v1 = add_vertex(key1, g); - key2vertex.insert(make_pair(key1, v1)); + key2vertex.insert(std::pair(key1, v1)); } else v1 = key2vertex[key1]; + // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key2) == key2vertex.end()) { v2 = add_vertex(key2, g); - key2vertex.insert(make_pair(key2, v2)); + key2vertex.insert(std::pair(key2, v2)); } else v2 = key2vertex[key2]; + // Add an edge with weight 1.0 boost::property edge_property(1.0); // assume constant edge weight here boost::add_edge(v1, v2, edge_property, g); } @@ -222,12 +229,11 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap return config; } -/* ************************************************************************* */ - /* ************************************************************************* */ template PredecessorMap findMinimumSpanningTree(const G& fg) { + // Convert to a graph that boost understands SDGraph g = gtsam::toBoostGraph(fg); // find minimum spanning tree @@ -237,13 +243,12 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { // convert edge to string pairs PredecessorMap tree; typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; - typename std::vector::Vertex>::iterator vi; - for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { + BOOST_FOREACH(const typename SDGraph::Vertex& vi, p_map){ KEY key = boost::get(boost::vertex_name, g, *itVertex); - KEY parent = boost::get(boost::vertex_name, g, *vi); + KEY parent = boost::get(boost::vertex_name, g, vi); tree.insert(key, parent); + itVertex++; } - return tree; } diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/LagoInitializer.cpp new file mode 100644 index 000000000..789ff8d87 --- /dev/null +++ b/gtsam/nonlinear/LagoInitializer.cpp @@ -0,0 +1,346 @@ +/* ---------------------------------------------------------------------------- + + * 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 LagoInitializer.h + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#include +#include + +namespace gtsam { + +static Matrix I = eye(1); +static Matrix I3 = eye(3); + +/* ************************************************************************* */ +double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, + const key2doubleMap& deltaThetaMap, const key2doubleMap& 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.at(key_child)==key_child) // if we reached the root + break; + // we sum the delta theta corresponding to the edge parent->child + nodeTheta += deltaThetaMap.at(key_child); + // we get the parent + key_parent = tree.at(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.at(key_parent); + break; + } + key_child = key_parent; // we move upwards in the tree + } + return nodeTheta; +} + +/* ************************************************************************* */ +key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, + const PredecessorMap& tree) { + + key2doubleMap thetaToRootMap; + key2doubleMap::const_iterator it; + + // Orientation of the roo + thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); + + // for all nodes in the tree + 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)); + } + return thetaToRootMap; +} + +/* ************************************************************************* */ +void getSymbolicGraph( + /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, + /*INPUTS*/ const 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; + // get the orientation - measured().theta(); + double deltaTheta = pose2Between->measured().theta(); + // insert (directed) orientations in the map "deltaThetaMap" + bool inTree=false; + if(tree.at(key1)==key2){ // key2 -> key1 + deltaThetaMap.insert(std::pair(key1, -deltaTheta)); + inTree = true; + } else if(tree.at(key2)==key1){ // key1 -> key2 + deltaThetaMap.insert(std::pair(key2, deltaTheta)); + inTree = true; + } + // store factor slot, distinguishing spanning tree edges from chordsIds + if(inTree == true) + spanningTreeIds.push_back(id); + else // it's a chord! + chordsIds.push_back(id); + } + id++; + } +} + +/* ************************************************************************* */ +void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, + Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { + + // Get the relative rotation measurement from the between factor + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + deltaTheta = (Vector(1) << pose2Between->measured().theta()); + + // Retrieve the noise model for the relative rotation + SharedNoiseModel model = pose2Between->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement + model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); +} + +/* ************************************************************************* */ +GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ + + GaussianFactorGraph lagoGraph; + Vector deltaTheta; + noiseModel::Diagonal::shared_ptr model_deltaTheta; + + // put original measurements in the spanning tree + 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 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); + double key1_DeltaTheta_key2 = deltaTheta(0); + ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; + double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord + double k = round(k2pi_noise/(2*M_PI)); + //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug + Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); + lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); + } + // prior on the anchor orientation + lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + return lagoGraph; +} + +/* ************************************************************************* */ +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 PriorFactor to BetweenFactor + 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())); + } + return pose2Graph; +} + +/* ************************************************************************* */ +PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { + + PredecessorMap tree; + Key minKey; + bool minUnassigned = true; + + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + + Key key1 = std::min(factor->keys()[0], factor->keys()[1]); + Key key2 = std::max(factor->keys()[0], factor->keys()[1]); + if(minUnassigned){ + minKey = key1; + minUnassigned = false; + } + if( key2 - key1 == 1){ // consecutive keys + tree.insert(key2, key1); + if(key1 < minKey) + minKey = key1; + } + } + tree.insert(minKey,keyAnchor); + tree.insert(keyAnchor,keyAnchor); // root + return tree; +} + +/* ************************************************************************* */ +VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ + + // Find a minimum spanning tree + PredecessorMap tree; + if (useOdometricPath) + tree = findOdometricPath(pose2Graph); + else + tree = findMinimumSpanningTree >(pose2Graph); + + // Create a linear factor graph (LFG) of scalars + key2doubleMap deltaThetaMap; + std::vector spanningTreeIds; // ids of between factors forming the spanning tree T + std::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); + + // Solve the LFG + VectorValues orientationsLago = lagoGraph.optimize(); + + return orientationsLago; +} + +/* ************************************************************************* */ +VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, 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 = buildPose2graph(graph); + + // Get orientations from relative orientation measurements + return computeLagoOrientations(pose2Graph, useOdometricPath); +} + +/* ************************************************************************* */ +Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { + + // Linearized graph on full poses + GaussianFactorGraph linearPose2graph; + + // We include the linear version of each between factor + BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph){ + + boost::shared_ptr< BetweenFactor > pose2Between = + boost::dynamic_pointer_cast< BetweenFactor >(factor); + + if(pose2Between){ + Key key1 = pose2Between->keys()[0]; + double theta1 = orientationsLago.at(key1)(0); + double s1 = sin(theta1); double c1 = cos(theta1); + + Key key2 = pose2Between->keys()[1]; + double theta2 = orientationsLago.at(key2)(0); + + double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta(); + linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize + + double dx = pose2Between->measured().x(); + double dy = pose2Between->measured().y(); + + Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy); + Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs + Matrix J1 = - I3; + J1(0,2) = s1*dx + c1*dy; + J1(1,2) = -c1*dx + s1*dy; + // Retrieve the noise model for the relative rotation + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(pose2Between->get_noiseModel()); + + linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + }else{ + throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); + } + } + // add prior + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel)); + + // optimize + VectorValues posesLago = linearPose2graph.optimize(); + + // put into Values structure + Values initialGuessLago; + for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ + Key key = it->first; + if (key != keyAnchor){ + Vector poseVector = posesLago.at(key); + Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); + initialGuessLago.insert(key, poseLago); + } + } + return initialGuessLago; +} + +/* ************************************************************************* */ +Values initializeLago(const NonlinearFactorGraph& graph, 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 = buildPose2graph(graph); + + // Get orientations from relative orientation measurements + VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath); + + // Compute the full poses + return computeLagoPoses(pose2Graph, orientationsLago); +} + +/* ************************************************************************* */ +Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { + Values initialGuessLago; + + // get the orientation estimates from LAGO + VectorValues orientations = initializeOrientationsLago(graph); + + // for all nodes in the tree + for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ + Key key = it->first; + 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; +} + +} // end of namespace gtsam diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/LagoInitializer.h new file mode 100644 index 000000000..92f23a66d --- /dev/null +++ b/gtsam/nonlinear/LagoInitializer.h @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 LagoInitializer.h + * @brief Initialize Pose2 in a factor graph 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 (can include arbitrary factors but we assume + * that there is a subgraph involving Pose2 and betweenFactors). Also in the current + * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) + * and a prior on x0. This assumption can be relaxed by using the extra argument + * useOdometricPath = false, although this part of code is not stable yet. + * @return Values: initial guess from LAGO (only pose2 are initialized) + * + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +typedef std::map key2doubleMap; +const Key keyAnchor = symbol('Z',9999999); +noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + +/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) + * for a node (nodeKey). The function starts at the nodes and moves towards the root + * summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap" + * The root is assumed to have orientation zero. + */ +double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, + const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap); + +/* This function computes the cumulative orientations (without wrapping) + * for all node wrt the root (root has zero orientation) + */ +key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, + const PredecessorMap& tree); + +/* 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 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 getSymbolicGraph( + /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, + /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g); + +/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor */ +void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, + Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); + +/* Linear factor graph with regularized orientation measurements */ +GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); + +/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ +NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); + +/* Returns the orientations of a graph including only BetweenFactors */ +VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); + +/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ +VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); + +/* Returns the values for the Pose2 in a generic factor graph */ +Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); + +/* Only corrects the orientation part in initialGuess */ +Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); + +} // end of namespace gtsam diff --git a/gtsam/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLagoInitializer.cpp new file mode 100644 index 000000000..64e43ae9b --- /dev/null +++ b/gtsam/nonlinear/tests/testLagoInitializer.cpp @@ -0,0 +1,332 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +#include + +#include + +#include +#include +#include + +#include +#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)); + +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)); + g.add(PriorFactor(x0, pose0, model)); + return g; +} +} + +/* *************************************************************************** */ +TEST( Lago, checkSTandChords ) { + NonlinearFactorGraph g = simple::graph(); + PredecessorMap tree = findMinimumSpanningTree >(g); + + key2doubleMap deltaThetaMap; + 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(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) + +} + +/* *************************************************************************** */ +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); + + 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)) + + key2doubleMap deltaThetaMap; + 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); + 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); + + key2doubleMap deltaThetaMap; + 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 = buildLinearOrientationGraph(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 chordsIds) + Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2); + + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraphVectorValues ) { + bool useOdometricPath = false; + VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), 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), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraphVectorValuesSP ) { + + VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePosePriors ) { + bool useOdometricPath = false; + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initialGuessLago = initializeOrientationsLago(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), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePosePriorsSP ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1, model)); + VectorValues initialGuessLago = initializeOrientationsLago(g); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePoseAndRotPriors ) { + bool useOdometricPath = false; + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initialGuessLago = initializeOrientationsLago(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), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); +} + +/* *************************************************************************** */ +TEST( Lago, multiplePoseAndRotPriorsSP ) { + NonlinearFactorGraph g = simple::graph(); + g.add(PriorFactor(x1, simple::pose1.theta(), model)); + VectorValues initialGuessLago = initializeOrientationsLago(g); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_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)); +} + +/* *************************************************************************** */ +TEST( Lago, smallGraph2 ) { + + // lago does not touch the Cartesian part and only fixed the orientations + Values actual = initializeLago(simple::graph()); + + // 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)); +} + +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy_orientations ) { + + NonlinearFactorGraph g; + Values initial; + string inputFile = findExampleDataFile("noisyToyGraph"); + readG2o(inputFile, g, initial); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = g; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + Values actual; + Key keyAnc = symbol('Z',9999999); + for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ + Key key = it->first; + if (key != keyAnc){ + Vector orientation = actualVV.at(key); + Pose2 poseLago = Pose2(0.0,0.0,orientation(0)); + actual.insert(key, poseLago); + } + } + NonlinearFactorGraph gmatlab; + Values expected; + string matlabFile = findExampleDataFile("orientationsNoisyToyGraph"); + readG2o(matlabFile, gmatlab, expected); + + BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-5)); + } +} + +/* *************************************************************************** */ +TEST( Lago, largeGraphNoisy ) { + + NonlinearFactorGraph g; + Values initial; + string inputFile = findExampleDataFile("noisyToyGraph"); + readG2o(inputFile, g, initial); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = g; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + + Values actual = initializeLago(graphWithPrior); + + NonlinearFactorGraph gmatlab; + Values expected; + string matlabFile = findExampleDataFile("optimizedNoisyToyGraph"); + readG2o(matlabFile, gmatlab, expected); + + BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){ + Key k = key_val.key; + EXPECT(assert_equal(expected.at(k), actual.at(k), 1e-2)); + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 47a0aca2a..f381a0786 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -544,6 +544,117 @@ bool readBundler(const string& filename, SfM_data &data) return true; } +/* ************************************************************************* */ +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, + const kernelFunctionType kernelFunction){ + + ifstream is(g2oFile.c_str()); + if (!is){ + throw std::invalid_argument("File not found!"); + return false; + } + + // READ INITIAL GUESS FROM G2O FILE + string tag; + while (is) { + if(! (is >> tag)) + break; + + if (tag == "VERTEX_SE2" || tag == "VERTEX2") { + int id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + initial.insert(id, Pose2(x, y, yaw)); + } + is.ignore(LINESIZE, '\n'); + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + // initial.print("initial guess"); + + // READ MEASUREMENTS FROM G2O FILE + while (is) { + if(! (is >> tag)) + break; + + if (tag == "EDGE_SE2" || tag == "EDGE2") { + int id1, id2; + double x, y, yaw; + double I11, I12, I13, I22, I23, I33; + + is >> id1 >> id2 >> x >> y >> yaw; + is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; + Pose2 l1Xl2(x, y, yaw); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33)); + + switch (kernelFunction) { + {case QUADRATIC: + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + graph.add(factor); + break;} + {case HUBER: + NonlinearFactor::shared_ptr huberFactor(new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + graph.add(huberFactor); + break;} + {case TUKEY: + NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); + graph.add(tukeyFactor); + break;} + } + } + is.ignore(LINESIZE, '\n'); + } + // Output which kernel is used + switch (kernelFunction) { + case QUADRATIC: + break; + case HUBER: + std::cout << "Robust kernel: Huber" << std::endl; break; + case TUKEY: + std::cout << "Robust kernel: Tukey" << std::endl; break; + } + return true; +} + +/* ************************************************************************* */ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ + + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) + { + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; + } + + // save edges + BOOST_FOREACH(boost::shared_ptr factor_, graph) + { + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); + if (!factor) + continue; + + SharedNoiseModel model = factor->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!"); + + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " + << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; + } + stream.close(); + return true; +} + /* ************************************************************************* */ bool readBAL(const string& filename, SfM_data &data) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 64ccd37b5..2f73db37f 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -117,6 +117,25 @@ struct SfM_data */ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); +/** + * @brief This function parses a g2o file and stores the measurements into a + * NonlinearFactorGraph and the initial guess in a Values structure + * @param filename The name of the g2o file + * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. + * @return initial Values containing the initial guess (VERTEX_SE2) + */ +enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC); + +/** + * @brief This function writes a g2o file from + * NonlinearFactorGraph and a Values structure + * @param filename The name of the g2o file to write + * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) + * @return estimate Values containing the values (VERTEX_SE2) + */ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate); + /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a * SfM_data structure diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4ae30f508..add0db29d 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -22,6 +22,8 @@ #include #include #include + +#include #include using namespace gtsam::symbol_shorthand; @@ -37,6 +39,20 @@ TEST(dataSet, findExampleDataFile) { EXPECT(assert_equal(expected_end, actual_end)); } +/* ************************************************************************* */ +//TEST( dataSet, load2D) +//{ +// ///< The structure where we will save the SfM data +// const string filename = findExampleDataFile("smallGraph.g2o"); +// boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000, +// false, false); +//// print +//// +//// print +//// +//// EXPECT(assert_equal(expected,actual,12)); +//} + /* ************************************************************************* */ TEST( dataSet, Balbianello) { @@ -58,6 +74,117 @@ TEST( dataSet, Balbianello) EXPECT(assert_equal(expected,actual,1)); } +/* ************************************************************************* */ +TEST( dataSet, readG2o) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph actualGraph; + Values actualValues; + readG2o(g2oFile, actualGraph, actualValues); + + Values expectedValues; + expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); + expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596)); + expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887)); + expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514)); + expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715)); + expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785)); + expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333)); + expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169)); + expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); + expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); + expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); + EXPECT(assert_equal(expectedValues,actualValues,1e-5)); + + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, readG2oHuber) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph actualGraph; + Values actualValues; + readG2o(g2oFile, actualGraph, actualValues, HUBER); + + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); + + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, readG2oTukey) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph actualGraph; + Values actualValues; + readG2o(g2oFile, actualGraph, actualValues, TUKEY); + + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); + SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + + NonlinearFactorGraph expectedGraph; + expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); +} + +/* ************************************************************************* */ +TEST( dataSet, writeG2o) +{ + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph expectedGraph; + Values expectedValues; + readG2o(g2oFile, expectedGraph, expectedValues); + + const string filenameToWrite = findExampleDataFile("pose2example-rewritten"); + writeG2o(filenameToWrite, expectedGraph, expectedValues); + + NonlinearFactorGraph actualGraph; + Values actualValues; + readG2o(filenameToWrite, actualGraph, actualValues); + EXPECT(assert_equal(expectedValues,actualValues,1e-5)); + EXPECT(assert_equal(actualGraph,expectedGraph,1e-5)); +} + /* ************************************************************************* */ TEST( dataSet, readBAL_Dubrovnik) { diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 842f2bd67..a1d2d8c41 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -18,7 +18,10 @@ #include #include +#include +#include #include +#include #include #include @@ -105,24 +108,38 @@ TEST( Graph, composePoses ) CHECK(assert_equal(expected, *actual)); } -// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g += X(1), I, X(2), I, b, model; -// g += X(1), I, X(3), I, b, model; -// g += X(1), I, X(4), I, b, model; -// g += X(2), I, X(3), I, b, model; -// g += X(2), I, X(4), I, b, model; -// g += X(3), I, X(4), I, b, model; -// -// map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[X(1)].compare(X(1))==0); -// EXPECT(tree[X(2)].compare(X(1))==0); -// EXPECT(tree[X(3)].compare(X(1))==0); -// EXPECT(tree[X(4)].compare(X(1))==0); -//} +/* ************************************************************************* */ +TEST( GaussianFactorGraph, findMinimumSpanningTree ) +{ + GaussianFactorGraph g; + Matrix I = eye(2); + Vector2 b(0, 0); + const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); + using namespace symbol_shorthand; + g += JacobianFactor(X(1), I, X(2), I, b, model); + g += JacobianFactor(X(1), I, X(3), I, b, model); + g += JacobianFactor(X(1), I, X(4), I, b, model); + g += JacobianFactor(X(2), I, X(3), I, b, model); + g += JacobianFactor(X(2), I, X(4), I, b, model); + g += JacobianFactor(X(3), I, X(4), I, b, model); + + PredecessorMap tree = findMinimumSpanningTree(g); + EXPECT_LONGS_EQUAL(X(1),tree[X(1)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(2)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(3)]); + EXPECT_LONGS_EQUAL(X(1),tree[X(4)]); + + // we add a disconnected component - does not work yet + // g += JacobianFactor(X(5), I, X(6), I, b, model); + // + // PredecessorMap forest = findMinimumSpanningTree(g); + // EXPECT_LONGS_EQUAL(X(1),forest[X(1)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(2)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(3)]); + // EXPECT_LONGS_EQUAL(X(1),forest[X(4)]); + // EXPECT_LONGS_EQUAL(X(5),forest[X(5)]); + // EXPECT_LONGS_EQUAL(X(5),forest[X(6)]); +} ///* ************************************************************************* */ // SL-FIX TEST( GaussianFactorGraph, split )