From 381f7bee3081b7191c383ba077301bae69a3ee06 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 21 Feb 2020 18:36:50 -0500 Subject: [PATCH 1/4] wrap SfM_data struct --- gtsam.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam.h b/gtsam.h index f943c2e9d..564afcaca 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2724,6 +2724,11 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +class SfM_data { + size_t number_cameras() const; + size_t number_tracks() const; +}; + string findExampleDataFile(string name); pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); From a2ef54c60f45242aead921dc34e765ec133526b1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Mar 2020 21:41:44 -0500 Subject: [PATCH 2/4] added more function to retrieve SfM_data and wrapped them --- gtsam.h | 9 +++++++++ gtsam/slam/dataset.h | 14 +++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 564afcaca..736fc69db 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2724,9 +2724,18 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include +class SfM_Track { + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair SIFT_index(size_t idx) const; +}; + class SfM_data { size_t number_cameras() const; size_t number_tracks() const; + //TODO(Varun) Need to fix issue #237 first before this can work + // gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfM_Track track(size_t idx) const; }; string findExampleDataFile(string name); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2106df48d..3f8c6fd65 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -173,7 +173,7 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { - SfM_Track():p(0,0,0) {} + 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 measurements; ///< The 2D image projections (id,(u,v)) @@ -181,6 +181,12 @@ struct SfM_Track { size_t number_measurements() const { return measurements.size(); } + SfM_Measurement measurement(size_t idx) const { + return measurements[idx]; + } + SIFT_Index SIFT_index(size_t idx) const { + return siftIndices[idx]; + } }; /// Define the structure for the camera poses @@ -196,6 +202,12 @@ struct SfM_data { size_t number_tracks() const { return tracks.size(); } ///< The number of reconstructed 3D points + SfM_Camera camera(size_t idx) const { + return cameras[idx]; + } + SfM_Track track(size_t idx) const { + return tracks[idx]; + } }; /** From 105c4b809e0e15e57c8dc08b3b6b00e4047f3c3e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Mar 2020 21:42:13 -0500 Subject: [PATCH 3/4] minor typo fixes --- gtsam/geometry/Cal3Bundler.cpp | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 223bcc242..0d64d0184 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -116,7 +116,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { if (iteration >= maxIterations) throw std::runtime_error( - "Cal3DS2::calibrate fails to converge. need a better initialization"); + "Cal3Bundler::calibrate fails to converge. need a better initialization"); return pn; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index d95c47f7b..4787f565b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -117,7 +117,7 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Conver a pixel coordinate to ideal coordinate + /// Convert a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate From 33924220a31221f09a1f249e87905034f470c927 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Mar 2020 13:22:29 -0500 Subject: [PATCH 4/4] added docstrings for SFM structs --- gtsam/slam/dataset.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3f8c6fd65..f3e802baf 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -173,17 +173,21 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { + /// Construct empty 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 measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; + /// Total number of measurements in this track size_t number_measurements() const { return measurements.size(); } + /// Get the measurement (camera index, Point2) at pose index `idx` SfM_Measurement measurement(size_t idx) const { return measurements[idx]; } + /// Get the SIFT feature index corresponding to the measurement at `idx` SIFT_Index SIFT_index(size_t idx) const { return siftIndices[idx]; } @@ -196,15 +200,19 @@ typedef PinholeCamera SfM_Camera; struct SfM_data { std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points + /// The number of camera poses size_t number_cameras() const { return cameras.size(); - } ///< The number of camera poses + } + /// The number of reconstructed 3D points size_t number_tracks() const { return tracks.size(); - } ///< The number of reconstructed 3D points + } + /// The camera pose at frame index `idx` SfM_Camera camera(size_t idx) const { return cameras[idx]; } + /// The track formed by series of landmark measurements SfM_Track track(size_t idx) const { return tracks[idx]; }