GTSAM 4.0.0 alpha2 Visual Studio fix

gtsam-4.0.0-alpha2-MSVC.patch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77434d135..46ce2e9a9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -53,9 +53,9 @@ endif()
 
 # Configurable Options
 if(GTSAM_UNSTABLE_AVAILABLE)
-    option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
+    option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          OFF)
 endif()
-option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead of shared" OFF)
+option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead of shared" ON)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
@@ -73,7 +73,7 @@ option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
+option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" OFF)
 
 # Check / set dependent variables for MATLAB wrapper
 if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP)
@@ -124,7 +124,7 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE
   message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
 endif()
 
-option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
+option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" ON)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
 set(GTSAM_BOOST_LIBRARIES
     ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
@@ -220,7 +220,7 @@ endif()
 ### use our patched version of Eigen
 ### See:  http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
 ###       http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
-option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ON)
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
@@ -526,4 +526,4 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
 endif()
 
 # Include CPack *after* all flags
-include(CPack)
+include(CPack)
\ No newline at end of file
diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake
index 15d4219e6..02691dc84 100644
--- a/cmake/GtsamTesting.cmake
+++ b/cmake/GtsamTesting.cmake
@@ -86,8 +86,8 @@ endmacro()
 # Build macros for using tests
 enable_testing()
 
-option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
-option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
+option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          OFF)
+option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       OFF)
 option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
 # Add option for combining unit tests
diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index 73fb320e1..1f5003261 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -107,7 +107,10 @@ namespace gtsam {
    * Template to create a binary predicate
    */
   template<class V>
-  struct equals : public std::binary_function<const V&, const V&, bool> {
+  struct equals {
+    using result_type = bool;
+    using first_argument_type = V;
+    using second_argument_type = V;
     double tol_;
     equals(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const V& expected, const V& actual) {
@@ -119,7 +122,10 @@ namespace gtsam {
    * Binary predicate on shared pointers
    */
   template<class V>
-  struct equals_star : public std::binary_function<const boost::shared_ptr<V>&, const boost::shared_ptr<V>&, bool> {
+  struct equals_star {
+    using result_type = bool;
+    using first_argument_type = const boost::shared_ptr<V>&;
+    using second_argument_type = const boost::shared_ptr<V>&;
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 4a918ef02..068c66a4a 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -88,6 +88,7 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
   ADT pFS(*this);
   Key j; size_t value;
   for(Key key: parents())
+  {
   try {
     j = (key);
     value = parentsValues.at(j);
@@ -98,6 +99,7 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
 //    pFS.print("pFS: ");
     throw runtime_error("DiscreteConditional::choose: parent value missing");
   };
+  }
   return pFS;
 }
 
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index d51f365be..956bddccb 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -29,8 +29,8 @@
 #include <gtsam/base/Vector.h>
 
 #include <boost/algorithm/string.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
+//#include <boost/archive/text_iarchive.hpp>
+//#include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/vector.hpp>
 #include <boost/range/adaptor/reversed.hpp>
 #include <boost/shared_ptr.hpp>
@@ -153,17 +153,17 @@ std::vector<size_t> Subgraph::edgeIndices() const {
 /****************************************************************************/
 void Subgraph::save(const std::string &fn) const {
   std::ofstream os(fn.c_str());
-  boost::archive::text_oarchive oa(os);
-  oa << *this;
+  //boost::archive::text_oarchive oa(os);
+  //oa << *this;
   os.close();
 }
 
 /****************************************************************************/
 Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
   std::ifstream is(fn.c_str());
-  boost::archive::text_iarchive ia(is);
+  //boost::archive::text_iarchive ia(is);
   Subgraph::shared_ptr subgraph(new Subgraph());
-  ia >> *subgraph;
+  //ia >> *subgraph;
   is.close();
   return subgraph;
 }
@@ -676,4 +676,4 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co
   return result;
 }
 
-} // nsamespace gtsam
+} // nsamespace gtsam
\ No newline at end of file
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 5f29c3bdf..4cffc1425 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -65,7 +65,7 @@ LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGr
 
 /* ************************************************************************* */
 void LevenbergMarquardtOptimizer::initTime() {
-  startTime_ = boost::posix_time::microsec_clock::universal_time();
+  //startTime_ = boost::posix_time::microsec_clock::universal_time();
 }
 
 /* ************************************************************************* */
@@ -105,14 +105,14 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
 inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
   auto currentState = static_cast<const State*>(state_.get());
 
-  if (!params_.logFile.empty()) {
-    ofstream os(params_.logFile.c_str(), ios::app);
-    boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
-    os << /*inner iterations*/ currentState->totalNumberInnerIterations << ","
-        << 1e-6 * (currentTime - startTime_).total_microseconds() << ","
-        << /*current error*/ currentError << "," << currentState->lambda << ","
-        << /*outer iterations*/ currentState->iterations << endl;
-  }
+  //if (!params_.logFile.empty()) {
+  //  ofstream os(params_.logFile.c_str(), ios::app);
+  //  boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
+  //  os << /*inner iterations*/ currentState->totalNumberInnerIterations << ","
+  //      << 1e-6 * (currentTime - startTime_).total_microseconds() << ","
+  //      << /*current error*/ currentError << "," << currentState->lambda << ","
+  //      << /*outer iterations*/ currentState->iterations << endl;
+  //}
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index f1135d2f0..183014941 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -23,7 +23,7 @@
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
 #include <gtsam/linear/VectorValues.h>
-#include <boost/date_time/posix_time/posix_time.hpp>
+//#include <boost/date_time/posix_time/posix_time.hpp>
 
 class NonlinearOptimizerMoreOptimizationTest;
 
@@ -36,7 +36,7 @@ class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer {
 
 protected:
   const LevenbergMarquardtParams params_; ///< LM parameters
-  boost::posix_time::ptime startTime_;
+  //boost::posix_time::ptime startTime_;
 
   void initTime();
 
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
deleted file mode 100644
index dc25026d2..000000000
--- a/gtsam/slam/dataset.cpp
+++ /dev/null
@@ -1,915 +0,0 @@
-/* ----------------------------------------------------------------------------
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file dataset.cpp
- * @date Jan 22, 2010
- * @author nikai, Luca Carlone
- * @brief utility functions for loading datasets
- */
-
-#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/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 <boost/assign/list_inserter.hpp>
-#include <boost/filesystem/operations.hpp>
-#include <boost/filesystem/path.hpp>
-
-#include <cmath>
-#include <fstream>
-#include <iostream>
-#include <stdexcept>
-
-using namespace std;
-namespace fs = boost::filesystem;
-using namespace gtsam::symbol_shorthand;
-
-#define LINESIZE 81920
-
-namespace gtsam {
-
-#ifndef MATLAB_MEX_FILE
-/* ************************************************************************* */
-string findExampleDataFile(const string& name) {
-  // Search source tree and installed location
-  vector<string> rootsToSearch;
-  rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
-  rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
-
-  // Search for filename as given, and with .graph and .txt extensions
-  vector<string> namesToSearch;
-  namesToSearch.push_back(name);
-  namesToSearch.push_back(name + ".graph");
-  namesToSearch.push_back(name + ".txt");
-  namesToSearch.push_back(name + ".out");
-
-  // Find first name that exists
-  for(const fs::path& root: rootsToSearch) {
-    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(
-    "gtsam::findExampleDataFile could not find a matching file in\n"
-    GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
-    GTSAM_INSTALLED_DATASET_DIR " named\n" +
-    name + ", " + name + ".graph, or " + name + ".txt");
-}
-
-/* ************************************************************************* */
-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"
-            + name);
-  }
-
-  fs::path p(name);
-  fs::path newpath = fs::path(p.parent_path().string())
-      / fs::path(p.stem().string() + "-rewritten.txt");
-
-  return newpath.string();
-}
-/* ************************************************************************* */
-#endif
-
-/* ************************************************************************* */
-GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
-    bool addNoise, bool smart, NoiseFormat noiseFormat,
-    KernelFunctionType kernelFunctionType) {
-  return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
-      noiseFormat, kernelFunctionType);
-}
-
-/* ************************************************************************* */
-// Read noise parameters and interpret them according to flags
-static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
-    NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) {
-  double v1, v2, v3, v4, v5, v6;
-  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 && v6 == 0.0)
-     {
-       // NoiseFormatGRAPH
-       noiseFormat = NoiseFormatGRAPH;
-     }
-     else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
-     {
-       // NoiseFormatCOV
-       noiseFormat = NoiseFormatCOV;
-     }
-     else
-     {
-       throw std::invalid_argument("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);
-  switch (noiseFormat) {
-  case NoiseFormatG2O:
-  case NoiseFormatCOV:
-    // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
-    if (v1 == 0.0 || v4 == 0.0 || v6 == 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;
-    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 ]
-    if (v1 == 0.0 || v3 == 0.0 || v4 == 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;
-    break;
-  default:
-    throw runtime_error("load2D: invalid noise format");
-  }
-
-  // Now, create a Gaussian noise model
-  // The smart flag will try to detect a simpler model, e.g., unit
-  SharedNoiseModel model;
-  switch (noiseFormat) {
-  case NoiseFormatG2O:
-  case NoiseFormatTORO:
-    // In both cases, what is stored in file is the information matrix
-    model = noiseModel::Gaussian::Information(M, smart);
-    break;
-  case NoiseFormatGRAPH:
-  case NoiseFormatCOV:
-    // These cases expect covariance matrix
-    model = noiseModel::Gaussian::Covariance(M, smart);
-    break;
-  default:
-    throw invalid_argument("load2D: invalid noise format");
-  }
-
-  switch (kernelFunctionType) {
-  case KernelFunctionTypeNONE:
-    return model;
-    break;
-  case KernelFunctionTypeHUBER:
-    return noiseModel::Robust::Create(
-        noiseModel::mEstimator::Huber::Create(1.345), model);
-    break;
-  case KernelFunctionTypeTUKEY:
-    return noiseModel::Robust::Create(
-        noiseModel::mEstimator::Tukey::Create(4.6851), model);
-    break;
-  default:
-    throw invalid_argument("load2D: invalid kernel function type");
-  }
-}
-
-/* ************************************************************************* */
-GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
-    bool addNoise, bool smart, NoiseFormat noiseFormat,
-    KernelFunctionType kernelFunctionType) {
-
-  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
-  while (!is.eof()) {
-    if (!(is >> tag))
-      break;
-
-    if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
-      Key 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);
-
-  // If asked, create a sampler with random number generator
-  Sampler sampler;
-  if (addNoise) {
-    noiseModel::Diagonal::shared_ptr noise;
-    if (model)
-      noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
-    if (!noise)
-      throw invalid_argument(
-          "gtsam::load2D: invalid noise model for adding noise"
-              "(current version assumes diagonal noise model)!");
-    sampler = Sampler(noise);
-  }
-
-  // Parse the pose constraints
-  Key id1, id2;
-  bool haveLandmark = false;
-  const bool useModelInFile = !model;
-  while (!is.eof()) {
-    if (!(is >> tag))
-      break;
-
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
-        || (tag == "ODOMETRY")) {
-
-      // Read transform
-      double x, y, yaw;
-      is >> id1 >> id2 >> x >> y >> yaw;
-      Pose2 l1Xl2(x, y, yaw);
-
-      // read noise model
-      SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
-          kernelFunctionType);
-
-      // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
-        continue;
-
-      if (useModelInFile)
-        model = modelInFile;
-
-      if (addNoise)
-        l1Xl2 = l1Xl2.retract(sampler.sample());
-
-      // 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);
-    }
-    // Parse measurements
-    double bearing, range, bearing_std, range_std;
-
-    // A bearing-range measurement
-    if (tag == "BR") {
-      is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
-    }
-
-    // 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;
-
-      is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
-
-      // Convert x,y to bearing,range
-      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
-      // 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);
-      } else {
-        bearing_std = 1;
-        range_std = 1;
-        if (!haveLandmark) {
-          cout
-              << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
-                  "non-uniform covariance on LANDMARK measurements in this file."
-              << endl;
-          haveLandmark = true;
-        }
-      }
-    }
-
-    // Do some common stuff for bearing-range measurements
-    if (tag == "LANDMARK" || tag == "BR") {
-
-      // optional filter
-      if (maxID && id1 >= maxID)
-        continue;
-
-      // Create noise model
-      noiseModel::Diagonal::shared_ptr measurementNoise =
-          noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std).finished());
-
-      // Add to graph
-      *graph += BearingRangeFactor<Pose2, Point2>(id1, L(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(L(id2))) {
-        Pose2 pose = initial->at<Pose2>(id1);
-        Point2 local(cos(bearing) * range, sin(bearing) * range);
-        Point2 global = pose.transform_from(local);
-        initial->insert(L(id2), global);
-      }
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-
-  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) {
-
-  fstream stream(filename.c_str(), fstream::out);
-
-  // save poses
-
-  for(const Values::ConstKeyValuePair& key_value: config) {
-    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();
-  Matrix RR = trans(R) * R; //prod(trans(R),R);
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
-    if (!factor)
-      continue;
-
-    Pose2 pose = factor->measured().inverse();
-    stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
-        << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
-        << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
-        << RR(0, 2) << " " << RR(1, 2) << endl;
-  }
-
-  stream.close();
-}
-
-/* ************************************************************************* */
-GraphAndValues readG2o(const string& g2oFile, const bool is3D,
-    KernelFunctionType kernelFunctionType) {
-  // just call load2D
-  int maxID = 0;
-  bool addNoise = false;
-  bool smart = true;
-
-  if(is3D)
-    return load3D(g2oFile);
-
-  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 2D & 3D poses
-  Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
-  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
-    stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
-        << key_value.value.y() << " " << key_value.value.theta() << endl;
-  }
-
-  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
-      Point3 p = key_value.value.translation();
-      Rot3 R = key_value.value.rotation();
-      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
-        << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
-        << " " << R.toQuaternion().w() << endl;
-  }
-
-  // save edges (2D or 3D)
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
-    if (factor){
-      SharedNoiseModel model = factor->noiseModel();
-      boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
-          boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
-      if (!gaussianModel){
-        model->print("model\n");
-        throw invalid_argument("writeG2o: invalid noise model!");
-      }
-      Matrix 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 (int i = 0; i < 3; i++){
-        for (int j = i; j < 3; j++){
-          stream << " " << Info(i, j);
-        }
-      }
-      stream << endl;
-    }
-
-    boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
-        boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
-
-    if (factor3D){
-      SharedNoiseModel model = factor3D->noiseModel();
-
-      boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
-          boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
-      if (!gaussianModel){
-        model->print("model\n");
-        throw invalid_argument("writeG2o: invalid noise model!");
-      }
-      Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
-      Pose3 pose3D = factor3D->measured();
-      Point3 p = pose3D.translation();
-      Rot3 R = pose3D.rotation();
-
-      stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
-          << p.x() << " "  << p.y() << " " << p.z()  << " " << R.toQuaternion().x()
-          << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()  << " " << R.toQuaternion().w();
-
-      Matrix InfoG2o = I_6x6;
-      InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
-      InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
-      InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
-      InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal
-
-      for (int i = 0; i < 6; i++){
-        for (int j = i; j < 6; j++){
-          stream << " " << InfoG2o(i, j);
-        }
-      }
-      stream << endl;
-    }
-  }
-  stream.close();
-}
-
-/* ************************************************************************* */
-GraphAndValues load3D(const string& filename) {
-
-  ifstream is(filename.c_str());
-  if (!is)
-    throw invalid_argument("load3D: can not find file " + filename);
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
-
-  while (!is.eof()) {
-    char buf[LINESIZE];
-    is.getline(buf, LINESIZE);
-    istringstream ls(buf);
-    string tag;
-    ls >> tag;
-
-    if (tag == "VERTEX3") {
-      Key id;
-      double x, y, z, roll, pitch, yaw;
-      ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
-    }
-    if (tag == "VERTEX_SE3:QUAT") {
-      Key id;
-      double x, y, z, qx, qy, qz, qw;
-      ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
-    }
-  }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
-
-  while (!is.eof()) {
-    char buf[LINESIZE];
-    is.getline(buf, LINESIZE);
-    istringstream ls(buf);
-    string tag;
-    ls >> tag;
-
-    if (tag == "EDGE3") {
-      Key id1, id2;
-      double x, y, z, roll, pitch, yaw;
-      ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
-      Point3 t = Point3(x, y, z);
-      Matrix m = I_6x6;
-      for (int i = 0; i < 6; i++)
-        for (int j = i; j < 6; j++)
-          ls >> m(i, j);
-      SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
-      graph->push_back(factor);
-    }
-    if (tag == "EDGE_SE3:QUAT") {
-      Matrix m = I_6x6;
-      Key id1, id2;
-      double x, y, z, qx, qy, qz, qw;
-      ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
-      Point3 t = Point3(x, y, z);
-      for (int i = 0; i < 6; i++){
-        for (int j = i; j < 6; j++){
-          double mij;
-          ls >> mij;
-          m(i, j) = mij;
-          m(j, i) = mij;
-        }
-      }
-      Matrix mgtsam = I_6x6;
-
-      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);
-      NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
-      graph->push_back(factor);
-    }
-  }
-  return make_pair(graph, initial);
-}
-
-/* ************************************************************************* */
-Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
-  /* R = [ 1   0   0
-   *       0  -1   0
-   *       0   0  -1]
-   */
-  Matrix3 R_mat = Matrix3::Zero(3, 3);
-  R_mat(0, 0) = 1.0;
-  R_mat(1, 1) = -1.0;
-  R_mat(2, 2) = -1.0;
-  return Rot3(R_mat);
-}
-
-/* ************************************************************************* */
-Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
-  Rot3 R90 = openGLFixedRotation();
-  Rot3 wRc = (R.inverse()).compose(R90);
-
-  // Our camera-to-world translation wTc = -R'*t
-  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -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));
-  return Pose3(cRw_openGL, t_openGL);
-}
-
-/* ************************************************************************* */
-Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
-  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
-      PoseGTSAM.z());
-}
-
-/* ************************************************************************* */
-bool readBundler(const string& filename, SfM_data &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBundler: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Ignore the first line
-  char aux[500];
-  is.getline(aux, 500);
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints;
-  is >> nrPoses >> nrPoints;
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    // Get the rotation matrix
-    float r11, r12, r13;
-    float r21, r22, r23;
-    float r31, r32, r33;
-    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
-
-    // Bundler-OpenGL rotation matrix
-    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
-
-    // Check for all-zero R, in which case quit
-    if (r11 == 0 && r12 == 0 && r13 == 0) {
-      cout << "Error in readBundler: zero rotation matrix for pose " << i
-          << endl;
-      return false;
-    }
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  data.tracks.reserve(nrPoints);
-  for (size_t j = 0; j < nrPoints; j++) {
-    SfM_Track track;
-
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    track.p = Point3(x, y, z);
-
-    // Get the color information
-    float r, g, b;
-    is >> r >> g >> b;
-    track.r = r / 255.f;
-    track.g = g / 255.f;
-    track.b = b / 255.f;
-
-    // Now get the visibility information
-    size_t nvisible = 0;
-    is >> nvisible;
-
-    track.measurements.reserve(nvisible);
-    track.siftIndices.reserve(nvisible);
-    for (size_t k = 0; k < nvisible; k++) {
-      size_t cam_idx = 0, point_idx = 0;
-      float u, v;
-      is >> cam_idx >> point_idx >> u >> v;
-      track.measurements.emplace_back(cam_idx, Point2(u, -v));
-      track.siftIndices.emplace_back(cam_idx, point_idx);
-    }
-
-    data.tracks.push_back(track);
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-bool readBAL(const string& filename, SfM_data &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBAL: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints, nrObservations;
-  is >> nrPoses >> nrPoints >> nrObservations;
-
-  data.tracks.resize(nrPoints);
-
-  // Get the information for the observations
-  for (size_t k = 0; k < nrObservations; k++) {
-    size_t i = 0, j = 0;
-    float u, v;
-    is >> i >> j >> u >> v;
-    data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
-  }
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the Rodrigues vector
-    float wx, wy, wz;
-    is >> wx >> wy >> wz;
-    Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  for (size_t j = 0; j < nrPoints; j++) {
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    SfM_Track& track = data.tracks[j];
-    track.p = Point3(x, y, z);
-    track.r = 0.4f;
-    track.g = 0.4f;
-    track.b = 0.4f;
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-bool writeBAL(const string& filename, SfM_data &data) {
-  // Open the output file
-  ofstream os;
-  os.open(filename.c_str());
-  os.precision(20);
-  if (!os.is_open()) {
-    cout << "Error in writeBAL: can not open the file!!" << endl;
-    return false;
-  }
-
-  // Write the number of camera poses and 3D points
-  size_t nrObservations = 0;
-  for (size_t j = 0; j < data.number_tracks(); j++) {
-    nrObservations += data.tracks[j].number_measurements();
-  }
-
-  // Write observations
-  os << data.number_cameras() << " " << data.number_tracks() << " "
-      << nrObservations << endl;
-  os << endl;
-
-  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    const SfM_Track& track = data.tracks[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
-            << "writeBAL has not been tested for calibration with nonzero (u0,v0)"
-            << endl;
-      }
-
-      double pixelBALx = track.measurements[k].second.x() - 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*/<< " "
-          << pixelMeasurement.x() /*u of the pixel*/<< " "
-          << pixelMeasurement.y() /*v of the pixel*/<< endl;
-    }
-  }
-  os << endl;
-
-  // Write cameras
-  for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
-    Pose3 poseGTSAM = data.cameras[i].pose();
-    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
-    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
-    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
-    os << poseOpenGL.translation().x() << endl;
-    os << poseOpenGL.translation().y() << endl;
-    os << poseOpenGL.translation().z() << endl;
-    os << cameraCalibration.fx() << endl;
-    os << cameraCalibration.k1() << endl;
-    os << cameraCalibration.k2() << endl;
-    os << endl;
-  }
-
-  // Write the points
-  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
-    Point3 point = data.tracks[j].p;
-    os << point.x() << endl;
-    os << point.y() << endl;
-    os << point.z() << endl;
-    os << endl;
-  }
-
-  os.close();
-  return true;
-}
-
-bool writeBALfromValues(const string& filename, const SfM_data &data,
-    Values& values) {
-
-  SfM_data dataValues = data;
-
-  // Store poses or cameras in SfM_data
-  Values valuesPoses = values.filter<Pose3>();
-  if (valuesPoses.size() == 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();
-      PinholeCamera<Cal3Bundler> camera(pose, K);
-      dataValues.cameras[i] = camera;
-    }
-  } else {
-    Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
-    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
-        Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera =
-            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
-        dataValues.cameras[i] = camera;
-      }
-    } else {
-      cout
-          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
-          << dataValues.number_cameras() << ") and values (#cameras "
-          << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
-          << endl;
-      return false;
-    }
-  }
-
-  // Store 3D points in SfM_data
-  Values valuesPoints = values.filter<Point3>();
-  if (valuesPoints.size() != dataValues.number_tracks()) {
-    cout
-        << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
-        << dataValues.number_tracks() << ") and values (#points "
-        << valuesPoints.size() << ")!!" << endl;
-  }
-
-  for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
-    Key pointKey = P(j);
-    if (values.exists(pointKey)) {
-      Point3 point = values.at<Point3>(pointKey);
-      dataValues.tracks[j].p = point;
-    } else {
-      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);
-    }
-  }
-
-  // Write SfM_data to file
-  return writeBAL(filename, dataValues);
-}
-
-Values initialCamerasEstimate(const SfM_data& db) {
-  Values initial;
-  size_t i = 0; // NO POINTS:  j = 0;
-  for(const SfM_Camera& camera: db.cameras)
-    initial.insert(i++, camera);
-  return initial;
-}
-
-Values initialCamerasAndPointsEstimate(const SfM_data& db) {
-  Values initial;
-  size_t i = 0, j = 0;
-  for(const SfM_Camera& camera: db.cameras)
-    initial.insert((i++), camera);
-  for(const SfM_Track& track: db.tracks)
-    initial.insert(P(j++), track.p);
-  return initial;
-}
-
-} // \namespace gtsam
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
deleted file mode 100644
index dc7804c2d..000000000
--- a/gtsam/slam/dataset.h
+++ /dev/null
@@ -1,257 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file dataset.h
- * @date Jan 22, 2010
- * @author nikai, Luca Carlone
- * @brief utility functions for loading datasets
- */
-
-#pragma once
-
-#include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Point2.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>
-#include <gtsam/base/types.h>
-
-#include <boost/smart_ptr/shared_ptr.hpp>
-#include <string>
-#include <utility> // for pair
-#include <vector>
-
-namespace gtsam {
-
-#ifndef MATLAB_MEX_FILE
-/**
- * Find the full path to an example dataset distributed with gtsam.  The name
- * may be specified with or without a file extension - if no extension is
- * given, this function first looks for the .graph extension, then .txt.  We
- * first check the gtsam source tree for the file, followed by the installed
- * example dataset location.  Both the source tree and installed locations
- * are obtained from CMake during compilation.
- * @return The full path and filename to the requested dataset.
- * @throw std::invalid_argument if no matching file could be found using the
- * search process described above.
- */
-GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
-
-/**
- * Creates a temporary file name that needs to be ignored in .gitingnore
- * for checking read-write oprations
- */
-GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
-#endif
-
-/// Indicates how noise parameters are stored in file
-enum NoiseFormat {
-  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
-  NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
-  NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33
-  NoiseFormatAUTO  ///< Try to guess covariance matrix layout
-};
-
-/// Robust kernel type to wrap around quadratic noise model
-enum KernelFunctionType {
-  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]
- * @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
- */
-GTSAM_EXPORT GraphAndValues load2D(
-    std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
-    bool addNoise = false,
-    bool smart = true, //
-    NoiseFormat noiseFormat = NoiseFormatAUTO,
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
-
-/**
- * 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 GraphAndValues load2D(const std::string& filename,
-    SharedNoiseModel model = SharedNoiseModel(), Key maxID = 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);
-
-/** 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 is3D indicates if the file describes a 2D or 3D problem
- * @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, const bool is3D = false,
-    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
- */
-GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
-
-/// A measurement with its camera index
-typedef std::pair<size_t, Point2> SfM_Measurement;
-
-/// SfM_Track
-typedef std::pair<size_t, size_t> SIFT_Index;
-
-/// Define the structure for the 3D points
-struct SfM_Track {
-  SfM_Track():p(0,0,0) {}
-  Point3 p; ///< 3D position of the point
-  float r, g, b; ///< RGB color of the 3D point
-  std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
-  std::vector<SIFT_Index> siftIndices;
-  size_t number_measurements() const {
-    return measurements.size();
-  }
-};
-
-/// Define the structure for the camera poses
-typedef PinholeCamera<Cal3Bundler> SfM_Camera;
-
-/// Define the structure for SfM data
-struct SfM_data {
-  std::vector<SfM_Camera> cameras; ///< Set of cameras
-  std::vector<SfM_Track> tracks; ///< Sparse set of points
-  size_t number_cameras() const {
-    return cameras.size();
-  } ///< The number of camera poses
-  size_t number_tracks() const {
-    return tracks.size();
-  } ///< The number of reconstructed 3D points
-};
-
-/**
- * @brief This function parses a bundler output file and stores the data into a
- * SfM_data structure
- * @param filename The name of the bundler file
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
-
-/**
- * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
- * SfM_data structure
- * @param filename The name of the BAL file
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool readBAL(const std::string& filename, SfM_data &data);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfM_data structure
- * @param filename The name of the BAL file to write
- * @param data SfM structure where the data is stored
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfM_data structure and a value structure (measurements are the same as the SfM input data,
- * while camera poses and values are read from Values)
- * @param filename The name of the BAL file to write
- * @param data SfM structure where the data is stored
- * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
- * cameras, and should be Point3 for the 3D points). Note that the current version
- * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
-    const SfM_data &data, Values& values);
-
-/**
- * @brief This function converts an openGL camera pose to an GTSAM camera pose
- * @param R rotation in openGL
- * @param tx x component of the translation in openGL
- * @param ty y component of the translation in openGL
- * @param tz z component of the translation in openGL
- * @return Pose3 in GTSAM format
- */
-GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param R rotation in GTSAM
- * @param tx x component of the translation in GTSAM
- * @param ty y component of the translation in GTSAM
- * @param tz z component of the translation in GTSAM
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param PoseGTSAM pose in GTSAM format
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
-
-/**
- * @brief This function creates initial values for cameras from db
- * @param SfM_data
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
-
-/**
- * @brief This function creates initial values for cameras and points from db
- * @param SfM_data
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
-
-} // namespace gtsam