add dataset.h/cpp
Kai Ni
parent
ab155999cf
commit
e88ae4a944
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,206 @@
|
|||
/*
|
||||
* dataset.cpp
|
||||
*
|
||||
* Created on: Jan 22, 2010
|
||||
* Author: nikai
|
||||
* Description: utility functions for loading datasets
|
||||
*/
|
||||
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <cstdlib>
|
||||
#include "graph-inl.h"
|
||||
|
||||
#include "dataset.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define LINESIZE 81920
|
||||
|
||||
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
|
||||
typedef boost::shared_ptr<Pose2Config> sharedPose2Config;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, const string& user_path) {
|
||||
string path = user_path, set = dataset;
|
||||
boost::optional<SharedDiagonal> model;
|
||||
boost::optional<SharedDiagonal> 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<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
|
||||
int maxID, boost::optional<SharedDiagonal> 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<NonlinearFactor<Pose2Config> > factor_, graph) {
|
||||
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(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;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* dataset.h
|
||||
*
|
||||
* Created on: Jan 22, 2010
|
||||
* Author: nikai
|
||||
* Description: utility functions for loading datasets
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <string>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#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<std::string, boost::optional<gtsam::SharedDiagonal> >
|
||||
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<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Config> > load2D(
|
||||
const std::string& filename, int maxID = 0,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
|
||||
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
|
||||
Loading…
Reference in New Issue