From e88ae4a944a593025b7a691bc5623f79712cef65 Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Fri, 22 Jan 2010 20:18:40 +0000 Subject: [PATCH] add dataset.h/cpp --- cpp/Makefile.am | 4 +- cpp/dataset.cpp | 206 ++++++++++++++++++++++++++++++++++++++++++++++++ cpp/dataset.h | 49 ++++++++++++ 3 files changed, 257 insertions(+), 2 deletions(-) create mode 100644 cpp/dataset.cpp create mode 100644 cpp/dataset.h diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 64e45123e..e2ee61518 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -187,8 +187,8 @@ headers += BetweenFactor.h PriorFactor.h headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h # 2D Pose SLAM -headers += -sources += pose2SLAM.cpp +headers += dataset.h +sources += pose2SLAM.cpp dataset.cpp check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior testPose2Prior_SOURCES = testPose2Prior.cpp testPose2Prior_LDADD = libgtsam.la diff --git a/cpp/dataset.cpp b/cpp/dataset.cpp new file mode 100644 index 000000000..a17195116 --- /dev/null +++ b/cpp/dataset.cpp @@ -0,0 +1,206 @@ +/* + * dataset.cpp + * + * Created on: Jan 22, 2010 + * Author: nikai + * Description: utility functions for loading datasets + */ + + +#include +#include +#include +#include "graph-inl.h" + +#include "dataset.h" + +using namespace std; +using namespace gtsam; + +#define LINESIZE 81920 + +typedef boost::shared_ptr sharedPose2Graph; +typedef boost::shared_ptr sharedPose2Config; + +namespace gtsam { + +/* ************************************************************************* */ +pair > dataset(const string& dataset, const string& user_path) { + string path = user_path, set = dataset; + boost::optional model; + boost::optional identity = SharedDiagonal(Vector_(3, 1., 1., 1.)); + + if (path.empty()) path = string(getenv("HOME")) + "/"; + if (set.empty()) set = string(getenv("DATASET")); + if (set == "intel") return make_pair(path + "data/iSAM/Laser/intel.graph", model); + if (set == "intel-gfs") return make_pair(path + "data/iSAM/Laser/intel.gfs.graph", model); + if (set == "Killian-gfs") return make_pair(path + "data/iSAM/Laser/Killian.gfs.graph", model); + if (set == "Killian") return make_pair(path + "data/iSAM/Laser/Killian.graph", model); + if (set == "Killian-noised") return make_pair(path + "data/iSAM/Laser/Killian-noised.graph", model); + if (set == "3") return make_pair(path + "borg/toro/data/2D/w3-odom.graph", identity); + if (set == "100") return make_pair(path + "borg/toro/data/2D/w100-odom.graph", identity); + if (set == "10K") return make_pair(path + "borg/toro/data/2D/w10000-odom.graph", identity); + if (set == "olson") return make_pair(path + "data/iSAM/ISAM2/olson06icra.txt", model); + if (set == "victoria") return make_pair(path + "data/iSAM/ISAM2/victoria_park.txt", model); + return make_pair("unknown", model); +} + +/* ************************************************************************* */ + +pair load2D(const string& filename, + int maxID, boost::optional model, bool addNoise, bool smart) { + cout << "Will try to read " << filename << endl; + ifstream is(filename.c_str()); + if (!is) { + cout << "load2D: can not find the file!"; + exit(-1); + } + + sharedPose2Config poses(new Pose2Config); + sharedPose2Graph graph(new Pose2Graph); + + // load the poses + bool firstPose; + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if ((tag == "VERTEX2") || (tag == "VERTEX")) { + int id; + double x, y, yaw; + ls >> id >> x >> y >> yaw; + // optional filter + if (maxID && id >= maxID) continue; + poses->insert(id, Pose2(x, y, yaw)); + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + // load the factors + Pose2 measured; + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { + int id1, id2; + double x, y, yaw; + ls >> id2 >> id1 >> x >> y >> yaw; + + Matrix m = eye(3); + ls >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2); + m(2, 0) = m(0, 2); + m(2, 1) = m(1, 2); + m(1, 0) = m(0, 1); + + // optional filter + if (maxID && (id1 >= maxID || id2 >= maxID)) continue; + + + measured = inverse(Pose2(x, y, yaw)); + +// SharedGaussian noise = noiseModel::Gaussian::Covariance(m, smart); + // hack use diagonal for now ! + //Vector variances = Vector_(3,m(1,1),m(2,2),m(3,3)); // UNITS in file ??? + if (!model) { + Vector variances = Vector_(3,0.0001,0.0001,0.0003); + model = noiseModel::Diagonal::Variances(variances, smart); + } + + if (addNoise) + measured = expmap(measured,(*model)->sample()); + + // Insert vertices if pure odometry file + if (!poses->exists(id1)) poses->insert(id1, Pose2()); + if (!poses->exists(id2)) poses->insert(id2, measured * poses->at(id1)); + + Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2,measured,*model)); + graph->push_back(factor); + } + } + + cout << "load2D read a graph file with " << poses->size() << " vertices and " + << graph->nrFactors() << " factors" << endl; + + return make_pair(graph, poses); +} + +/* ************************************************************************* */ +void save2D(const Pose2Graph& graph, const Pose2Config& config, const SharedDiagonal model, + const string& filename) { + typedef Pose2Config::Key Key; + + fstream stream(filename.c_str(), fstream::out); + + // save poses + Pose2Config::Key key; + Pose2 pose; + BOOST_FOREACH(boost::tie(key, pose), config) + stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + + // save edges + Matrix R = model->R(); + Matrix RR = prod(trans(R),R); + BOOST_FOREACH(boost::shared_ptr > factor_, graph) { + boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); + if (!factor) continue; + + pose = inverse(factor->measured()); + stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() + << " " << pose.x() << " " << pose.y() << " " << pose.theta() + << " " << R(0, 0) << " " << R(0, 1) << " " << R(1, 1) << " " << R(2, 2) + << " " << R(0, 2) << " " << R(1, 2) << endl; + } + + stream.close(); +} + +/* ************************************************************************* */ +bool load3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) return false; + + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX3") { + int id; + double x, y, z, roll, pitch, yaw; + ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + bool edgesOk = true; + while (is) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "EDGE3") { + int id1, id2; + double x, y, z, roll, pitch, yaw; + ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Matrix m = eye(6); + for (int i = 0; i < 6; i++) + for (int j = i; j < 6; j++) + ls >> m(i, j); + } + } + return true; +} +} diff --git a/cpp/dataset.h b/cpp/dataset.h new file mode 100644 index 000000000..3dc337d6e --- /dev/null +++ b/cpp/dataset.h @@ -0,0 +1,49 @@ +/* + * dataset.h + * + * Created on: Jan 22, 2010 + * Author: nikai + * Description: utility functions for loading datasets + */ + +#pragma once + + +#include +#include +#include "pose2SLAM.h" +#include "graph.h" + +namespace gtsam +{ +/** + * Construct dataset filename from short name + * Currently has "Killian" "intel.gfs", "10K", etc... + * @param filename + * @param optional dataset, if empty will try to getenv $DATASET + * @param optional path, if empty will try to getenv $HOME + */ +std::pair > + dataset(const std::string& dataset = "", const std::string& path = ""); + +/** + * Load TORO 2D Graph + * @param filename + * @param maxID, if non-zero cut out vertices >= maxID + * @param smart: try to reduce complexity of covariance to cheapest model + */ +std::pair, boost::shared_ptr > load2D( + const std::string& filename, int maxID = 0, + boost::optional model = boost::optional(), + bool addNoise=false, bool smart=true); + +/** save 2d graph */ +void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Config& config, const gtsam::SharedDiagonal model, + const std::string& filename); + +/** + * Load TORO 3D Graph + */ +bool load3D(const std::string& filename); + +} // namespace gtsam