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

Merge pull request #396 from borglab/feature/g2o-vertices 

Support for landmarks in g2o files
release/4.3a0
Varun Agrawal 2020-07-25 13:38:38 -04:00 committed by GitHub
parent a27fbd5e5b d2063d928e
commit e612495b85
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 171 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
examples/Data/example_with_vertices.g2o Normal file
View File

@ -0,0 +1,16 @@
VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 7782220156096217088 10 10 10
VERTEX_TRACKXYZ 7782220156096217089 -10 10 10
VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10
VERTEX_TRACKXYZ 7782220156096217091 10 -10 10
VERTEX_TRACKXYZ 7782220156096217092 10 10 -10
VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10
VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10
VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10

107
gtsam/slam/dataset.cpp
View File

@ -11,7 +11,10 @@
/**
* @file dataset.cpp
* @date Jan 22, 2010
* @author Kai Ni, Luca Carlone, Frank Dellaert
* @author Kai Ni
* @author Luca Carlone
* @author Frank Dellaert
* @author Varun Agrawal
* @brief utility functions for loading datasets
*/
@ -192,8 +195,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
}
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
return parseVertexPose(is, tag);
}
#endif
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
Key id;
double x, y, yaw;
@ -204,6 +214,18 @@ boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
}
}
/* ************************************************************************* */
boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string& tag) {
if (tag == "VERTEX_XY") {
Key id;
double x, y;
is >> id >> x >> y;
return IndexedLandmark(id, Point2(x, y));
} else {
return boost::none;
}
}
/* ************************************************************************* */
boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
@ -232,12 +254,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
string tag;
// load the poses
// load the poses and landmarks
while (!is.eof()) {
if (!(is >> tag))
break;
const auto indexed_pose = parseVertex(is, tag);
const auto indexed_pose = parseVertexPose(is, tag);
if (indexed_pose) {
Key id = indexed_pose->first;
@ -247,6 +269,16 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
initial->insert(id, indexed_pose->second);
}
const auto indexed_landmark = parseVertexLandmark(is, tag);
if (indexed_landmark) {
Key id = indexed_landmark->first;
// optional filter
if (maxID && id >= maxID)
continue;
initial->insert(id, indexed_landmark->second);
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
@ -429,7 +461,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
const string& filename) {
fstream stream(filename.c_str(), fstream::out);
// save 2D & 3D poses
// save 2D poses
for (const auto& key_value : estimate) {
auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
if (!p) continue;
@ -438,15 +470,34 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< pose.y() << " " << pose.theta() << endl;
}
// save 3D poses
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}
// save 2D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point2>*>(&key_value.value);
if (!p) continue;
const Point2& point = p->value();
stream << "VERTEX_XY " << key_value.key << " " << point.x() << " "
<< point.y() << endl;
}
// save 3D landmarks
for(const auto& key_value: estimate) {
auto p = dynamic_cast<const GenericValue<Point3>*>(&key_value.value);
if (!p) continue;
const Point3& point = p->value();
stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " "
<< point.y() << " " << point.z() << endl;
}
// save edges (2D or 3D)
@ -515,6 +566,7 @@ static Rot3 NormalizedRot3(double w, double x, double y, double z) {
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
return Rot3::Quaternion(f * w, f * x, f * y, f * z);
}
/* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str());
@ -545,6 +597,30 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
return poses;
}
/* ************************************************************************* */
std::map<Key, Point3> parse3DLandmarks(const string& filename) {
ifstream is(filename.c_str());
if (!is)
throw invalid_argument("parse3DLandmarks: can not find file " + filename);
std::map<Key, Point3> landmarks;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag == "VERTEX_TRACKXYZ") {
Key id;
double x, y, z;
ls >> id >> x >> y >> z;
landmarks.emplace(id, Point3(x, y, z));
}
}
return landmarks;
}
/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(
const string& filename,
@ -617,11 +693,16 @@ GraphAndValues load3D(const string& filename) {
graph->push_back(factor);
}
const auto poses = parse3DPoses(filename);
Values::shared_ptr initial(new Values);
const auto poses = parse3DPoses(filename);
for (const auto& key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
const auto landmarks = parse3DLandmarks(filename);
for (const auto& key_landmark : landmarks) {
initial->insert(key_landmark.first, key_landmark.second);
}
return make_pair(graph, initial);
}

25
gtsam/slam/dataset.h
View File

@ -76,8 +76,10 @@ enum KernelFunctionType {
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
@ -85,6 +87,24 @@ typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
const std::string& tag);
#endif
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
const std::string& tag);
/**
* Parse G2O landmark vertex "id x y"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag);
/**
* Parse TORO/G2O edge "id1 id2 x y yaw"
@ -162,9 +182,12 @@ using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO graph file into a map of Pose3s.
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);

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

@ -43,13 +43,13 @@ TEST(dataSet, findExampleDataFile) {
}
/* ************************************************************************* */
TEST( dataSet, parseVertex)
TEST( dataSet, parseVertexPose)
{
const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertex(is, tag);
const auto actual = parseVertexPose(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
@ -57,6 +57,21 @@ TEST( dataSet, parseVertex)
}
}
/* ************************************************************************* */
TEST( dataSet, parseVertexLandmark)
{
const string str = "VERTEX_XY 1 2.000000 3.000000";
istringstream is(str);
string tag;
EXPECT(is >> tag);
const auto actual = parseVertexLandmark(is, tag);
EXPECT(actual);
if (actual) {
EXPECT_LONGS_EQUAL(1, actual->first);
EXPECT(assert_equal(Point2(2, 3), actual->second));
}
}
/* ************************************************************************* */
TEST( dataSet, parseEdge)
{
@ -182,6 +197,12 @@ TEST(dataSet, readG2o3D) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parse3DLandmarks(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check graph version
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
@ -252,6 +273,19 @@ TEST(dataSet, readG2oCheckDeterminants) {
const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size());
}
/* ************************************************************************* */
TEST(dataSet, readG2oLandmarks) {
const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
// Check number of poses and landmarks. Should be 8 each.
const map<Key, Pose3> poses = parse3DPoses(g2oFile);
EXPECT_LONGS_EQUAL(8, poses.size());
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
EXPECT_LONGS_EQUAL(8, landmarks.size());
}
/* ************************************************************************* */

2
tests/testNonlinearISAM.cpp
View File

@ -288,7 +288,7 @@ TEST(testNonlinearISAM, loop_closures ) {
break;
// Check if vertex
const auto indexedPose = parseVertex(is, tag);
const auto indexedPose = parseVertexPose(is, tag);
if (indexedPose) {
Key id = indexedPose->first;
initialEstimate.insert(Symbol('x', id), indexedPose->second);