python wrapper for sfmdata

gtsam/gtsam.i

@@ -2759,21 +2759,34 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+class SfmMeasurement{};
+class SiftIndex{ };
+class SfmMeasurements{};
+
 class SfmTrack {
+  SfmTrack();
   Point3 point3() const;
   size_t number_measurements() const;
-  pair<size_t, gtsam::Point2> measurement(size_t idx) const;
-  pair<size_t, size_t> siftIndex(size_t idx) const;
+  gtsam::SfmMeasurement measurement(size_t idx) const;
+  gtsam::SiftIndex siftIndex(size_t idx) const;
+  // gtsam::Measurements add_measurement(pair<size_t, gtsam::Point2> m);
+  void add_measurement(pair<size_t, gtsam::Point2> m);
+  SfmMeasurements& measurements();
 };
 
 class SfmData {
+  SfmData();
   size_t number_cameras() const;
   size_t number_tracks() const;
   gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
   gtsam::SfmTrack track(size_t idx) const;
+  // std::vector<gtsam::SfmTrack> add_track(gtsam::SfmTrack t);
+  void add_track(gtsam::SfmTrack t);
+  void delete_track(size_t idx);
 };
 
 gtsam::SfmData readBal(string filename);
+bool writeBAL(string filename, gtsam::SfmData& data);
 gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 

gtsam/slam/dataset.cpp

@@ -1052,13 +1052,13 @@ bool readBundler(const string &filename, SfmData &data) {
     size_t nvisible = 0;
     is >> nvisible;
 
-    track.measurements.reserve(nvisible);
+    track.Measurements.reserve(nvisible);
     track.siftIndices.reserve(nvisible);
     for (size_t k = 0; k < nvisible; k++) {
       size_t cam_idx = 0, point_idx = 0;
       float u, v;
       is >> cam_idx >> point_idx >> u >> v;
-      track.measurements.emplace_back(cam_idx, Point2(u, -v));
+      track.Measurements.emplace_back(cam_idx, Point2(u, -v));
       track.siftIndices.emplace_back(cam_idx, point_idx);
     }
 
@@ -1089,7 +1089,7 @@ bool readBAL(const string &filename, SfmData &data) {
     size_t i = 0, j = 0;
     float u, v;
     is >> i >> j >> u >> v;
-    data.tracks[j].measurements.emplace_back(i, Point2(u, -v));
+    data.tracks[j].Measurements.emplace_back(i, Point2(u, -v));
   }
 
   // Get the information for the camera poses
@@ -1163,7 +1163,7 @@ bool writeBAL(const string &filename, SfmData &data) {
 
     for (size_t k = 0; k < track.number_measurements();
          k++) { // for each observation of the 3D point j
-      size_t i = track.measurements[k].first; // camera id
+      size_t i = track.Measurements[k].first; // camera id
       double u0 = data.cameras[i].calibration().u0();
       double v0 = data.cameras[i].calibration().v0();
 
@@ -1173,9 +1173,9 @@ bool writeBAL(const string &filename, SfmData &data) {
              << endl;
       }
 
-      double pixelBALx = track.measurements[k].second.x() -
+      double pixelBALx = track.Measurements[k].second.x() -
                          u0; // center of image is the origin
-      double pixelBALy = -(track.measurements[k].second.y() -
+      double pixelBALy = -(track.Measurements[k].second.y() -
                            v0); // center of image is the origin
       Point2 pixelMeasurement(pixelBALx, pixelBALy);
       os << i /*camera id*/ << " " << j /*point id*/ << " "

gtsam/slam/dataset.h

@@ -210,6 +210,7 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
 /// A measurement with its camera index
 typedef std::pair<size_t, Point2> SfmMeasurement;
+typedef std::vector<SfmMeasurement> SfmMeasurements;
 
 /// SfmTrack
 typedef std::pair<size_t, size_t> SiftIndex;
@@ -219,15 +220,16 @@ struct SfmTrack {
   SfmTrack(): p(0,0,0) {}
   Point3 p; ///< 3D position of the point
   float r, g, b; ///< RGB color of the 3D point
-  std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
+  std::vector<SfmMeasurement> Measurements; ///< The 2D image projections (id,(u,v))
   std::vector<SiftIndex> siftIndices;
+  
   /// Total number of measurements in this track
   size_t number_measurements() const {
-    return measurements.size();
+    return Measurements.size();
   }
   /// Get the measurement (camera index, Point2) at pose index `idx`
   SfmMeasurement measurement(size_t idx) const {
-    return measurements[idx];
+    return Measurements[idx];
   }
   /// Get the SIFT feature index corresponding to the measurement at `idx`
   SiftIndex siftIndex(size_t idx) const {
@@ -236,13 +238,27 @@ struct SfmTrack {
   Point3 point3() const {
     return p;
   }
+  void add_measurement(pair<size_t, gtsam::Point2> m) {
+    Measurements.push_back(m);
+  }
+
+  SfmMeasurements& measurements() {
+    return Measurements;
+  }
+
+  void clear() {
+    Measurements.clear();
+  }
+
 };
 
+
 /// Define the structure for the camera poses
 typedef PinholeCamera<Cal3Bundler> SfmCamera;
 
 /// Define the structure for SfM data
 struct SfmData {
+  std::vector<SfmMeasurement> Measurements;
   std::vector<SfmCamera> cameras; ///< Set of cameras
   std::vector<SfmTrack> tracks; ///< Sparse set of points
   size_t number_cameras() const {
@@ -260,6 +276,14 @@ struct SfmData {
   SfmTrack track(size_t idx) const {
     return tracks[idx];
   }
+
+  void add_track(SfmTrack t) {
+    tracks.push_back(t);
+  }
+  /// Delete track at `idx`
+  void delete_track(size_t idx){
+    tracks[idx].clear();
+  }
 };
 
 /**

python/CMakeLists.txt

@@ -36,6 +36,8 @@ set(ignore
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::Pose3Vector
+    gtsam::SfmMeasurement
+    gtsam::SiftIndex
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
     gtsam::KeyPairDoubleMap)

python/gtsam/preamble.h

@@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmMeasurement>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SiftIndex>);

python/gtsam/specializations.h

@@ -13,3 +13,5 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "Bina
 py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
 py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+py::bind_vector<std::vector<gtsam::SfmMeasurement> >(m_, "Measurement");
+py::bind_vector<std::vector<gtsam::SiftIndex> >(m_, "SiftIndexVector");