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]; + } }; /**