diff --git a/examples/Data/example_with_vertices.g2o b/examples/Data/example_with_vertices.g2o new file mode 100644 index 000000000..6c2f1a530 --- /dev/null +++ b/examples/Data/example_with_vertices.g2o @@ -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 diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 136b7a93c..b6b1776d2 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -277,6 +277,17 @@ TEST(dataSet, readG2oCheckDeterminants) { 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 poses = parse3DPoses(g2oFile); + EXPECT_LONGS_EQUAL(8, poses.size()); + const map landmarks = parse3DLandmarks(g2oFile); + EXPECT_LONGS_EQUAL(8, landmarks.size()); +} + /* ************************************************************************* */ static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) { NonlinearFactorGraph g;