Add to wrapper

gtsam/sfm/sfm.i

@@ -31,15 +31,23 @@ class SfmTrack {
 #include <gtsam/sfm/SfmData.h>
 class SfmData {
   SfmData();
-  gtsam::SfmData FromBundlerFile(string filename);
+  static gtsam::SfmData FromBundlerFile(string filename);
-  gtsam::SfmData FromBalFile(string filename);
+  static gtsam::SfmData FromBalFile(string filename);
 
-  size_t numberCameras() const;
-  size_t numberTracks() const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
-  gtsam::SfmTrack track(size_t idx) const;
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
+  size_t numberTracks() const;
+  size_t numberCameras() const;
+  gtsam::SfmTrack track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+
+  gtsam::NonlinearFactorGraph generalSfmFactors(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
+  gtsam::NonlinearFactorGraph sfmFactorGraph(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
+      size_t fixedCamera = 0, size_t fixedPoint = 0) const;
 
   // enabling serialization functionality
   void serialize() const;

python/gtsam/examples/SFMExample_bal.py

@@ -15,7 +15,7 @@ import logging
 import sys
 
 import gtsam
-from gtsam import (GeneralSFMFactorCal3Bundler,
+from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
                    PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
 from gtsam.symbol_shorthand import C, P  # type: ignore
 from gtsam.utils import plot  # type: ignore
@@ -26,7 +26,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
 DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
 
 
-def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
+def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
     """Plot the SFM results."""
     plot_vals = gtsam.Values()
     for cam_idx in range(scene_data.numberCameras()):
@@ -46,7 +46,7 @@ def run(args: argparse.Namespace) -> None:
     input_file = args.input_file
 
     # Load the SfM data from file
-    scene_data = gtsam.readBal(input_file)
+    scene_data = SfmData.FromBalFile(input_file)
     logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
                  scene_data.numberCameras())
 

python/gtsam/tests/test_SfmData.py

@@ -17,6 +17,7 @@ import unittest
 import numpy as np
 
 import gtsam
+from gtsam import SfmData, SfmTrack, Point2, Point3
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -25,29 +26,34 @@ class TestSfmData(GtsamTestCase):
 
     def setUp(self):
         """Initialize SfmData and SfmTrack"""
-        self.data = gtsam.SfmData()
+        self.data = SfmData()
         # initialize SfmTrack with 3D point
-        self.tracks = gtsam.SfmTrack()
+        self.tracks = SfmTrack()
 
     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)
+        uv_i1 = Point2(12.6, 82)
+        
         # translating point uv_i1 along X-axis
-        uv_i2 = gtsam.Point2(24.88, 82)
+        uv_i2 = Point2(24.88, 82)
+        
         # add measurements to the track
         self.tracks.addMeasurement(i1, uv_i1)
         self.tracks.addMeasurement(i2, uv_i2)
+        
         # Number of measurements in the track is 2
         self.assertEqual(self.tracks.numberMeasurements(), 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(0.,0.,0.), 
+            Point3(0.,0.,0.), 
             self.tracks.point3()
         )
 
@@ -56,24 +62,58 @@ class TestSfmData(GtsamTestCase):
         """Test functions in SfmData"""
         # Create new track with 3 measurements
         i1, i2, i3 = 3,5,6
-        uv_i1 = gtsam.Point2(21.23, 45.64)
+        uv_i1 = Point2(21.23, 45.64)
+        
         # translating along X-axis
-        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i2 = Point2(45.7, 45.64)
-        uv_i3 = gtsam.Point2(68.35, 45.64)
+        uv_i3 = Point2(68.35, 45.64)
+        
         # add measurements and arbitrary point to the track
         measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
-        pt = gtsam.Point3(1.0, 6.0, 2.0)
+        pt = Point3(1.0, 6.0, 2.0)
-        track2 = gtsam.SfmTrack(pt)
+        track2 = SfmTrack(pt)
         track2.addMeasurement(i1, uv_i1)
         track2.addMeasurement(i2, uv_i2)
         track2.addMeasurement(i3, uv_i3)
         self.data.addTrack(self.tracks)
         self.data.addTrack(track2)
+
         # Number of tracks in SfmData is 2
         self.assertEqual(self.data.numberTracks(), 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)
 
+    def test_Balbianello(self):
+        # The structure where we will save the SfM data
+        filename = gtsam.findExampleDataFile("Balbianello")
+        sfm_data = SfmData.FromBundlerFile(filename)
+
+        # Check number of things
+        self.assertEqual(5, sfm_data.numberCameras())
+        self.assertEqual(544, sfm_data.numberTracks())
+        track0 = sfm_data.track(0)
+        self.assertEqual(3, track0.numberMeasurements())
+
+        # Check projection of a given point
+        self.assertEqual(0, track0.measurement(0)[0])
+        camera0 = sfm_data.camera(0)
+        expected = camera0.project(track0.point3())
+        actual = track0.measurement(0)[1]
+        self.gtsamAssertEquals(expected, actual, 1)
+
+        # We share *one* noiseModel between all projection factors
+        model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+
+        # Convert to NonlinearFactorGraph
+        graph = sfm_data.sfmFactorGraph(model)
+        self.assertEqual(1419, graph.size())  # regression
+
+        # Get initial estimate
+        values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
+        self.assertEqual(549, values.size())  # regression
+
+
 if __name__ == '__main__':
     unittest.main()