From 5d6b8f445e3bcb9f986b4a6cfbb21e076e092682 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 13:44:28 -0500 Subject: [PATCH] Add to wrapper --- gtsam/sfm/sfm.i | 20 ++++++--- python/gtsam/examples/SFMExample_bal.py | 6 +-- python/gtsam/tests/test_SfmData.py | 60 ++++++++++++++++++++----- 3 files changed, 67 insertions(+), 19 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index ea229f51f..bf9a73ac5 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -31,15 +31,23 @@ class SfmTrack { #include class SfmData { SfmData(); - gtsam::SfmData FromBundlerFile(string filename); - gtsam::SfmData FromBalFile(string filename); + static gtsam::SfmData FromBundlerFile(string filename); + static gtsam::SfmData FromBalFile(string filename); - size_t numberCameras() const; - size_t numberTracks() const; - gtsam::PinholeCamera 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 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; diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 8db1c2cb4..415436157 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/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()) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 4a45f91ba..1e4fba8b6 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/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_i3 = gtsam.Point2(68.35, 45.64) + uv_i2 = Point2(45.7, 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) - track2 = gtsam.SfmTrack(pt) + pt = Point3(1.0, 6.0, 2.0) + 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()