added more function to retrieve SfM_data and wrapped them

gtsam.h

@@ -2724,9 +2724,18 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+class SfM_Track {
+  size_t number_measurements() const;
+  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
+  pair<size_t, size_t> 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<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::SfM_Track track(size_t idx) const;
 };
 
 string findExampleDataFile(string name);

gtsam/slam/dataset.h

@@ -173,7 +173,7 @@ typedef std::pair<size_t, size_t> 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<SfM_Measurement> 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];
+  }
 };
 
 /**