Switch from optional to (possibly empty) shared_ptr

gtsam/slam/dataset.cpp

@@ -16,18 +16,18 @@
  * @brief utility functions for loading datasets
  */
 
-#include <fstream>
-#include <sstream>
-#include <cstdlib>
-
-#include <boost/filesystem.hpp>
-
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/linear/Sampler.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <boost/filesystem.hpp>
+
+#include <fstream>
+#include <sstream>
+#include <cstdlib>
 
 using namespace std;
 namespace fs = boost::filesystem;
@@ -90,16 +90,16 @@ string createRewrittenFileName(const string& name) {
 
 /* ************************************************************************* */
 pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
-    pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
+    pair<string, SharedNoiseModel> dataset, int maxID, bool addNoise,
-    int maxID, bool addNoise, bool smart) {
+    bool smart) {
   return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
 }
 
 /* ************************************************************************* */
 pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
-    const string& filename,
+    const string& filename, SharedNoiseModel model, int maxID, bool addNoise,
-    boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
+    bool smart) {
-    bool addNoise, bool smart) {
+
   cout << "Will try to read " << filename << endl;
   ifstream is(filename.c_str());
   if (!is)
@@ -119,9 +119,11 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
       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');
@@ -130,7 +132,17 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
   is.seekg(0, ios::beg);
 
   // Create a sampler with random number generator
-  Sampler sampler(42u);
+  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
   int id1, id2;
@@ -146,21 +158,6 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
       is >> id1 >> id2 >> x >> y >> yaw;
       is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
 
-      // Try to guess covariance matrix layout
-      Matrix m(3, 3);
-      if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
-          && v6 == 0.0) {
-        // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
-        m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
-      } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
-          && v6 != 0.0) {
-        // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
-        m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
-      } else {
-        throw std::invalid_argument(
-            "load2D: unrecognized covariance matrix format in dataset file");
-      }
-
       // optional filter
       if (maxID && (id1 >= maxID || id2 >= maxID))
         continue;
@@ -169,12 +166,28 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
 
       // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
       if (!model) {
+
+        // Try to guess covariance matrix layout
+        Matrix m(3, 3);
+        if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
+            && v6 == 0.0) {
+          // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
+          m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
+        } else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
+            && v6 != 0.0) {
+          // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
+          m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
+        } else {
+          throw std::invalid_argument(
+              "load2D: unrecognized covariance matrix format in dataset file");
+        }
+
         Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
         model = noiseModel::Diagonal::Variances(variances, smart);
       }
 
       if (addNoise)
-        l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
+        l1Xl2 = l1Xl2.retract(sampler.sample());
 
       // Insert vertices if pure odometry file
       if (!initial->exists(id1))
@@ -183,7 +196,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
         initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
 
       NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
+          new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
       graph->push_back(factor);
     }
 

gtsam/slam/dataset.h

@@ -60,8 +60,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
  * @param smart try to reduce complexity of covariance to cheapest model
  */
 GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
-    std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
+    std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
-    int maxID = 0, bool addNoise = false, bool smart = true);
+    bool addNoise = false, bool smart = true);
 
 /**
  * Load TORO 2D Graph
@@ -72,18 +72,17 @@ GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> loa
  * @param smart try to reduce complexity of covariance to cheapest model
  */
 GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
-    const std::string& filename,
+    const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
-    boost::optional<gtsam::SharedDiagonal> model = boost::optional<
+    int maxID = 0, bool addNoise = false, bool smart = true);
-    noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
-    bool smart = true);
 
 GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
-    const std::string& filename,
+    const std::string& filename, noiseModel::Base::shared_ptr& model,
-    gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
+    int maxID = 0);
 
 /** save 2d graph */
-GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
+GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
-    const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
+    const Values& config, const noiseModel::Diagonal::shared_ptr model,
+    const std::string& filename);
 
 /**
  * Load TORO 3D Graph
@@ -91,27 +90,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config
 GTSAM_EXPORT bool load3D(const std::string& filename);
 
 /// A measurement with its camera index
-typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
+typedef std::pair<size_t, Point2> SfM_Measurement;
 
 /// Define the structure for the 3D points
-struct SfM_Track
+struct SfM_Track {
-{
+  Point3 p; ///< 3D position of the point
-  gtsam::Point3 p; ///< 3D position of the point
+  float r, g, b; ///< RGB color of the 3D point
-  float r,g,b; ///< RGB color of the 3D point
   std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
-  size_t number_measurements() const { return measurements.size();}
+  size_t number_measurements() const {
+    return measurements.size();
+  }
 };
 
 /// Define the structure for the camera poses
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
+typedef PinholeCamera<Cal3Bundler> SfM_Camera;
 
 /// Define the structure for SfM data
-struct SfM_data
+struct SfM_data {
-{
+  std::vector<SfM_Camera> cameras; ///< Set of cameras
-  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_cameras() const {
-  size_t number_tracks()  const { return tracks.size();}  ///< The number of reconstructed 3D points
+    return cameras.size();
+  } ///< The number of camera poses
+  size_t number_tracks() const {
+    return tracks.size();
+  } ///< The number of reconstructed 3D points
 };
 
 /**
@@ -130,8 +133,12 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
  * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
  * @return initial Values containing the initial guess (VERTEX_SE2)
  */
-enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
+enum kernelFunctionType {
-GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
+  QUADRATIC, HUBER, TUKEY
+};
+GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
+    NonlinearFactorGraph& graph, Values& initial,
+    const kernelFunctionType kernelFunction = QUADRATIC);
 
 /**
  * @brief This function writes a g2o file from
@@ -140,7 +147,8 @@ GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& grap
  * @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);
+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
@@ -171,7 +179,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
  * 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);
+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
@@ -214,5 +223,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
  */
 GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
 
-
 } // namespace gtsam