12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

write correct information matrix to data file

release/4.3a0
Kai Ni 2010-01-22 23:20:12 +00:00
parent 2150b24e29
commit e87faa078d
1 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
cpp/dataset.cpp
View File

@ -27,22 +27,25 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, const string& user_path) { pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, const string& user_path) {
string path = user_path, set = dataset; string path = user_path, set = dataset;
boost::optional<SharedDiagonal> model; boost::optional<SharedDiagonal> null_model;
boost::optional<SharedDiagonal> identity = SharedDiagonal(Vector_(3, 1., 1., 1.)); boost::optional<SharedDiagonal> identity = SharedDiagonal(Vector_(3, 1., 1., 1.));
boost::optional<SharedDiagonal> small = SharedDiagonal(noiseModel::Diagonal::Variances(
gtsam::Vector_(3,0.0001,0.0001,0.0003), true));
if (path.empty()) path = string(getenv("HOME")) + "/"; if (path.empty()) path = string(getenv("HOME")) + "/";
if (set.empty()) set = string(getenv("DATASET")); if (set.empty()) set = string(getenv("DATASET"));
if (set == "intel") return make_pair(path + "data/iSAM/Laser/intel.graph", model); if (set == "intel") return make_pair(path + "data/iSAM/Laser/intel.graph", null_model);
if (set == "intel-gfs") return make_pair(path + "data/iSAM/Laser/intel.gfs.graph", model); if (set == "intel-gfs") return make_pair(path + "data/iSAM/Laser/intel.gfs.graph", null_model);
if (set == "Killian-gfs") return make_pair(path + "data/iSAM/Laser/Killian.gfs.graph", model); if (set == "Killian-gfs") return make_pair(path + "data/iSAM/Laser/Killian.gfs.graph", null_model);
if (set == "Killian") return make_pair(path + "data/iSAM/Laser/Killian.graph", model); if (set == "Killian") return make_pair(path + "data/iSAM/Laser/Killian.graph", small);
if (set == "Killian-noised") return make_pair(path + "data/iSAM/Laser/Killian-noised.graph", model); if (set == "Killian-noised") return make_pair(path + "data/iSAM/Laser/Killian-noised.graph", null_model);
if (set == "3") return make_pair(path + "borg/toro/data/2D/w3-odom.graph", identity); 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 == "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 == "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 == "olson") return make_pair(path + "data/iSAM/ISAM2/olson06icra.txt", null_model);
if (set == "victoria") return make_pair(path + "data/iSAM/ISAM2/victoria_park.txt", model); if (set == "victoria") return make_pair(path + "data/iSAM/ISAM2/victoria_park.txt", null_model);
return make_pair("unknown", model); return make_pair("unknown", null_model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -155,8 +158,8 @@ void save2D(const Pose2Graph& graph, const Pose2Config& config, const SharedDiag
pose = inverse(factor->measured()); pose = inverse(factor->measured());
stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
<< " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta()
<< " " << R(0, 0) << " " << R(0, 1) << " " << R(1, 1) << " " << R(2, 2) << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
<< " " << R(0, 2) << " " << R(1, 2) << endl; << " " << RR(0, 2) << " " << RR(1, 2) << endl;
} }
stream.close(); stream.close();