unittested features in SfmData

gtsam/gtsam.i

@@ -2759,17 +2759,19 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+// Dummy classes, for MATLAB wrappers
 class SfmMeasurement{};
 class SiftIndex{ };
 class SfmMeasurements{};
+class SfmCamera{};
 
 class SfmTrack {
   SfmTrack();
   Point3 point3() const;
   size_t number_measurements() const;
+  void setP(gtsam::Point3& p_);
   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();
 };
@@ -2780,9 +2782,8 @@ class SfmData {
   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);
+  void add_camera(gtsam::SfmCamera cam);
 };
 
 gtsam::SfmData readBal(string filename);

gtsam/slam/dataset.h

@@ -227,6 +227,11 @@ struct SfmTrack {
   size_t number_measurements() const {
     return Measurements.size();
   }
+  /// Set 3D point
+  void setP(Point3& p_){
+    p = p_;
+  }
+
   /// Get the measurement (camera index, Point2) at pose index `idx`
   SfmMeasurement measurement(size_t idx) const {
     return Measurements[idx];
@@ -246,10 +251,6 @@ struct SfmTrack {
     return Measurements;
   }
 
-  void clear() {
-    Measurements.clear();
-  }
-
 };
 
 
@@ -280,9 +281,9 @@ struct SfmData {
   void add_track(SfmTrack t) {
     tracks.push_back(t);
   }
-  /// Delete track at `idx`
-  void delete_track(size_t idx){
-    tracks[idx].clear();
+
+  void add_camera(SfmCamera cam){
+    cameras.push_back(cam);
   }
 };
 

python/CMakeLists.txt

@@ -37,6 +37,7 @@ set(ignore
     gtsam::Point2Vector
     gtsam::Pose3Vector
     gtsam::SfmMeasurement
+    gtsam::SfmCamera
     gtsam::SiftIndex
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3

python/gtsam/preamble.h

@@ -11,4 +11,5 @@ PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::P
 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>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SiftIndex>);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmCamera>);

python/gtsam/specializations.h

@@ -14,4 +14,5 @@ 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");
+py::bind_vector<std::vector<gtsam::SiftIndex> >(m_, "SiftIndexVector");
+py::bind_vector<std::vector<gtsam::SfmCamera> >(m_, "cameras");

python/gtsam/tests/test_SfmData.py

@@ -0,0 +1,79 @@
+"""
+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 import SfmCamera
+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()
+        self.tracks = gtsam.SfmTrack()
+
+    def test_tracks(self):
+        """Test functions in SfmTrack"""
+        # measurement is of format (camera_idx, imgPoint)
+        # create camera indices for two cameras
+        i1, i2 = np.random.randint(5), np.random.randint(5)
+        # create imgPoint for cameras i1 and i2
+        uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5))
+        uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5))
+        m_i1 = (i1, uv_i1)
+        m_i2 = (i2, uv_i2)
+        # add measurements to the track
+        self.tracks.add_measurement(m_i1)
+        self.tracks.add_measurement(m_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
+        self.assertEqual(self.tracks.measurement(0)[0], i1)
+        # Set arbitrary 3D point corresponding to the track
+        self.tracks.setP(gtsam.Point3(2.5, 3.3, 1.2))
+        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"""
+        #cam1 = gtsam.SfmCamera(1500, 1200, 0, 640, 480)
+        # Create new track with 3 measurements
+        track2 = gtsam.SfmTrack()
+        i1, i2, i3 = np.random.randint(5), np.random.randint(5), np.random.randint(5)
+        uv_i1 = gtsam.Point2(np.random.randint(5), np.random.randint(5))
+        uv_i2 = gtsam.Point2(np.random.randint(5), np.random.randint(5))
+        uv_i3 = gtsam.Point2(np.random.randint(5), np.random.randint(5))
+        m_i1, m_i2, m_i3 = (i1, uv_i1),  (i2, uv_i2), (i3, uv_i3)
+        # add measurements to the track
+        track2.add_measurement(m_i1)
+        track2.add_measurement(m_i2)
+        track2.add_measurement(m_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
+        self.assertEqual(self.data.track(1).measurement(0)[0], i1)
+
+if __name__ == '__main__':
+    unittest.main()