Merge pull request #227 from borglab/wrap-sfm-data

Wrap SfM_data struct

gtsam.h

@@ -2724,6 +2724,20 @@ 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);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);

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;
 }

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

gtsam/slam/dataset.h

@@ -173,14 +173,24 @@ 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) {}
+  /// 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<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
   std::vector<SIFT_Index> 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<Cal3Bundler> SfM_Camera;
 struct SfM_data {
   std::vector<SfM_Camera> cameras; ///< Set of cameras
   std::vector<SfM_Track> 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];
+  }
 };
 
 /**