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

Split parsing and moved to dataset.*

release/4.3a0
Frank Dellaert 2019-03-14 00:58:22 -04:00
parent d8ee79fb8f
commit a47c52cb5e
3 changed files with 67 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
gtsam/slam/dataset.cpp
View File

@ -410,16 +410,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues readG2o(const string& g2oFile, const bool is3D, GraphAndValues readG2o(const string& g2oFile, const bool is3D,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
if (is3D) {
return load3D(g2oFile);
} else {
// just call load2D // just call load2D
int maxID = 0; int maxID = 0;
bool addNoise = false; bool addNoise = false;
bool smart = true; bool smart = true;
if(is3D)
return load3D(g2oFile);
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
NoiseFormatG2O, kernelFunctionType); NoiseFormatG2O, kernelFunctionType);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load3D(const string& filename) { std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) if (!is)
throw invalid_argument("load3D: can not find file " + filename); throw invalid_argument("parse3DPoses: can not find file " + filename);
Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
std::map<Key, Pose3> poses;
while (!is.eof()) { while (!is.eof()) {
char buf[LINESIZE]; char buf[LINESIZE];
is.getline(buf, LINESIZE); is.getline(buf, LINESIZE);
@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) {
Key id; Key id;
double x, y, z, roll, pitch, yaw; double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll); poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
} }
if (tag == "VERTEX_SE3:QUAT") { if (tag == "VERTEX_SE3:QUAT") {
Key id; Key id;
double x, y, z, qx, qy, qz, qw; double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
} }
} }
is.clear(); /* clears the end-of-file and error flags */ return poses;
is.seekg(0, ios::beg); }
/* ************************************************************************* */
std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const string& filename) {
ifstream is(filename.c_str());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
while (!is.eof()) { while (!is.eof()) {
char buf[LINESIZE]; char buf[LINESIZE];
is.getline(buf, LINESIZE); is.getline(buf, LINESIZE);
@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) {
Key id1, id2; Key id1, id2;
double x, y, z, roll, pitch, yaw; double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll); Matrix m(6, 6);
Point3 t = Point3(x, y, z); for (size_t i = 0; i < 6; i++)
Matrix m = I_6x6; for (size_t j = i; j < 6; j++) ls >> m(i, j);
for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++)
ls >> m(i, j);
SharedNoiseModel model = noiseModel::Gaussian::Information(m); SharedNoiseModel model = noiseModel::Gaussian::Information(m);
NonlinearFactor::shared_ptr factor( factors.emplace_back(new BetweenFactor<Pose3>(
new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model)); id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
graph->push_back(factor);
} }
if (tag == "EDGE_SE3:QUAT") { if (tag == "EDGE_SE3:QUAT") {
Matrix m = I_6x6;
Key id1, id2; Key id1, id2;
double x, y, z, qx, qy, qz, qw; double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Matrix m(6, 6);
Point3 t = Point3(x, y, z); for (size_t i = 0; i < 6; i++) {
for (int i = 0; i < 6; i++){ for (size_t j = i; j < 6; j++) {
for (int j = i; j < 6; j++){
double mij; double mij;
ls >> mij; ls >> mij;
m(i, j) = mij; m(i, j) = mij;
m(j, i) = mij; m(j, i) = mij;
} }
} }
Matrix mgtsam = I_6x6; Matrix mgtsam(6, 6);
mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation
mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation
mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal
mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model)); factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
}
}
return factors;
}
/* ************************************************************************* */
GraphAndValues load3D(const string& filename) {
const auto factors = parse3DFactors(filename);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
for (const auto& factor : factors) {
graph->push_back(factor); graph->push_back(factor);
} }
const auto poses = parse3DPoses(filename);
Values::shared_ptr initial(new Values);
for (const auto& key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
} }
return make_pair(graph, initial); return make_pair(graph, initial);
} }

10
gtsam/slam/dataset.h
View File

@ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename); const Values& estimate, const std::string& filename);
/** /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
* Load TORO 3D Graph GTSAM_EXPORT std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename);
*/
/// Parse vertices in 3D TORO graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index /// A measurement with its camera index

101
gtsam/slam/tests/testDataset.cpp
View File

@ -162,90 +162,6 @@ TEST( dataSet, readG2o)
} }
/* ************************************************************************* */ /* ************************************************************************* */
using BetweenFactors = std::vector<BetweenFactor<Pose3>::shared_ptr>;
using Poses = std::map<Key, Pose3>;
std::pair<BetweenFactors, Poses> parseG2o3D(const std::string&);
#include <fstream>
#define LINESIZE 81920
/* ************************************************************************* */
std::pair<BetweenFactors, Poses> parseG2o3D(const string& filename) {
ifstream is(filename.c_str());
if (!is) throw invalid_argument("load3D: can not find file " + filename);
BetweenFactors factors;
Poses poses;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "VERTEX3") {
Key id;
double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
}
if (tag == "VERTEX_SE3:QUAT") {
Key id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
}
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "EDGE3") {
Key id1, id2;
double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Matrix m(6, 6);
for (size_t i = 0; i < 6; i++)
for (size_t j = i; j < 6; j++) ls >> m(i, j);
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
}
if (tag == "EDGE_SE3:QUAT") {
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Matrix m(6, 6);
for (size_t i = 0; i < 6; i++) {
for (size_t j = i; j < 6; j++) {
double mij;
ls >> mij;
m(i, j) = mij;
m(j, i) = mij;
}
}
Matrix mgtsam(6, 6);
mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation
mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation
mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
}
}
return make_pair(factors, poses);
}
TEST(dataSet, readG2o3D) { TEST(dataSet, readG2o3D) {
const string g2oFile = findExampleDataFile("pose3example"); const string g2oFile = findExampleDataFile("pose3example");
auto model = noiseModel::Isotropic::Precision(6, 10000); auto model = noiseModel::Isotropic::Precision(6, 10000);
@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) {
keys.first, keys.second, relative_poses[i++], model); keys.first, keys.second, relative_poses[i++], model);
} }
// Call parse version // Check factor parsing
BetweenFactors actualFactors; const auto actualFactors = parse3DFactors(g2oFile);
Poses actualPoses;
std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile);
// Check factors
for (size_t i : {0, 1, 2, 3, 4, 5}) { for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal( EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]), *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*actualFactors[i], 1e-5)); *actualFactors[i], 1e-5));
} }
// Check poses // Check pose parsing
const auto actualPoses = parse3DPoses(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) { for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
} }
// Call graph version // Check graph version
NonlinearFactorGraph::shared_ptr actualGraph; NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues; Values::shared_ptr actualValues;
bool is3D = true; bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
// Check factor graph
EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
// Check values
for (size_t j : {0, 1, 2, 3, 4}) { for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5)); EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
} }