Add edge/vertex parsing from any input stream

gtsam/slam/dataset.cpp

@@ -195,6 +195,33 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   }
 }
 
+/* ************************************************************************* */
+boost::optional<pair<Key, Pose2> > parseVertex(istream& is, const string& tag) {
+  if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
+    Key id;
+    double x, y, yaw;
+    is >> id >> x >> y >> yaw;
+    return pair<Key, Pose2>(id, Pose2(x, y, yaw));
+  } else {
+    return boost::none;
+  }
+}
+
+/* ************************************************************************* */
+boost::optional<pair<pair<Key, Key>, Pose2> > parseEdge(istream& is, const string& tag) {
+  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
+      || (tag == "ODOMETRY")) {
+
+    Key id1, id2;
+    double x, y, yaw;
+    is >> id1 >> id2 >> x >> y >> yaw;
+    return pair<pair<Key, Key>, Pose2>(pair<Key, Key>(id1, id2),
+        Pose2(x, y, yaw));
+  } else {
+    return boost::none;
+  }
+}
+
 /* ************************************************************************* */
 GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     bool addNoise, bool smart, NoiseFormat noiseFormat,
@@ -214,16 +241,15 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     if (!(is >> tag))
       break;
 
-    if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
+    const auto indexed_pose = parseVertex(is, tag);
-      Key id;
+    if (indexed_pose) {
-      double x, y, yaw;
+      Key id = indexed_pose->first;
-      is >> id >> x >> y >> yaw;
 
       // optional filter
       if (maxID && id >= maxID)
         continue;
 
-      initial->insert(id, Pose2(x, y, yaw));
+      initial->insert(id, indexed_pose->second);
     }
     is.ignore(LINESIZE, '\n');
   }
@@ -251,13 +277,10 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     if (!(is >> tag))
       break;
 
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
+    auto between_pose = parseEdge(is, tag);
-        || (tag == "ODOMETRY")) {
+    if (between_pose) {
-
+      std::tie(id1, id2) = between_pose->first;
-      // Read transform
+      Pose2& l1Xl2 = between_pose->second;
-      double x, y, yaw;
-      is >> id1 >> id2 >> x >> y >> yaw;
-      Pose2 l1Xl2(x, y, yaw);
 
       // read noise model
       SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,

gtsam/slam/dataset.h

@@ -33,6 +33,7 @@
 #include <string>
 #include <utility> // for pair
 #include <vector>
+#include <iosfwd>
 
 namespace gtsam {
 
@@ -88,6 +89,22 @@ GTSAM_EXPORT GraphAndValues load2D(
     NoiseFormat noiseFormat = NoiseFormatAUTO,
     KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
+/**
+ * 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
+ */
+boost::optional<std::pair<Key, Pose2> > parseVertex(std::istream& is,
+    const std::string& tag);
+
+/**
+ * Parse TORO/G2O edge "id1 id2 x y yaw"
+ * @param is input stream
+ * @param tag string parsed from input stream, will only parse if edge type
+ */
+boost::optional<std::pair<std::pair<Key, Key>, Pose2> > parseEdge(
+    std::istream& is, const std::string& tag);
+
 /**
  * Load TORO/G2O style graph files
  * @param filename

gtsam/slam/tests/testDataset.cpp

@@ -26,6 +26,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <iostream>
+#include <sstream>
+
 using namespace gtsam::symbol_shorthand;
 using namespace std;
 using namespace gtsam;
@@ -39,6 +42,37 @@ TEST(dataSet, findExampleDataFile) {
   EXPECT(assert_equal(expected_end, actual_end));
 }
 
+/* ************************************************************************* */
+TEST( dataSet, parseVertex)
+{
+  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);
+  EXPECT(actual);
+  if (actual) {
+    EXPECT_LONGS_EQUAL(1, actual->first);
+    EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
+  }
+}
+
+/* ************************************************************************* */
+TEST( dataSet, parseEdge)
+{
+  const string str = "EDGE2 0 1 2.000000 3.000000 4.000000";
+  istringstream is(str);
+  string tag;
+  EXPECT(is >> tag);
+  const auto actual = parseEdge(is, tag);
+  EXPECT(actual);
+  if (actual) {
+    pair<Key, Key> expected(0, 1);
+    EXPECT(expected == actual->first);
+    EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
+  }
+}
+
 /* ************************************************************************* */
 TEST( dataSet, load2D)
 {