Sanitized G2o I/O interface to conform to what we had before. No sense in having many different styles, and this works better for MATLAB (now wrapped, as well).

BAL reading/writing should be similarly cleaned up.

examples/Pose2SLAMExample_g2o.cpp

@@ -35,18 +35,18 @@ int main(const int argc, const char *argv[]) {
   else
     g2oFile = argv[1];
 
-  NonlinearFactorGraph graph;
+  NonlinearFactorGraph::shared_ptr graph;
-  Values initial;
+  Values::shared_ptr initial;
-  readG2o(g2oFile, graph, initial);
+  boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = graph;
+  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
@@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(outputFile, graph, result);
+    writeG2o(*graph, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/Pose2SLAMExample_lago.cpp

@@ -36,12 +36,12 @@ int main(const int argc, const char *argv[]) {
   else
     g2oFile = argv[1];
 
-  NonlinearFactorGraph graph;
+  NonlinearFactorGraph::shared_ptr graph;
-  Values initial;
+  Values::shared_ptr initial;
-  readG2o(g2oFile, graph, initial);
+  boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = graph;
+  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
@@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(outputFile, graph, estimateLago);
+    writeG2o(*graph, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
   }
 

gtsam.h

@@ -2249,6 +2249,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
     gtsam::noiseModel::Base* model);
+void save2D(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
+    string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& estimate, string filename);
 
 //*************************************************************************
 // Navigation

gtsam/nonlinear/tests/testLago.cpp

@@ -258,13 +258,13 @@ TEST( Lago, smallGraph2 ) {
 /* *************************************************************************** */
 TEST( Lago, largeGraphNoisy_orientations ) {
 
-  NonlinearFactorGraph g;
-  Values initial;
   string inputFile = findExampleDataFile("noisyToyGraph");
-  readG2o(inputFile, g, initial);
+  NonlinearFactorGraph::shared_ptr g;
+  Values::shared_ptr initial;
+  boost::tie(g, initial) = readG2o(inputFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = g;
+  NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
@@ -279,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) {
       actual.insert(key, poseLago);
     }
   }
-  NonlinearFactorGraph gmatlab;
-  Values expected;
   string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
-  readG2o(matlabFile, gmatlab, expected);
+  NonlinearFactorGraph::shared_ptr gmatlab;
+  Values::shared_ptr expected;
+  boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
+  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
     Key k = key_val.key;
-    EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
+    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
   }
 }
 
 /* *************************************************************************** */
 TEST( Lago, largeGraphNoisy ) {
 
-  NonlinearFactorGraph g;
-  Values initial;
   string inputFile = findExampleDataFile("noisyToyGraph");
-  readG2o(inputFile, g, initial);
+  NonlinearFactorGraph::shared_ptr g;
+  Values::shared_ptr initial;
+  boost::tie(g, initial) = readG2o(inputFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = g;
+  NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
   Values actual = lago::initialize(graphWithPrior);
 
-  NonlinearFactorGraph gmatlab;
-  Values expected;
   string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
-  readG2o(matlabFile, gmatlab, expected);
+  NonlinearFactorGraph::shared_ptr gmatlab;
+  Values::shared_ptr expected;
+  boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
+  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
     Key k = key_val.key;
-    EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
+    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
   }
 }
 

gtsam/slam/dataset.cpp

@@ -62,7 +62,7 @@ string findExampleDataFile(const string& name) {
 
   // If we did not return already, then we did not find the file
   throw
-std::invalid_argument(
+invalid_argument(
     "gtsam::findExampleDataFile could not find a matching file in\n"
     SOURCE_TREE_DATASET_DIR " or\n"
     INSTALLED_DATASET_DIR " named\n" +
@@ -73,7 +73,7 @@ std::invalid_argument(
 string createRewrittenFileName(const string& name) {
   // Search source tree and installed location
   if (!exists(fs::path(name))) {
-    throw std::invalid_argument(
+    throw invalid_argument(
         "gtsam::createRewrittenFileName could not find a matching file in\n"
             + name);
   }
@@ -89,9 +89,8 @@ string createRewrittenFileName(const string& name) {
 #endif
 
 /* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
-    pair<string, SharedNoiseModel> dataset, int maxID, bool addNoise,
+    bool addNoise, bool smart, NoiseFormat noiseFormat,
-    bool smart, NoiseFormat noiseFormat,
     KernelFunctionType kernelFunctionType) {
   return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
       noiseFormat, kernelFunctionType);
@@ -111,7 +110,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   case NoiseFormatCOV:
     // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
     if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
-      throw std::runtime_error(
+      throw runtime_error(
           "load2D::readNoiseModel looks like this is not G2O matrix order");
     M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
     break;
@@ -121,12 +120,12 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
     // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
     // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
     if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
-      throw std::invalid_argument(
+      throw invalid_argument(
           "load2D::readNoiseModel looks like this is not TORO matrix order");
     M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
     break;
   default:
-    throw std::runtime_error("load2D: invalid noise format");
+    throw runtime_error("load2D: invalid noise format");
   }
 
   // Now, create a Gaussian noise model
@@ -144,11 +143,11 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
     model = noiseModel::Gaussian::Covariance(M, smart);
     break;
   default:
-    throw std::invalid_argument("load2D: invalid noise format");
+    throw invalid_argument("load2D: invalid noise format");
   }
 
   switch (kernelFunctionType) {
-  case KernelFunctionTypeQUADRATIC:
+  case KernelFunctionTypeNONE:
     return model;
     break;
   case KernelFunctionTypeHUBER:
@@ -160,20 +159,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
         noiseModel::mEstimator::Tukey::Create(4.6851), model);
     break;
   default:
-    throw std::invalid_argument("load2D: invalid kernel function type");
+    throw invalid_argument("load2D: invalid kernel function type");
   }
 }
 
 /* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
-    const string& filename, SharedNoiseModel model, int maxID, bool addNoise,
+    bool addNoise, bool smart, NoiseFormat noiseFormat,
-    bool smart, NoiseFormat noiseFormat,
     KernelFunctionType kernelFunctionType) {
 
-  cout << "Will try to read " << filename << endl;
   ifstream is(filename.c_str());
   if (!is)
-    throw std::invalid_argument("load2D: can not find file " + filename);
+    throw invalid_argument("load2D: can not find file " + filename);
 
   Values::shared_ptr initial(new Values);
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
@@ -270,8 +267,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
       is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
 
       // Convert x,y to bearing,range
-      bearing = std::atan2(lmy, lmx);
+      bearing = atan2(lmy, lmx);
-      range = std::sqrt(lmx * lmx + lmy * lmy);
+      range = sqrt(lmx * lmx + lmy * lmy);
 
       // In our experience, the x-y covariance on landmark sightings is not very good, so assume
       // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
@@ -319,12 +316,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
     is.ignore(LINESIZE, '\n');
   }
 
-  cout << "load2D read a graph file with " << initial->size()
-      << " vertices and " << graph->nrFactors() << " factors" << endl;
-
   return make_pair(graph, initial);
 }
 
+/* ************************************************************************* */
+GraphAndValues load2D_robust(const string& filename,
+    noiseModel::Base::shared_ptr& model, int maxID) {
+  return load2D(filename, model, maxID);
+}
+
 /* ************************************************************************* */
 void save2D(const NonlinearFactorGraph& graph, const Values& config,
     const noiseModel::Diagonal::shared_ptr model, const string& filename) {
@@ -357,6 +357,54 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
   stream.close();
 }
 
+/* ************************************************************************* */
+GraphAndValues readG2o(const string& g2oFile,
+    KernelFunctionType kernelFunctionType) {
+  // just call load2D
+  int maxID = 0;
+  bool addNoise = false;
+  bool smart = true;
+  return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+      NoiseFormatG2O, kernelFunctionType);
+}
+
+/* ************************************************************************* */
+void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
+    const string& filename) {
+
+  fstream stream(filename.c_str(), fstream::out);
+
+  // save poses
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
+    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
+  }
+
+  // save edges
+  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
+    boost::shared_ptr<BetweenFactor<Pose2> > factor =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
+    if (!factor)
+      continue;
+
+    SharedNoiseModel model = factor->get_noiseModel();
+    boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
+        boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+    if (!diagonalModel)
+      throw invalid_argument(
+          "writeG2o: invalid noise model (current version assumes diagonal noise model)!");
+
+    Pose2 pose = factor->measured(); //.inverse();
+    stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
+        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
+        << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
+        << diagonalModel->precision(1) << " " << 0.0 << " "
+        << diagonalModel->precision(2) << endl;
+  }
+  stream.close();
+}
+
 /* ************************************************************************* */
 bool load3D(const string& filename) {
   ifstream is(filename.c_str());
@@ -399,105 +447,6 @@ bool load3D(const string& filename) {
   return true;
 }
 
-/* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
-    const string& filename, noiseModel::Base::shared_ptr& model, int maxID) {
-  cout << "Will try to read " << filename << endl;
-  ifstream is(filename.c_str());
-  if (!is)
-    throw std::invalid_argument("load2D: can not find the file!");
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
-
-  string tag;
-
-  // load the poses
-  while (is) {
-    is >> tag;
-
-    if ((tag == "VERTEX2") || (tag == "VERTEX")) {
-      int id;
-      double x, y, yaw;
-      is >> id >> x >> y >> yaw;
-      // optional filter
-      if (maxID && id >= maxID)
-        continue;
-      initial->insert(id, Pose2(x, y, yaw));
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
-
-  // Create a sampler with random number generator
-  Sampler sampler(42u);
-
-  // load the factors
-  while (is) {
-    is >> tag;
-
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
-      int id1, id2;
-      double x, y, yaw;
-
-      is >> id1 >> id2 >> x >> y >> yaw;
-      Matrix m = eye(3);
-      is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
-      m(2, 0) = m(0, 2);
-      m(2, 1) = m(1, 2);
-      m(1, 0) = m(0, 1);
-
-      // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
-        continue;
-
-      Pose2 l1Xl2(x, y, yaw);
-
-      // Insert vertices if pure odometry file
-      if (!initial->exists(id1))
-        initial->insert(id1, Pose2());
-      if (!initial->exists(id2))
-        initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
-
-      NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
-      graph->push_back(factor);
-    }
-    if (tag == "BR") {
-      int id1, id2;
-      double bearing, range, bearing_std, range_std;
-
-      is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
-
-      // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
-        continue;
-
-      noiseModel::Diagonal::shared_ptr measurementNoise =
-          noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
-      *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range,
-          measurementNoise);
-
-      // Insert poses or points if they do not exist yet
-      if (!initial->exists(id1))
-        initial->insert(id1, Pose2());
-      if (!initial->exists(id2)) {
-        Pose2 pose = initial->at<Pose2>(id1);
-        Point2 local(cos(bearing) * range, sin(bearing) * range);
-        Point2 global = pose.transform_from(local);
-        initial->insert(id2, global);
-      }
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-
-  cout << "load2D read a graph file with " << initial->size()
-      << " vertices and " << graph->nrFactors() << " factors" << endl;
-
-  return make_pair(graph, initial);
-}
-
 /* ************************************************************************* */
 Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
   /* R = [ 1   0   0
@@ -617,61 +566,6 @@ bool readBundler(const string& filename, SfM_data &data) {
   return true;
 }
 
-/* ************************************************************************* */
-bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
-    Values& initial, KernelFunctionType kernelFunctionType) {
-
-  // just call load2D
-  NonlinearFactorGraph::shared_ptr graph_ptr;
-  Values::shared_ptr initial_ptr;
-  int maxID = 0;
-  bool addNoise = false;
-  bool smart = true;
-  boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(),
-      maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType);
-  graph = *graph_ptr;
-  initial = *initial_ptr;
-  return true;
-}
-
-/* ************************************************************************* */
-bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph,
-    const Values& estimate) {
-
-  fstream stream(filename.c_str(), fstream::out);
-
-  // save poses
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
-    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
-    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
-        << pose.y() << " " << pose.theta() << endl;
-  }
-
-  // save edges
-  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
-    if (!factor)
-      continue;
-
-    SharedNoiseModel model = factor->get_noiseModel();
-    boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
-        boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
-    if (!diagonalModel)
-      throw std::invalid_argument(
-          "writeG2o: invalid noise model (current version assumes diagonal noise model)!");
-
-    Pose2 pose = factor->measured(); //.inverse();
-    stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
-        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
-        << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
-        << diagonalModel->precision(1) << " " << 0.0 << " "
-        << diagonalModel->precision(2) << endl;
-  }
-  stream.close();
-  return true;
-}
-
 /* ************************************************************************* */
 bool readBAL(const string& filename, SfM_data &data) {
   // Load the data file

gtsam/slam/dataset.h

@@ -54,16 +54,20 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
 
 /// Indicates how noise parameters are stored in file
 enum NoiseFormat {
-  NoiseFormatG2O,   ///< Information matrix I11, I12, I13, I22, I23, I33
+  NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
-  NoiseFormatTORO,  ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
+  NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
   NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
-  NoiseFormatCOV    ///< Covariance matrix C11, C12, C13, C22, C23, C33
+  NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
 };
 
+/// Robust kernel type to wrap around quadratic noise model
 enum KernelFunctionType {
-  KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
+  KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
 };
 
+/// Return type for load functions
+typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
+
 /**
  * Load TORO 2D Graph
  * @param dataset/model pair as constructed by [dataset]
@@ -71,36 +75,58 @@ enum KernelFunctionType {
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  */
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT GraphAndValues load2D(
     std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
     bool addNoise = false,
     bool smart = true, //
     NoiseFormat noiseFormat = NoiseFormatGRAPH,
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /**
- * Load TORO 2D Graph
+ * 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 addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
+ * @param noiseFormat how noise parameters are stored
+ * @param kernelFunctionType whether to wrap the noise model in a robust kernel
+ * @return graph and initial values
  */
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
-    const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
+    SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
-    int maxID = 0, bool addNoise = false, bool smart = true,
+        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
-    NoiseFormat noiseFormat = NoiseFormatGRAPH, //
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
 
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
+/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
-    const std::string& filename, noiseModel::Base::shared_ptr& model,
+GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
-    int maxID = 0);
+    noiseModel::Base::shared_ptr& model, int maxID = 0);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
     const Values& config, const noiseModel::Diagonal::shared_ptr model,
     const std::string& filename);
 
+/**
+ * @brief This function parses a g2o file and stores the measurements into a
+ * NonlinearFactorGraph and the initial guess in a Values structure
+ * @param filename The name of the g2o file
+ * @param kernelFunctionType whether to wrap the noise model in a robust kernel
+ * @return graph and initial values
+ */
+GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile,
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+
+/**
+ * @brief This function writes a g2o file from
+ * NonlinearFactorGraph and a Values structure
+ * @param filename The name of the g2o file to write
+ * @param graph NonlinearFactor graph storing the measurements
+ * @param estimate Values
+ */
+GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
+    const Values& estimate, const std::string& filename);
+
 /**
  * Load TORO 3D Graph
  */
@@ -143,27 +169,6 @@ struct SfM_data {
  */
 GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
 
-/**
- * @brief This function parses a g2o file and stores the measurements into a
- * NonlinearFactorGraph and the initial guess in a Values structure
- * @param filename The name of the g2o file
- * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
- * @return initial Values containing the initial guess (VERTEX_SE2)
- */
-GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
-    NonlinearFactorGraph& graph, Values& initial,
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
-
-/**
- * @brief This function writes a g2o file from
- * NonlinearFactorGraph and a Values structure
- * @param filename The name of the g2o file to write
- * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
- * @return estimate Values containing the values (VERTEX_SE2)
- */
-GTSAM_EXPORT bool writeG2o(const std::string& filename,
-    const NonlinearFactorGraph& graph, const Values& estimate);
-
 /**
  * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
  * SfM_data structure

gtsam/slam/tests/testDataset.cpp

@@ -81,9 +81,9 @@ TEST( dataSet, Balbianello)
 TEST( dataSet, readG2o)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
 
   Values expectedValues;
   expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
@@ -97,7 +97,7 @@ TEST( dataSet, readG2o)
   expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
   expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
   expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
-  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
 
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   NonlinearFactorGraph expectedGraph;
@@ -113,16 +113,16 @@ TEST( dataSet, readG2o)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, readG2oHuber)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER);
 
   noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@@ -140,16 +140,16 @@ TEST( dataSet, readG2oHuber)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, readG2oTukey)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY);
 
   noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
@@ -167,25 +167,25 @@ TEST( dataSet, readG2oTukey)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, writeG2o)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph expectedGraph;
+  NonlinearFactorGraph::shared_ptr expectedGraph;
-  Values expectedValues;
+  Values::shared_ptr expectedValues;
-  readG2o(g2oFile, expectedGraph, expectedValues);
+  boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
 
   const string filenameToWrite = createRewrittenFileName(g2oFile);
-  writeG2o(filenameToWrite, expectedGraph, expectedValues);
+  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
 
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(filenameToWrite, actualGraph, actualValues);
+  boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
-  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+  EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */