From 04533107263d72e39f99bb58b218eb37210d5163 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:35:45 -0400 Subject: [PATCH] adding functions to read/write g2o files --- examples/Pose2SLAMExampleRobust_g2o.cpp | 69 +-------------- gtsam/slam/dataset.cpp | 108 ++++++++++++++++++++++++ gtsam/slam/dataset.h | 10 +++ 3 files changed, 122 insertions(+), 65 deletions(-) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp index e326838f2..ad560ab59 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -39,89 +39,28 @@ int main(const int argc, const char *argv[]){ std::cout << "Please specify input file (in g2o format) and output file" << std::endl; const string g2oFile = argv[1]; - ifstream is(g2oFile.c_str()); - if (!is) - throw std::invalid_argument("File not found!"); - - std::cout << "Reading g2o file: " << g2oFile << std::endl; - // READ INITIAL GUESS FROM G2O FILE - Values initial; - 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 NonlinearFactorGraph graph; - 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; - - // Try to guess covariance matrix layout - Matrix m(3,3); - m << I11, I12, I13, I12, I22, I23, I13, I23, I33; - - Pose2 l1Xl2(x, y, yaw); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); - - // NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, - // noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); - - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); - - graph.add(factor); - } - is.ignore(LINESIZE, '\n'); - } - - //std::cout << "Robust kernel: Huber" << std::endl; - std::cout << "Robust kernel: Tukey" << std::endl; + Values initial; + readG2o(g2oFile, graph, initial); // otherwise GTSAM cannot solve the problem 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)); - // GaussNewtonParams parameters; - // Stop iterating once the change in error between steps is less than this value - // parameters.relativeErrorTol = 1e-5; - // Do not perform more than N iteration steps - // parameters.maxIterations = 100; // Create the optimizer ... std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); // ... and optimize Values result = optimizer.optimize(); - // result.print("results"); std::cout << "Optimization complete" << std::endl; const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0, 0.0, 0.0)); - save2D(graph, result, model, outputFile); + writeG2o(graph, result, model, outputFile); + std::cout << "done! " << std::endl; return 0; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 17d996a52..6e0cf11ba 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -537,6 +537,114 @@ bool readBundler(const string& filename, SfM_data &data) return true; } +/* ************************************************************************* */ +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, + const kernelFunctionType kernelFunction = QUADRATIC){ + + 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 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; + +// Matrix sqrtInfo = factor->get_Noise Model (). +// Matrix info = sqrtInfo.tra + + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << info(0, 0) << " " << info(0, 1) << " " << info(1, 1) << " " + << info(2, 2) << " " << info(0, 2) << " " << info(1, 2) << endl; + } + stream.close(); +} + /* ************************************************************************* */ bool readBAL(const string& filename, SfM_data &data) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 64ccd37b5..42c8e4f9a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -117,6 +117,16 @@ 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 readG2o2D(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction); + /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a * SfM_data structure