diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp new file mode 100644 index 000000000..26fee923c --- /dev/null +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * 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.cpp + * @brief A 2D Pose SLAM example that reads input from g2o and uses robust kernels in optimization + * @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) << 0.01, 0.01, 0.001)); + 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/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 88359e0f8..db4c8da83 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { double dot = p.dot(q); // Check for special cases - if (std::abs(dot - 1.0) < 1e-20) + if (std::abs(dot - 1.0) < 1e-16) return (Vector(2) << 0, 0); - else if (std::abs(dot + 1.0) < 1e-20) + else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0); else { // no special case diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index a6c04681b..dea4113f7 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { - if(newDelta == Delta || lastAction == DECREASED_DELTA) + if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased Delta, so try again to increase Delta diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..1029d7615 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -537,6 +537,118 @@ 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; + } + + std::cout << "Reading g2o file: " << g2oFile << std::endl; + // READ INITIAL GUESS FROM G2O FILE + string tag; + while (is) { + if(! (is >> tag)) + break; + + if (tag == "VERTEX_SE2") { + 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") { + 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: + std::cout << "Robust kernel: None" << std::endl; 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..7bbc88f70 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) + * @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