Feature/wrap sfm data

gtsam/gtsam.i

@@ -2759,21 +2759,29 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+
 class SfmTrack {
-  Point3 point3() const;
+  SfmTrack();
+  SfmTrack(const gtsam::Point3& pt);
+  const 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;
+  void add_measurement(size_t idx, const gtsam::Point2& m);
 };
 
 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;
+  void add_track(const gtsam::SfmTrack& t) ;
+  void add_camera(const gtsam::SfmCamera& cam);
 };
 
 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.h

@@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 /// A measurement with its camera index
 typedef std::pair<size_t, Point2> SfmMeasurement;
 
-/// SfmTrack
+/// Sift index for SfmTrack
 typedef std::pair<size_t, size_t> SiftIndex;
 
 /// Define the structure for the 3D points
 struct SfmTrack {
   SfmTrack(): p(0,0,0) {}
+  SfmTrack(const gtsam::Point3& pt) : p(pt) {}
   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<SiftIndex> siftIndices;
+
   /// Total number of measurements in this track
   size_t number_measurements() const {
     return measurements.size();
@@ -233,11 +235,17 @@ struct SfmTrack {
   SiftIndex siftIndex(size_t idx) const {
     return siftIndices[idx];
   }
-  Point3 point3() const {
+  /// Get 3D point
+  const Point3& point3() const {
     return p;
   }
+  /// Add measurement (camera_idx, Point2) to track
+  void add_measurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
+  }
 };
 
+
 /// Define the structure for the camera poses
 typedef PinholeCamera<Cal3Bundler> SfmCamera;
 
@@ -260,6 +268,14 @@ struct SfmData {
   SfmTrack track(size_t idx) const {
     return tracks[idx];
   }
+  /// Add a track to SfmData
+  void add_track(const SfmTrack& t)  {
+    tracks.push_back(t);
+  }
+  /// Add a camera to SfmData
+  void add_camera(const SfmCamera& cam){
+    cameras.push_back(cam);
+  }
 };
 
 /**

python/gtsam/preamble.h

@@ -9,4 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
 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::IndexPair>);

python/gtsam/tests/test_SfmData.py

@@ -0,0 +1,77 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert (Python: Sushmita Warrier)
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSfmData(GtsamTestCase):
+    """Tests for SfmData and SfmTrack modules."""
+
+    def setUp(self):
+        """Initialize SfmData and SfmTrack"""
+        self.data = gtsam.SfmData()
+        # initialize SfmTrack with 3D point
+        self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2))
+
+    def test_tracks(self):
+        """Test functions in SfmTrack"""
+        # measurement is of format (camera_idx, imgPoint)
+        # create arbitrary camera indices for two cameras
+        i1, i2 = 4,5
+        # create arbitrary image measurements for cameras i1 and i2
+        uv_i1 = gtsam.Point2(12.6, 82)
+        # translating point uv_i1 along X-axis
+        uv_i2 = gtsam.Point2(24.88, 82)
+        # add measurements to the track
+        self.tracks.add_measurement(i1, uv_i1)
+        self.tracks.add_measurement(i2, uv_i2)
+        # Number of measurements in the track is 2
+        self.assertEqual(self.tracks.number_measurements(), 2)
+        # camera_idx in the first measurement of the track corresponds to i1
+        cam_idx, img_measurement = self.tracks.measurement(0)
+        self.assertEqual(cam_idx, i1)
+        np.testing.assert_array_almost_equal(
+            gtsam.Point3(2.5,3.3,1.2), 
+            self.tracks.point3()
+        )
+
+
+    def test_data(self):
+        """Test functions in SfmData"""
+        # Create new track with 3 measurements
+        track2 = gtsam.SfmTrack()
+        i1, i2, i3 = 3,5,6
+        uv_i1 = gtsam.Point2(21.23, 45.64)
+        # translating along X-axis
+        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i3 = gtsam.Point2(68.35, 45.64)
+        # add measurements to the track
+        track2.add_measurement(i1, uv_i1)
+        track2.add_measurement(i2, uv_i2)
+        track2.add_measurement(i3, uv_i3)
+        self.data.add_track(self.tracks)
+        self.data.add_track(track2)
+        # Number of tracks in SfmData is 2
+        self.assertEqual(self.data.number_tracks(), 2)
+        # camera idx of first measurement of second track corresponds to i1
+        cam_idx, img_measurement = self.data.track(1).measurement(0)
+        self.assertEqual(cam_idx, i1)
+
+if __name__ == '__main__':
+    unittest.main()