From 2430b0bbf03949acc14eeb4d2666da9d8b5251fe Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:53:37 -0400 Subject: [PATCH] added functions to read/write g2o files --- gtsam/slam/dataset.cpp | 16 ++++++++++------ gtsam/slam/dataset.h | 11 ++++++++++- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6e0cf11ba..9a1fb3303 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -539,7 +539,7 @@ bool readBundler(const string& filename, SfM_data &data) /* ************************************************************************* */ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction = QUADRATIC){ + const kernelFunctionType kernelFunction){ ifstream is(g2oFile.c_str()); if (!is){ @@ -613,7 +613,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in } /* ************************************************************************* */ -bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ fstream stream(filename.c_str(), fstream::out); @@ -633,16 +633,20 @@ bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const V if (!factor) continue; -// Matrix sqrtInfo = factor->get_Noise Model (). -// Matrix info = sqrtInfo.tra + 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() << " " - << info(0, 0) << " " << info(0, 1) << " " << info(1, 1) << " " - << info(2, 2) << " " << info(0, 2) << " " << info(1, 2) << endl; + << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " + << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; } stream.close(); + return true; } /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 42c8e4f9a..7bbc88f70 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -125,7 +125,16 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); * @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); +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