From 0e1b52150d1a24b71b474b3885acc6c0e4452c60 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 31 May 2014 19:01:54 -0400 Subject: [PATCH] Switch from optional to (possibly empty) shared_ptr --- gtsam/slam/dataset.cpp | 77 ++++++++++++++++++++++++------------------ gtsam/slam/dataset.h | 62 +++++++++++++++++++--------------- 2 files changed, 80 insertions(+), 59 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 3528424fa..805c8eea6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -16,18 +16,18 @@ * @brief utility functions for loading datasets */ -#include -#include -#include - -#include - -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -90,16 +90,16 @@ string createRewrittenFileName(const string& name) { /* ************************************************************************* */ pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { + pair dataset, int maxID, bool addNoise, + bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } /* ************************************************************************* */ pair load2D( - const string& filename, - boost::optional model, int maxID, - bool addNoise, bool smart) { + const string& filename, SharedNoiseModel model, int maxID, bool addNoise, + bool smart) { + cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) @@ -119,9 +119,11 @@ pair 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 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(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 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 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 load2D( initial->insert(id2, initial->at(id1) * l1Xl2); NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, *model)); + new BetweenFactor(id1, id2, l1Xl2, model)); graph->push_back(factor); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 854daa237..858217b95 100644 --- a/gtsam/slam/dataset.h +++ b/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 load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); + std::pair dataset, int maxID = 0, + bool addNoise = false, bool smart = true); /** * Load TORO 2D Graph @@ -72,18 +72,17 @@ GTSAM_EXPORT std::pair loa * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional model = boost::optional< - noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, - bool smart = true); + const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), + int maxID = 0, bool addNoise = false, bool smart = true); GTSAM_EXPORT std::pair load2D_robust( - const std::string& filename, - gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); + 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); +GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, + 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 SfM_Measurement; +typedef std::pair SfM_Measurement; /// Define the structure for the 3D points -struct SfM_Track -{ - gtsam::Point3 p; ///< 3D position of the point - float r,g,b; ///< RGB color of the 3D point +struct SfM_Track { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point std::vector 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 SfM_Camera; +typedef PinholeCamera SfM_Camera; /// Define the structure for SfM data -struct SfM_data -{ - std::vector cameras; ///< Set of cameras +struct SfM_data { + std::vector cameras; ///< Set of cameras std::vector 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 + 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 }; /** @@ -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 }; -GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC); +enum kernelFunctionType { + 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