Templated some methods internally

gtsam/slam/dataset.cpp

@@ -105,10 +105,10 @@ string createRewrittenFileName(const string& name) {
 }
 
 /* ************************************************************************* */
-GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
+GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, Key maxNr,
     bool addNoise, bool smart, NoiseFormat noiseFormat,
     KernelFunctionType kernelFunctionType) {
-  return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
+  return load2D(dataset.first, dataset.second, maxNr, addNoise, smart,
       noiseFormat, kernelFunctionType);
 }
 
@@ -226,6 +226,54 @@ boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string&
   }
 }
 
+/* ************************************************************************* */
+// Parse types T into a Key-indexed map
+template <typename T>
+static map<Key, T> parseIntoMap(const string &filename, Key maxNr = 0) {
+  ifstream is(filename.c_str());
+  if (!is)
+    throw invalid_argument("parse: can not find file " + filename);
+
+  map<Key, T> result;
+  string tag;
+  while (!is.eof()) {
+    boost::optional<pair<Key, T>> indexed_t;
+    is >> indexed_t;
+    if (indexed_t) {
+      // optional filter
+      if (maxNr && indexed_t->first >= maxNr)
+        continue;
+      result.insert(*indexed_t);
+    }
+    is.ignore(LINESIZE, '\n');
+  }
+  return result;
+}
+
+/* ************************************************************************* */
+istream &operator>>(istream &is, boost::optional<IndexedPose> &indexed) {
+  string tag;
+  is >> tag;
+  indexed = parseVertexPose(is, tag);
+  return is;
+}
+
+map<Key, Pose2> parse2DPoses(const string &filename, Key maxNr) {
+  return parseIntoMap<Pose2>(filename, maxNr);
+}
+
+/* ************************************************************************* */
+istream &operator>>(istream &is, boost::optional<IndexedLandmark> &indexed) {
+  string tag;
+  is >> tag;
+  indexed = parseVertexLandmark(is, tag);
+  return is;
+}
+
+map<Key, Point2> parse2DLandmarks(const string &filename, Key maxNr) {
+  return parseIntoMap<Point2>(filename, maxNr);
+}
+
 /* ************************************************************************* */
 boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
   if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
@@ -241,49 +289,27 @@ boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
 }
 
 /* ************************************************************************* */
-GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
+GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
                       bool addNoise, bool smart, NoiseFormat noiseFormat,
                       KernelFunctionType kernelFunctionType) {
 
+  Values::shared_ptr initial(new Values);
+
+  const auto poses = parse2DPoses(filename, maxNr);
+  for (const auto& key_pose : poses) {
+    initial->insert(key_pose.first, key_pose.second);
+  }
+  const auto landmarks = parse2DLandmarks(filename, maxNr);
+  for (const auto& key_landmark : landmarks) {
+    initial->insert(key_landmark.first, key_landmark.second);
+  }
+
   ifstream is(filename.c_str());
   if (!is)
     throw invalid_argument("load2D: can not find file " + filename);
 
-  Values::shared_ptr initial(new Values);
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
-  string tag;
-
-  // load the poses and landmarks
-  while (!is.eof()) {
-    if (!(is >> tag))
-      break;
-
-    const auto indexed_pose = parseVertexPose(is, tag);
-    if (indexed_pose) {
-      Key id = indexed_pose->first;
-
-      // optional filter
-      if (maxID && id >= maxID)
-        continue;
-
-      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 */
-  is.seekg(0, ios::beg);
-
   // If asked, create a sampler with random number generator
   std::unique_ptr<Sampler> sampler;
   if (addNoise) {
@@ -302,6 +328,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
   bool haveLandmark = false;
   const bool useModelInFile = !model;
   while (!is.eof()) {
+    string tag;
     if (!(is >> tag))
       break;
 
@@ -315,7 +342,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
           kernelFunctionType);
 
       // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
+      if (maxNr && (id1 >= maxNr || id2 >= maxNr))
         continue;
 
       if (useModelInFile)
@@ -375,7 +402,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     if (tag == "LANDMARK" || tag == "BR") {
 
       // optional filter
-      if (maxID && id1 >= maxID)
+      if (maxNr && id1 >= maxNr)
         continue;
 
       // Create noise model
@@ -404,8 +431,8 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
 
 /* ************************************************************************* */
 GraphAndValues load2D_robust(const string& filename,
-    noiseModel::Base::shared_ptr& model, int maxID) {
+    noiseModel::Base::shared_ptr& model, Key maxNr) {
-  return load2D(filename, model, maxID);
+  return load2D(filename, model, maxNr);
 }
 
 /* ************************************************************************* */
@@ -448,10 +475,10 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D,
     return load3D(g2oFile);
   } else {
     // just call load2D
-    int maxID = 0;
+    Key maxNr = 0;
     bool addNoise = false;
     bool smart = true;
-    return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+    return load2D(g2oFile, SharedNoiseModel(), maxNr, addNoise, smart,
                   NoiseFormatG2O, kernelFunctionType);
   }
 }
@@ -516,8 +543,8 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
       Pose2 pose = factor->measured(); //.inverse();
       stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
           << pose.x() << " " << pose.y() << " " << pose.theta();
-      for (int i = 0; i < 3; i++){
+      for (size_t i = 0; i < 3; i++){
-        for (int j = i; j < 3; j++){
+        for (size_t j = i; j < 3; j++){
           stream << " " << Info(i, j);
         }
       }
@@ -545,13 +572,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
              << " " << q.y() << " " << q.z() << " " << q.w();
 
       Matrix InfoG2o = I_6x6;
-      InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
+      InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation
-      InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
+      InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation
-      InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
+      InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal
-      InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal
+      InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal
 
-      for (int i = 0; i < 6; i++){
+      for (size_t i = 0; i < 6; i++){
-        for (int j = i; j < 6; j++){
+        for (size_t j = i; j < 6; j++){
           stream << " " << InfoG2o(i, j);
         }
       }
@@ -562,126 +589,157 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
 }
 
 /* ************************************************************************* */
-static Rot3 NormalizedRot3(double w, double x, double y, double z) {
+// parse quaternion in x,y,z,w order, and normalize to unit length
+istream &operator>>(istream &is, Quaternion &q) {
+  double x, y, z, w;
+  is >> x >> y >> z >> w;
   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);
+  q = Quaternion(f * w, f * x, f * y, f * z);
+  return is;
 }
 
 /* ************************************************************************* */
-std::map<Key, Pose3> parse3DPoses(const string& filename) {
+// parse Rot3 from roll, pitch, yaw
-  ifstream is(filename.c_str());
+istream &operator>>(istream &is, Rot3 &R) {
-  if (!is)
+  double yaw, pitch, roll;
-    throw invalid_argument("parse3DPoses: can not find file " + filename);
+  is >> roll >> pitch >> yaw;  // notice order !
+  R = Rot3::Ypr(yaw, pitch, roll);
+  return is;
+}
 
-  std::map<Key, Pose3> poses;
+/* ************************************************************************* */
-  while (!is.eof()) {
+istream &operator>>(istream &is, boost::optional<pair<Key, Pose3>> &indexed) {
-    char buf[LINESIZE];
-    is.getline(buf, LINESIZE);
-    istringstream ls(buf);
   string tag;
-    ls >> tag;
+  is >> tag;
-
   if (tag == "VERTEX3") {
     Key id;
-      double x, y, z, roll, pitch, yaw;
+    double x, y, z;
-      ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
+    Rot3 R;
-      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
+    is >> id >> x >> y >> z >> R;
-    }
+    indexed.reset({id, Pose3(R, {x, y, z})});
-    if (tag == "VERTEX_SE3:QUAT") {
+  } else if (tag == "VERTEX_SE3:QUAT") {
     Key id;
-      double x, y, z, qx, qy, qz, qw;
+    double x, y, z;
-      ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
+    Quaternion q;
-      poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z}));
+    is >> id >> x >> y >> z >> q;
+    indexed.reset({id, Pose3(q, {x, y, z})});
   }
+  return is;
 }
-  return poses;
+
+map<Key, Pose3> parse3DPoses(const string &filename, Key maxNr) {
+  return parseIntoMap<Pose3>(filename, maxNr);
 }
 
 /* ************************************************************************* */
-std::map<Key, Point3> parse3DLandmarks(const string& filename) {
+istream &operator>>(istream &is, boost::optional<pair<Key, Point3>> &indexed) {
-  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;
+  is >> tag;
-
   if (tag == "VERTEX_TRACKXYZ") {
     Key id;
     double x, y, z;
-      ls >> id >> x >> y >> z;
+    is >> id >> x >> y >> z;
-      landmarks.emplace(id, Point3(x, y, z));
+    indexed.reset({id, Point3(x, y, z)});
   }
+  return is;
 }
-  return landmarks;
+
+map<Key, Point3> parse3DLandmarks(const string &filename, Key maxNr) {
+  return parseIntoMap<Point3>(filename, maxNr);
 }
 
 /* ************************************************************************* */
-BetweenFactorPose3s parse3DFactors(
+// Parse BetweenFactor<T> shared pointers into a vector
-    const string& filename,
+template <typename T>
-    const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
+static vector<boost::shared_ptr<BetweenFactor<T>>>
+parseIntoVector(const string &filename, Key maxNr = 0) {
   ifstream is(filename.c_str());
-  if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
+  if (!is)
+    throw invalid_argument("parse: can not find file " + filename);
 
-  boost::optional<Sampler> sampler;
+  vector<boost::shared_ptr<BetweenFactor<T>>> result;
-  if (corruptingNoise) {
+  string tag;
-    sampler = Sampler(corruptingNoise);
+  while (!is.eof()) {
+    boost::shared_ptr<BetweenFactor<T>> shared;
+    is >> shared;
+    if (shared) {
+      // optional filter
+      if (maxNr && (shared->key1() >= maxNr || shared->key1() >= maxNr))
+        continue;
+      result.push_back(shared);
+    }
+    is.ignore(LINESIZE, '\n');
+  }
+  return result;
 }
 
-  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
+/* ************************************************************************* */
-  while (!is.eof()) {
+// Parse a symmetric covariance matrix (onlyupper-triangular part is stored)
-    char buf[LINESIZE];
+istream &operator>>(istream &is, Matrix6 &m) {
-    is.getline(buf, LINESIZE);
+  for (size_t i = 0; i < 6; i++)
-    istringstream ls(buf);
+    for (size_t j = i; j < 6; j++){
-    string tag;
+      is >> m(i, j);
-    ls >> tag;
+      m(j,i) = m(i,j);
+  }
+  return is;
+}
 
+/* ************************************************************************* */
+istream &operator>>(istream &is,
+                    boost::shared_ptr<BetweenFactor<Pose3>> &shared) {
+  string tag;
+  is >> tag;
+
+  Matrix6 m;
   if (tag == "EDGE3") {
     Key id1, id2;
-      double x, y, z, roll, pitch, yaw;
+    double x, y, z;
-      ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
+    Rot3 R;
-      Matrix m(6, 6);
+    is >> id1 >> id2 >> x >> y >> z >> R;
-      for (size_t i = 0; i < 6; i++)
+    Pose3 pose(R, {x, y, z});
-        for (size_t j = i; j < 6; j++) ls >> m(i, j);
+
+    is >> m;
     SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      factors.emplace_back(new BetweenFactor<Pose3>(
+    shared.reset(new BetweenFactor<Pose3>(id1, id2, pose, model));
-          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;
+    double x, y, z;
-      ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
+    Quaternion q;
-      Matrix m(6, 6);
+    is >> id1 >> id2 >> x >> y >> z >> q;
-      for (size_t i = 0; i < 6; i++) {
+    Pose3 pose(q, {x, y, z});
-        for (size_t j = i; j < 6; j++) {
-          double mij;
-          ls >> mij;
-          m(i, j) = mij;
-          m(j, i) = mij;
-        }
-      }
-      Matrix mgtsam(6, 6);
 
+    // EDGE_SE3:QUAT stores covariance in t,R order, unlike GTSAM:
+    is >> m;
+    Matrix6 mgtsam;
     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);
-      auto R12 = NormalizedRot3(qw, qx, qy, qz);
+
-      if (sampler) {
+    shared.reset(new BetweenFactor<Pose3>(id1, id2, pose, model));
-        R12 = R12.retract(sampler->sample());
+  }
+  return is;
 }
 
-      factors.emplace_back(new BetweenFactor<Pose3>(
+/* ************************************************************************* */
-          id1, id2, Pose3(R12, {x, y, z}), model));
+BetweenFactorPose3s
+parse3DFactors(const string &filename,
+               const noiseModel::Diagonal::shared_ptr &corruptingNoise,
+               Key maxNr) {
+  auto factors =  parseIntoVector<Pose3>(filename, maxNr);
+
+  if (corruptingNoise) {
+    Sampler sampler(corruptingNoise);
+    for (auto factor : factors) {
+      auto pose = factor->measured();
+      factor.reset(new BetweenFactor<Pose3>(factor->key1(), factor->key2(),
+                                            pose.retract(sampler.sample()),
+                                            factor->noiseModel()));
     }
   }
+
   return factors;
 }
 

gtsam/slam/dataset.h

@@ -23,10 +23,8 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Rot3.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/NoiseModel.h>
@@ -114,18 +112,35 @@ GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream&
 GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
     const std::string& tag);
 
+using BetweenFactorPose2s =
+    std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>;
+
+/// Parse edges in 2D g2o graph file into a set of BetweenFactors.
+GTSAM_EXPORT BetweenFactorPose2s parse2DFactors(
+    const std::string &filename,
+    const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr);
+
+/// Parse vertices in 2D g2o graph file into a map of Pose2s.
+GTSAM_EXPORT std::map<Key, Pose2> parse2DPoses(const std::string &filename,
+                                               Key maxNr = 0);
+
+/// Parse landmarks in 2D g2o graph file into a map of Point2s.
+GTSAM_EXPORT std::map<Key, Point2> parse2DLandmarks(const string &filename,
+                                                    Key maxNr = 0);
+
 /// Return type for load functions
-typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
+using GraphAndValues =
+    std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
 
 /**
  * Load TORO 2D Graph
  * @param dataset/model pair as constructed by [dataset]
- * @param maxID if non-zero cut out vertices >= maxID
+ * @param maxNr if non-zero cut out vertices >= maxNr
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  */
 GTSAM_EXPORT GraphAndValues load2D(
-    std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
+    std::pair<std::string, SharedNoiseModel> dataset, Key maxNr = 0,
     bool addNoise = false,
     bool smart = true, //
     NoiseFormat noiseFormat = NoiseFormatAUTO,
@@ -135,7 +150,7 @@ GTSAM_EXPORT GraphAndValues load2D(
  * Load TORO/G2O style graph files
  * @param filename
  * @param model optional noise model to use instead of one specified by file
- * @param maxID if non-zero cut out vertices >= maxID
+ * @param maxNr if non-zero cut out vertices >= maxNr
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  * @param noiseFormat how noise parameters are stored
@@ -143,13 +158,13 @@ GTSAM_EXPORT GraphAndValues load2D(
  * @return graph and initial values
  */
 GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
-    SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise =
+    SharedNoiseModel model = SharedNoiseModel(), Key maxNr = 0, bool addNoise =
         false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
     KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
 GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
-    noiseModel::Base::shared_ptr& model, int maxID = 0);
+    noiseModel::Base::shared_ptr& model, Key maxNr = 0);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@@ -179,14 +194,18 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 
 /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
 using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
-GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, 
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(
-    const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
+    const std::string &filename,
+    const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr,
+    Key maxNr = 0);
 
 /// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
-GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
+GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string &filename,
+                                               Key maxNr = 0);
 
 /// Parse landmarks in 3D g2o graph file into a map of Point3s.
-GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
+GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const std::string &filename,
+                                                    Key maxNr = 0);
 
 /// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);

gtsam/slam/tests/testDataset.cpp

@@ -89,8 +89,7 @@ TEST( dataSet, parseEdge)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, load2D)
+TEST(dataSet, load2D) {
-{
   ///< The structure where we will save the SfM data
   const string filename = findExampleDataFile("w100.graph");
   NonlinearFactorGraph::shared_ptr graph;
@@ -98,11 +97,30 @@ TEST( dataSet, load2D)
   boost::tie(graph, initial) = load2D(filename);
   EXPECT_LONGS_EQUAL(300, graph->size());
   EXPECT_LONGS_EQUAL(100, initial->size());
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
+  auto model = noiseModel::Unit::Create(3);
-  BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model);
+  BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
-  BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast<
+                                model);
-      BetweenFactor<Pose2> >(graph->at(0));
+  BetweenFactor<Pose2>::shared_ptr actual =
+      boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
   EXPECT(assert_equal(expected, *actual));
+
+  // // Check factor parsing
+  // const auto actualFactors = parse2DFactors(filename);
+  // for (size_t i : {0, 1, 2, 3, 4, 5}) {
+  //   EXPECT(assert_equal(
+  //       *boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
+  //       *actualFactors[i], 1e-5));
+  // }
+
+  // Check pose parsing
+  const auto actualPoses = parse2DPoses(filename);
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(initial->at<Pose2>(j), actualPoses.at(j), 1e-5));
+  }
+
+  // Check landmark parsing
+  const auto actualLandmarks = parse2DLandmarks(filename);
+  EXPECT_LONGS_EQUAL(0, actualLandmarks.size());
 }
 
 /* ************************************************************************* */