diff --git a/gtsam.h b/gtsam.h index f943c2e9d..736fc69db 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2724,6 +2724,20 @@ 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); pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); 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 diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2106df48d..f3e802baf 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -173,14 +173,24 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { - SfM_Track():p(0,0,0) {} + /// 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]; + } }; /// Define the structure for the camera poses @@ -190,12 +200,22 @@ 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]; + } }; /**