Very generic parseToVector

gtsam/slam/dataset.cpp

@@ -21,21 +21,25 @@
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Rot3.h>
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/Values-inl.h>
+
+#include <gtsam/linear/Sampler.h>
+
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/nonlinear/Values-inl.h>
-#include <gtsam/linear/Sampler.h>
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/types.h>
 #include <gtsam/base/Value.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/types.h>
 
 #include <boost/assign/list_inserter.hpp>
 #include <boost/filesystem/operations.hpp>
@@ -56,7 +60,7 @@ using namespace gtsam::symbol_shorthand;
 namespace gtsam {
 
 /* ************************************************************************* */
-string findExampleDataFile(const string& name) {
+string findExampleDataFile(const string &name) {
   // Search source tree and installed location
   vector<string> rootsToSearch;
 
@@ -73,33 +77,32 @@ string findExampleDataFile(const string& name) {
   namesToSearch.push_back(name + ".xml");
 
   // Find first name that exists
-  for(const fs::path root: rootsToSearch) {
+  for (const fs::path root : rootsToSearch) {
-    for(const fs::path name: namesToSearch) {
+    for (const fs::path name : namesToSearch) {
       if (fs::is_regular_file(root / name))
         return (root / name).string();
     }
   }
 
   // If we did not return already, then we did not find the file
-  throw invalid_argument(
+  throw invalid_argument("gtsam::findExampleDataFile could not find a matching "
-      "gtsam::findExampleDataFile could not find a matching file in\n"
+                         "file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
-      GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
+                         " or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" +
-      GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name
+                         name + ", " + name + ".graph, or " + name + ".txt");
-          + ".graph, or " + name + ".txt");
 }
 
 /* ************************************************************************* */
-string createRewrittenFileName(const string& name) {
+string createRewrittenFileName(const string &name) {
   // Search source tree and installed location
   if (!exists(fs::path(name))) {
     throw invalid_argument(
-        "gtsam::createRewrittenFileName could not find a matching file in\n"
+        "gtsam::createRewrittenFileName could not find a matching file in\n" +
-            + name);
+        name);
   }
 
   fs::path p(name);
-  fs::path newpath = fs::path(p.parent_path().string())
+  fs::path newpath = fs::path(p.parent_path().string()) /
-      / fs::path(p.stem().string() + "-rewritten.txt");
+                     fs::path(p.stem().string() + "-rewritten.txt");
 
   return newpath.string();
 }
@@ -113,48 +116,49 @@ GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, Key maxNr,
 }
 
 /* ************************************************************************* */
-// Read noise parameters and interpret them according to flags
+// Interpret noise parameters according to flags
-static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
+static SharedNoiseModel
-    NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) {
+createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
-  double v1, v2, v3, v4, v5, v6;
+                 KernelFunctionType kernelFunctionType) {
-  is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
-
   if (noiseFormat == NoiseFormatAUTO) {
     // Try to guess covariance matrix layout
-    if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
+    if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
-        && v6 == 0.0) {
+        v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
-      // NoiseFormatGRAPH
       noiseFormat = NoiseFormatGRAPH;
-    } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
+    } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
-        && v6 != 0.0) {
+               v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
-      // NoiseFormatCOV
       noiseFormat = NoiseFormatCOV;
     } else {
       throw std::invalid_argument(
-          "load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
+          "load2D: unrecognized covariance matrix format in dataset file. "
+          "Please specify the noise format.");
     }
   }
 
   // Read matrix and check that diagonal entries are non-zero
-  Matrix M(3, 3);
+  Matrix3 M;
   switch (noiseFormat) {
   case NoiseFormatG2O:
   case NoiseFormatCOV:
-    // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
+    // i.e., [v(0)  v(1)  v(2);
-    if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
+    //        v(1)' v(3)  v(4);
+    //        v(2)' v(4)' v(5) ]
+    if (v(0) == 0.0 || v(3) == 0.0 || v(5) == 0.0)
       throw runtime_error(
           "load2D::readNoiseModel looks like this is not G2O matrix order");
-    M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
+    M << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5);
     break;
   case NoiseFormatTORO:
   case NoiseFormatGRAPH:
     // http://www.openslam.org/toro.html
     // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
-    // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
+    // i.e., [v(0)  v(1)  v(4);
-    if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
+    //        v(1)' v(2)  v(5);
+    //        v(4)' v(5)' v(3) ]
+    if (v(0) == 0.0 || v(2) == 0.0 || v(3) == 0.0)
       throw invalid_argument(
           "load2D::readNoiseModel looks like this is not TORO matrix order");
-    M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
+    M << v(0), v(1), v(4), v(1), v(2), v(5), v(4), v(5), v(3);
     break;
   default:
     throw runtime_error("load2D: invalid noise format");
@@ -195,15 +199,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   }
 }
 
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 /* ************************************************************************* */
-boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
+// Read noise parameters and interpret them according to flags
-  return parseVertexPose(is, tag);
+static SharedNoiseModel readNoiseModel(istream &is, bool smart,
+                                       NoiseFormat noiseFormat,
+                                       KernelFunctionType kernelFunctionType) {
+  Vector6 v;
+  is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5);
+  return createNoiseModel(v, smart, noiseFormat, kernelFunctionType);
 }
-#endif
 
 /* ************************************************************************* */
-boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
+boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
   if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
     Key id;
     double x, y, yaw;
@@ -215,7 +222,8 @@ boost::optional<IndexedPose> parseVertexPose(istream& is, const string& tag) {
 }
 
 /* ************************************************************************* */
-boost::optional<IndexedLandmark> parseVertexLandmark(istream& is, const string& tag) {
+boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
+                                                     const string &tag) {
   if (tag == "VERTEX_XY") {
     Key id;
     double x, y;
@@ -275,9 +283,37 @@ map<Key, Point2> parse2DLandmarks(const string &filename, Key maxNr) {
 }
 
 /* ************************************************************************* */
-boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
+// Parse a file by first parsing the `Parsed` type and then processing it with
-  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
+// the supplied `process` function. Result is put in a vector evaluates to true.
-      || (tag == "ODOMETRY")) {
+// Requires: a stream >> operator for boost::optional<Parsed>
+template <typename Parsed, typename Output>
+static vector<Output>
+parseToVector(const string &filename,
+              const std::function<Output(const Parsed &)> process) {
+  ifstream is(filename.c_str());
+  if (!is)
+    throw invalid_argument("parse: can not find file " + filename);
+
+  vector<Output> result;
+  string tag;
+  while (!is.eof()) {
+    boost::optional<Parsed> parsed;
+    is >> parsed;
+    if (parsed) {
+      Output factor = process(*parsed); // can return nullptr
+      if (factor) {
+        result.push_back(factor);
+      }
+    }
+    is.ignore(LINESIZE, '\n');
+  }
+  return result;
+}
+
+/* ************************************************************************* */
+boost::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
+  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
+      (tag == "ODOMETRY")) {
 
     Key id1, id2;
     double x, y, yaw;
@@ -288,6 +324,86 @@ boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
   }
 }
 
+/* ************************************************************************* */
+struct Measurement2 {
+  Key id1, id2;
+  Pose2 pose;
+  Vector6 v; // 6 noise model parameters for edge
+};
+
+istream &operator>>(istream &is, boost::optional<Measurement2> &edge) {
+  string tag;
+  is >> tag;
+  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
+      (tag == "ODOMETRY")) {
+    Key id1, id2;
+    double x, y, yaw;
+    Vector6 v;
+    is >> id1 >> id2 >> x >> y >> yaw >> //
+        v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5);
+    edge.reset({id1, id2, Pose2(x, y, yaw), v});
+  }
+  return is;
+}
+
+/* ************************************************************************* */
+// Create a sampler
+boost::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
+  auto noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+  if (!noise)
+    throw invalid_argument("gtsam::load: invalid noise model for adding noise"
+                           "(current version assumes diagonal noise model)!");
+  return boost::shared_ptr<Sampler>(new Sampler(noise));
+}
+
+/* ************************************************************************* */
+// Small functor to process the edge into a BetweenFactor<Pose2>::shared_ptr
+struct ProcessPose2 {
+  // The arguments
+  Key maxNr = 0;
+  SharedNoiseModel model;
+  boost::shared_ptr<Sampler> sampler;
+
+  // Arguments for parsing noise model
+  bool smart = true;
+  NoiseFormat noiseFormat = NoiseFormatAUTO;
+  KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE;
+
+  // The actual function
+  BetweenFactor<Pose2>::shared_ptr operator()(const Measurement2 &edge) {
+    // optional filter
+    if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr))
+      return nullptr;
+
+    // Get pose and optionally add noise
+    Pose2 T12 = edge.pose;
+    if (sampler)
+      T12 = T12.retract(sampler->sample());
+
+    // Create factor
+    // If model is nullptr we use the model from the file
+    return boost::make_shared<BetweenFactor<Pose2>>(
+        edge.id1, edge.id2, T12,
+        model
+            ? model
+            : createNoiseModel(edge.v, smart, noiseFormat, kernelFunctionType));
+  }
+};
+
+/* ************************************************************************* */
+BetweenFactorPose2s
+parse2DFactors(const string &filename,
+               const noiseModel::Diagonal::shared_ptr &corruptingNoise,
+               Key maxNr) {
+  ProcessPose2 process;
+  process.maxNr = maxNr;
+  if (corruptingNoise) {
+    process.sampler = createSampler(corruptingNoise);
+  }
+  return parseToVector<Measurement2, BetweenFactor<Pose2>::shared_ptr>(filename,
+                                                                       process);
+}
+
 /* ************************************************************************* */
 GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
                       bool addNoise, bool smart, NoiseFormat noiseFormat,
@@ -296,11 +412,11 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
   Values::shared_ptr initial(new Values);
 
   const auto poses = parse2DPoses(filename, maxNr);
-  for (const auto& key_pose : poses) {
+  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) {
+  for (const auto &key_landmark : landmarks) {
     initial->insert(key_landmark.first, key_landmark.second);
   }
 
@@ -311,7 +427,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
   // If asked, create a sampler with random number generator
-  std::unique_ptr<Sampler> sampler;
+  boost::shared_ptr<Sampler> sampler;
   if (addNoise) {
     noiseModel::Diagonal::shared_ptr noise;
     if (model)
@@ -335,11 +451,11 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
     auto between_pose = parseEdge(is, tag);
     if (between_pose) {
       std::tie(id1, id2) = between_pose->first;
-      Pose2& l1Xl2 = between_pose->second;
+      Pose2 l1Xl2 = between_pose->second;
 
       // read noise model
-      SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
+      SharedNoiseModel modelInFile =
-          kernelFunctionType);
+          readNoiseModel(is, smart, noiseFormat, kernelFunctionType);
 
       // optional filter
       if (maxNr && (id1 >= maxNr || id2 >= maxNr))
@@ -369,7 +485,8 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
       is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
     }
 
-    // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range
+    // A landmark measurement, TODO Frank says: don't know why is converted to
+    // bearing-range
     if (tag == "LANDMARK") {
       double lmx, lmy;
       double v1, v2, v3;
@@ -380,8 +497,9 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
       bearing = atan2(lmy, lmx);
       range = sqrt(lmx * lmx + lmy * lmy);
 
-      // In our experience, the x-y covariance on landmark sightings is not very good, so assume
+      // In our experience, the x-y covariance on landmark sightings is not
-      // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
+      // very good, so assume it describes the uncertainty at a range of 10m,
+      // and convert that to bearing/range uncertainty.
       if (std::abs(v1 - v3) < 1e-4) {
         bearing_std = sqrt(v1 / 10.0);
         range_std = sqrt(v1);
@@ -389,9 +507,10 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
         bearing_std = 1;
         range_std = 1;
         if (!haveLandmark) {
-          cout
+          cout << "Warning: load2D is a very simple dataset loader and is "
-              << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
+                  "ignoring the\n"
-                  "non-uniform covariance on LANDMARK measurements in this file."
+                  "non-uniform covariance on LANDMARK measurements in this "
+                  "file."
                << endl;
           haveLandmark = true;
         }
@@ -407,7 +526,8 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
 
       // Create noise model
       noiseModel::Diagonal::shared_ptr measurementNoise =
-          noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished());
+          noiseModel::Diagonal::Sigmas(
+              (Vector(2) << bearing_std, range_std).finished());
 
       // Add to graph
       *graph += BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range,
@@ -430,46 +550,46 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
 }
 
 /* ************************************************************************* */
-GraphAndValues load2D_robust(const string& filename,
+GraphAndValues load2D_robust(const string &filename,
-    noiseModel::Base::shared_ptr& model, Key maxNr) {
+                             noiseModel::Base::shared_ptr &model, Key maxNr) {
   return load2D(filename, model, maxNr);
 }
 
 /* ************************************************************************* */
-void save2D(const NonlinearFactorGraph& graph, const Values& config,
+void save2D(const NonlinearFactorGraph &graph, const Values &config,
-    const noiseModel::Diagonal::shared_ptr model, const string& filename) {
+            const noiseModel::Diagonal::shared_ptr model,
+            const string &filename) {
 
   fstream stream(filename.c_str(), fstream::out);
 
   // save poses
-
+  for (const Values::ConstKeyValuePair &key_value : config) {
-  for(const Values::ConstKeyValuePair& key_value: config) {
+    const Pose2 &pose = key_value.value.cast<Pose2>();
-    const Pose2& pose = key_value.value.cast<Pose2>();
     stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
            << " " << pose.theta() << endl;
   }
 
   // save edges
-  Matrix R = model->R();
+  // TODO(frank): why don't we use model in factor?
-  Matrix RR = trans(R) * R; //prod(trans(R),R);
+  Matrix3 R = model->R();
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
+  Matrix3 RR = R.transpose() * R;
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
+  for (auto f : graph) {
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
+    auto factor = boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(f);
     if (!factor)
       continue;
 
-    Pose2 pose = factor->measured().inverse();
+    const Pose2 pose = factor->measured().inverse();
     stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
-        << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
+           << pose.x() << " " << pose.y() << " " << pose.theta() << " "
-        << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
+           << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
-        << RR(0, 2) << " " << RR(1, 2) << endl;
+           << " " << RR(0, 2) << " " << RR(1, 2) << endl;
   }
 
   stream.close();
 }
 
 /* ************************************************************************* */
-GraphAndValues readG2o(const string& g2oFile, const bool is3D,
+GraphAndValues readG2o(const string &g2oFile, const bool is3D,
                        KernelFunctionType kernelFunctionType) {
   if (is3D) {
     return load3D(g2oFile);
@@ -484,24 +604,26 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D,
 }
 
 /* ************************************************************************* */
-void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
+void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
-    const string& filename) {
+              const string &filename) {
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D poses
-  for (const auto& key_value : estimate) {
+  for (const auto &key_value : estimate) {
-    auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
+    auto p = dynamic_cast<const GenericValue<Pose2> *>(&key_value.value);
-    if (!p) continue;
+    if (!p)
-    const Pose2& pose = p->value();
+      continue;
+    const Pose2 &pose = p->value();
     stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
            << pose.y() << " " << pose.theta() << endl;
   }
 
   // save 3D poses
-  for(const auto& key_value: estimate) {
+  for (const auto &key_value : estimate) {
-    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    auto p = dynamic_cast<const GenericValue<Pose3> *>(&key_value.value);
-    if (!p) continue;
+    if (!p)
-    const Pose3& pose = p->value();
+      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() << " "
@@ -510,60 +632,62 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   }
 
   // save 2D landmarks
-  for(const auto& key_value: estimate) {
+  for (const auto &key_value : estimate) {
-    auto p = dynamic_cast<const GenericValue<Point2>*>(&key_value.value);
+    auto p = dynamic_cast<const GenericValue<Point2> *>(&key_value.value);
-    if (!p) continue;
+    if (!p)
-    const Point2& point = p->value();
+      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) {
+  for (const auto &key_value : estimate) {
-    auto p = dynamic_cast<const GenericValue<Point3>*>(&key_value.value);
+    auto p = dynamic_cast<const GenericValue<Point3> *>(&key_value.value);
-    if (!p) continue;
+    if (!p)
-    const Point3& point = p->value();
+      continue;
+    const Point3 &point = p->value();
     stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " "
            << point.y() << " " << point.z() << endl;
   }
 
   // save edges (2D or 3D)
-  for(const auto& factor_: graph) {
+  for (const auto &factor_ : graph) {
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
+    boost::shared_ptr<BetweenFactor<Pose2>> factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
-    if (factor){
+    if (factor) {
       SharedNoiseModel model = factor->noiseModel();
       boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
           boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
-      if (!gaussianModel){
+      if (!gaussianModel) {
         model->print("model\n");
         throw invalid_argument("writeG2o: invalid noise model!");
       }
-      Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
+      Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
       Pose2 pose = factor->measured(); //.inverse();
       stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
              << pose.x() << " " << pose.y() << " " << pose.theta();
-      for (size_t i = 0; i < 3; i++){
+      for (size_t i = 0; i < 3; i++) {
-        for (size_t j = i; j < 3; j++){
+        for (size_t j = i; j < 3; j++) {
           stream << " " << Info(i, j);
         }
       }
       stream << endl;
     }
 
-    boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
+    boost::shared_ptr<BetweenFactor<Pose3>> factor3D =
-        boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
+        boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
 
-    if (factor3D){
+    if (factor3D) {
       SharedNoiseModel model = factor3D->noiseModel();
 
       boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
           boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
-      if (!gaussianModel){
+      if (!gaussianModel) {
         model->print("model\n");
         throw invalid_argument("writeG2o: invalid noise model!");
       }
-      Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
+      Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R();
       const Pose3 pose3D = factor3D->measured();
       const Point3 p = pose3D.translation();
       const auto q = pose3D.rotation().toQuaternion();
@@ -571,14 +695,14 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
              << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x()
              << " " << q.y() << " " << q.z() << " " << q.w();
 
-      Matrix InfoG2o = I_6x6;
+      Matrix6 InfoG2o = I_6x6;
       InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation
       InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation
       InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(0, 3); // off diagonal
       InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(3, 0); // off diagonal
 
-      for (size_t i = 0; i < 6; i++){
+      for (size_t i = 0; i < 6; i++) {
-        for (size_t j = i; j < 6; j++){
+        for (size_t j = i; j < 6; j++) {
           stream << " " << InfoG2o(i, j);
         }
       }
@@ -648,69 +772,43 @@ map<Key, Point3> parse3DLandmarks(const string &filename, Key maxNr) {
   return parseIntoMap<Point3>(filename, maxNr);
 }
 
-/* ************************************************************************* */
-// Parse BetweenFactor<T> shared pointers into a vector
-template <typename T>
-static vector<boost::shared_ptr<BetweenFactor<T>>>
-parseIntoVector(const string &filename, Key maxNr = 0) {
-  ifstream is(filename.c_str());
-  if (!is)
-    throw invalid_argument("parse: can not find file " + filename);
-
-  vector<boost::shared_ptr<BetweenFactor<T>>> result;
-  string tag;
-  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;
-}
-
 /* ************************************************************************* */
 // Parse a symmetric covariance matrix (onlyupper-triangular part is stored)
 istream &operator>>(istream &is, Matrix6 &m) {
   for (size_t i = 0; i < 6; i++)
-    for (size_t j = i; j < 6; j++){
+    for (size_t j = i; j < 6; j++) {
       is >> m(i, j);
-      m(j,i) = m(i,j);
+      m(j, i) = m(i, j);
     }
   return is;
 }
 
 /* ************************************************************************* */
-istream &operator>>(istream &is,
+struct Measurement3 {
-                    boost::shared_ptr<BetweenFactor<Pose3>> &shared) {
+  Key id1, id2;
+  Pose3 pose;
+  SharedNoiseModel model;
+};
+
+istream &operator>>(istream &is, boost::optional<Measurement3> &edge) {
   string tag;
   is >> tag;
-
   Matrix6 m;
   if (tag == "EDGE3") {
     Key id1, id2;
     double x, y, z;
     Rot3 R;
-    is >> id1 >> id2 >> x >> y >> z >> R;
+    is >> id1 >> id2 >> x >> y >> z >> R >> m;
-    Pose3 pose(R, {x, y, z});
+    edge.reset(
-
+        {id1, id2, Pose3(R, {x, y, z}), noiseModel::Gaussian::Information(m)});
-    is >> m;
-    SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-    shared.reset(new BetweenFactor<Pose3>(id1, id2, pose, model));
   }
   if (tag == "EDGE_SE3:QUAT") {
     Key id1, id2;
     double x, y, z;
     Quaternion q;
-    is >> id1 >> id2 >> x >> y >> z >> q;
+    is >> id1 >> id2 >> x >> y >> z >> q >> m;
-    Pose3 pose(q, {x, y, z});
 
     // 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
@@ -718,47 +816,67 @@ istream &operator>>(istream &is,
     mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
     SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
 
-    shared.reset(new BetweenFactor<Pose3>(id1, id2, pose, model));
+    edge.reset({id1, id2, Pose3(q, {x, y, z}),
+                noiseModel::Gaussian::Information(mgtsam)});
   }
   return is;
 }
 
+/* ************************************************************************* */
+// Small functor to process the edge into a BetweenFactor<Pose3>::shared_ptr
+struct ProcessPose3 {
+  // The arguments
+  Key maxNr = 0;
+  SharedNoiseModel model;
+  boost::shared_ptr<Sampler> sampler;
+
+  // The actual function
+  BetweenFactor<Pose3>::shared_ptr operator()(const Measurement3 &edge) {
+    // optional filter
+    if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr))
+      return nullptr;
+
+    // Get pose and optionally add noise
+    Pose3 T12 = edge.pose;
+    if (sampler)
+      T12 = T12.retract(sampler->sample());
+
+    // Create factor
+    return boost::make_shared<BetweenFactor<Pose3>>(edge.id1, edge.id2, T12,
+                                                    edge.model);
+  }
+};
+
 /* ************************************************************************* */
 BetweenFactorPose3s
 parse3DFactors(const string &filename,
                const noiseModel::Diagonal::shared_ptr &corruptingNoise,
                Key maxNr) {
-  auto factors =  parseIntoVector<Pose3>(filename, maxNr);
+  ProcessPose3 process;
-
+  process.maxNr = maxNr;
   if (corruptingNoise) {
-    Sampler sampler(corruptingNoise);
+    process.sampler = createSampler(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 parseToVector<Measurement3, BetweenFactor<Pose3>::shared_ptr>(filename,
-
+                                                                       process);
-  return factors;
 }
 
 /* ************************************************************************* */
-GraphAndValues load3D(const string& filename) {
+GraphAndValues load3D(const string &filename) {
   const auto factors = parse3DFactors(filename);
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
-  for (const auto& factor : factors) {
+  for (const auto &factor : factors) {
     graph->push_back(factor);
   }
 
   Values::shared_ptr initial(new Values);
 
   const auto poses = parse3DPoses(filename);
-  for (const auto& key_pose : poses) {
+  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) {
+  for (const auto &key_landmark : landmarks) {
     initial->insert(key_landmark.first, key_landmark.second);
   }
 
@@ -766,7 +884,8 @@ GraphAndValues load3D(const string& filename) {
 }
 
 /* ************************************************************************* */
-Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
+Rot3 openGLFixedRotation() { // this is due to different convention for
+                             // cameras in gtsam and openGL
   /* R = [ 1   0   0
    *       0  -1   0
    *       0   0  -1]
@@ -779,7 +898,7 @@ Rot3 openGLFixedRotation() { // this is due to different convention for cameras
 }
 
 /* ************************************************************************* */
-Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
+Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
   Rot3 R90 = openGLFixedRotation();
   Rot3 wRc = (R.inverse()).compose(R90);
 
@@ -788,7 +907,7 @@ Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
 }
 
 /* ************************************************************************* */
-Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
+Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
   Rot3 R90 = openGLFixedRotation();
   Rot3 cRw_openGL = R90.compose(R.inverse());
   Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
@@ -796,13 +915,13 @@ Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
 }
 
 /* ************************************************************************* */
-Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
+Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
   return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
                       PoseGTSAM.z());
 }
 
 /* ************************************************************************* */
-bool readBundler(const string& filename, SfmData &data) {
+bool readBundler(const string &filename, SfmData &data) {
   // Load the data file
   ifstream is(filename.c_str(), ifstream::in);
   if (!is) {
@@ -889,7 +1008,7 @@ bool readBundler(const string& filename, SfmData &data) {
 }
 
 /* ************************************************************************* */
-bool readBAL(const string& filename, SfmData &data) {
+bool readBAL(const string &filename, SfmData &data) {
   // Load the data file
   ifstream is(filename.c_str(), ifstream::in);
   if (!is) {
@@ -937,7 +1056,7 @@ bool readBAL(const string& filename, SfmData &data) {
     // Get the 3D position
     float x, y, z;
     is >> x >> y >> z;
-    SfmTrack& track = data.tracks[j];
+    SfmTrack &track = data.tracks[j];
     track.p = Point3(x, y, z);
     track.r = 0.4f;
     track.g = 0.4f;
@@ -949,7 +1068,7 @@ bool readBAL(const string& filename, SfmData &data) {
 }
 
 /* ************************************************************************* */
-bool writeBAL(const string& filename, SfmData &data) {
+bool writeBAL(const string &filename, SfmData &data) {
   // Open the output file
   ofstream os;
   os.open(filename.c_str());
@@ -971,25 +1090,28 @@ bool writeBAL(const string& filename, SfmData &data) {
   os << endl;
 
   for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    const SfmTrack& track = data.tracks[j];
+    const SfmTrack &track = data.tracks[j];
 
-    for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
+    for (size_t k = 0; k < track.number_measurements();
+         k++) { // for each observation of the 3D point j
       size_t i = track.measurements[k].first; // camera id
       double u0 = data.cameras[i].calibration().u0();
       double v0 = data.cameras[i].calibration().v0();
 
       if (u0 != 0 || v0 != 0) {
-        cout
+        cout << "writeBAL has not been tested for calibration with nonzero "
-            << "writeBAL has not been tested for calibration with nonzero (u0,v0)"
+                "(u0,v0)"
              << endl;
       }
 
-      double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
+      double pixelBALx = track.measurements[k].second.x() -
-      double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
+                         u0; // center of image is the origin
+      double pixelBALy = -(track.measurements[k].second.y() -
+                           v0); // center of image is the origin
       Point2 pixelMeasurement(pixelBALx, pixelBALy);
-      os << i /*camera id*/<< " " << j /*point id*/<< " "
+      os << i /*camera id*/ << " " << j /*point id*/ << " "
-          << pixelMeasurement.x() /*u of the pixel*/<< " "
+         << pixelMeasurement.x() /*u of the pixel*/ << " "
-          << pixelMeasurement.y() /*v of the pixel*/<< endl;
+         << pixelMeasurement.y() /*v of the pixel*/ << endl;
     }
   }
   os << endl;
@@ -1022,15 +1144,17 @@ bool writeBAL(const string& filename, SfmData &data) {
   return true;
 }
 
-bool writeBALfromValues(const string& filename, const SfmData &data,
+bool writeBALfromValues(const string &filename, const SfmData &data,
-    Values& values) {
+                        Values &values) {
   using Camera = PinholeCamera<Cal3Bundler>;
   SfmData dataValues = data;
 
   // Store poses or cameras in SfmData
   size_t nrPoses = values.count<Pose3>();
-  if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
+  if (nrPoses ==
-    for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
+      dataValues.number_cameras()) { // we only estimated camera poses
+    for (size_t i = 0; i < dataValues.number_cameras();
+         i++) { // for each camera
       Key poseKey = symbol('x', i);
       Pose3 pose = values.at<Pose3>(poseKey);
       Cal3Bundler K = dataValues.cameras[i].calibration();
@@ -1039,29 +1163,29 @@ bool writeBALfromValues(const string& filename, const SfmData &data,
     }
   } else {
     size_t nrCameras = values.count<Camera>();
-    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
+    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera
+                                                    // poses and calibration
       for (size_t i = 0; i < nrCameras; i++) {      // for each camera
         Key cameraKey = i;                          // symbol('c',i);
         Camera camera = values.at<Camera>(cameraKey);
         dataValues.cameras[i] = camera;
       }
     } else {
-      cout
+      cout << "writeBALfromValues: different number of cameras in "
-          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras "
+              "SfM_dataValues (#cameras "
            << dataValues.number_cameras() << ") and values (#cameras "
-          << nrPoses << ", #poses " << nrCameras << ")!!"
+           << nrPoses << ", #poses " << nrCameras << ")!!" << endl;
-          << endl;
       return false;
     }
   }
 
   // Store 3D points in SfmData
-  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
+  size_t nrPoints = values.count<Point3>(),
+         nrTracks = dataValues.number_tracks();
   if (nrPoints != nrTracks) {
-    cout
+    cout << "writeBALfromValues: different number of points in "
-        << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
+            "SfM_dataValues (#points= "
-        << nrTracks << ") and values (#points "
+         << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
-        << nrPoints << ")!!" << endl;
   }
 
   for (size_t j = 0; j < nrTracks; j++) { // for each point
@@ -1073,7 +1197,7 @@ bool writeBALfromValues(const string& filename, const SfmData &data,
       dataValues.tracks[j].r = 1.0;
       dataValues.tracks[j].g = 0.0;
       dataValues.tracks[j].b = 0.0;
-      dataValues.tracks[j].p = Point3(0,0,0);
+      dataValues.tracks[j].p = Point3(0, 0, 0);
     }
   }
 
@@ -1081,22 +1205,22 @@ bool writeBALfromValues(const string& filename, const SfmData &data,
   return writeBAL(filename, dataValues);
 }
 
-Values initialCamerasEstimate(const SfmData& db) {
+Values initialCamerasEstimate(const SfmData &db) {
   Values initial;
   size_t i = 0; // NO POINTS:  j = 0;
-  for(const SfmCamera& camera: db.cameras)
+  for (const SfmCamera &camera : db.cameras)
     initial.insert(i++, camera);
   return initial;
 }
 
-Values initialCamerasAndPointsEstimate(const SfmData& db) {
+Values initialCamerasAndPointsEstimate(const SfmData &db) {
   Values initial;
   size_t i = 0, j = 0;
-  for(const SfmCamera& camera: db.cameras)
+  for (const SfmCamera &camera : db.cameras)
     initial.insert((i++), camera);
-  for(const SfmTrack& track: db.tracks)
+  for (const SfmTrack &track : db.tracks)
     initial.insert(P(j++), track.p);
   return initial;
 }
 
-} // \namespace gtsam
+} // namespace gtsam

gtsam/slam/dataset.h

@@ -77,16 +77,6 @@ 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
- * @param tag string parsed from input stream, will only parse if vertex type
- */
-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
@@ -118,7 +108,8 @@ using BetweenFactorPose2s =
 /// 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);
+    const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr,
+    Key maxNr = 0);
 
 /// Parse vertices in 2D g2o graph file into a map of Pose2s.
 GTSAM_EXPORT std::map<Key, Pose2> parse2DPoses(const std::string &filename,
@@ -343,4 +334,15 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
  */
 GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+/**
+ * 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> parseVertex(std::istream &is,
+                                                      const std::string &tag) {
+  return parseVertexPose(is, tag);
+}
+#endif
 }  // namespace gtsam

gtsam/slam/tests/testDataset.cpp

@@ -104,13 +104,13 @@ TEST(dataSet, load2D) {
       boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
   EXPECT(assert_equal(expected, *actual));
 
-  // // Check factor parsing
+  // Check factor parsing
-  // const auto actualFactors = parse2DFactors(filename);
+  const auto actualFactors = parse2DFactors(filename);
-  // for (size_t i : {0, 1, 2, 3, 4, 5}) {
+  for (size_t i : {0, 1, 2, 3, 4, 5}) {
-  //   EXPECT(assert_equal(
+    EXPECT(assert_equal(
-  //       *boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
+        *boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
-  //       *actualFactors[i], 1e-5));
+        *actualFactors[i], 1e-5));
-  // }
+  }
 
   // Check pose parsing
   const auto actualPoses = parse2DPoses(filename);