Working parseG2o3D

release/4.3a0
Frank Dellaert 2019-03-14 00:27:02 -04:00
parent 0ca3f9d199
commit d8ee79fb8f
1 changed files with 135 additions and 32 deletions

View File

@ -162,33 +162,96 @@ TEST( dataSet, readG2o)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(dataSet, readG2o3D) { using BetweenFactors = std::vector<BetweenFactor<Pose3>::shared_ptr>;
const string g2oFile = findExampleDataFile("pose3example"); using Poses = std::map<Key, Pose3>;
NonlinearFactorGraph::shared_ptr actualGraph; std::pair<BetweenFactors, Poses> parseG2o3D(const std::string&);
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
// Initialize 5 poses with quaternion/point3 values: #include <fstream>
const std::vector<Pose3> poses = { #define LINESIZE 81920
{{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
{{0.854230, 0.190253, 0.283162, -0.392318},
{1.001367, 0.015390, 0.004948}},
{{0.421446, -0.351729, -0.597838, 0.584174},
{1.993500, 0.023275, 0.003793}},
{{0.067024, 0.331798, -0.200659, 0.919323},
{2.004291, 1.024305, 0.018047}},
{{0.765488, -0.035697, -0.462490, 0.445933},
{0.999908, 1.055073, 0.020212}},
};
// Check values /* ************************************************************************* */
for (size_t i : {0, 1, 2, 3, 4}) { std::pair<BetweenFactors, Poses> parseG2o3D(const string& filename) {
EXPECT(assert_equal(poses[i], actualValues->at<Pose3>(i), 1e-5)); 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) {
const string g2oFile = findExampleDataFile("pose3example");
auto model = noiseModel::Isotropic::Precision(6, 10000);
// Initialize 6 relative measurements with quaternion/point3 values: // Initialize 6 relative measurements with quaternion/point3 values:
const std::vector<Pose3> measurements = { const std::vector<Pose3> relative_poses = {
{{0.854230, 0.190253, 0.283162, -0.392318}, {{0.854230, 0.190253, 0.283162, -0.392318},
{1.001367, 0.015390, 0.004948}}, {1.001367, 0.015390, 0.004948}},
{{0.105373, 0.311512, 0.656877, -0.678505}, {{0.105373, 0.311512, 0.656877, -0.678505},
@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) {
{-0.623267, 0.086928, 0.773222}}, {-0.623267, 0.086928, 0.773222}},
}; };
// Specify connectivity: // Initialize 5 poses with quaternion/point3 values:
std::vector<pair<Key, Key>> pairs = {{0, 1}, {1, 2}, {2, 3}, const std::vector<Pose3> poses = {
{3, 4}, {1, 4}, {3, 0}}; {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
{{0.854230, 0.190253, 0.283162, -0.392318},
{1.001367, 0.015390, 0.004948}},
{{0.421446, -0.351729, -0.597838, 0.584174},
{1.993500, 0.023275, 0.003793}},
{{0.067024, 0.331798, -0.200659, 0.919323},
{2.004291, 1.024305, 0.018047}},
{{0.765488, -0.035697, -0.462490, 0.445933},
{0.999908, 1.055073, 0.020212}},
};
// Create expected factor graph // Specify connectivity:
auto model = noiseModel::Isotropic::Precision(6, 10000); using KeyPair = pair<Key, Key>;
NonlinearFactorGraph expectedGraph; std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
// Created expected factor graph
size_t i = 0; size_t i = 0;
for (const auto& keys : pairs) { NonlinearFactorGraph expectedGraph;
for (const auto& keys : edges) {
expectedGraph.emplace_shared<BetweenFactor<Pose3>>( expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
keys.first, keys.second, measurements[i++], model); keys.first, keys.second, relative_poses[i++], model);
} }
// Check if actual graph is correct // Call parse version
BetweenFactors actualFactors;
Poses actualPoses;
std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile);
// Check factors
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*actualFactors[i], 1e-5));
}
// Check poses
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Call graph version
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
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}) {
EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
}
} }
/* ************************************************************************* */ /* ************************************************************************* */