From 04c12c364fbf93ce5264b4f78049a8590e6d8cdf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 1 Oct 2020 23:40:54 -0400 Subject: [PATCH 01/10] add --- python/gtsam/examples/SFMExample_bal.py | 117 ++++++++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 python/gtsam/examples/SFMExample_bal.py diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py new file mode 100644 index 000000000..8efc8ec19 --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,117 @@ +""" + GTSAM Copyright 2010, Georgia Tech Research Corporation, + Atlanta, Georgia 30332-0415 + All Rights Reserved + Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + See LICENSE for the license information + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: John Lambert +""" + +import argparse +import logging + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +from gtsam import ( + PinholeCameraCal3Bundler, + readBal, + symbol_shorthand +) + +C = symbol_shorthand.C +P = symbol_shorthand.P + +import pdb + +#include + +# We will be using a projection factor that ties a SFM_Camera to a 3D point. +# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +# and has a total of 9 free parameters +#typedef GeneralSFMFactor MyFactor; + +PinholeCameraCal3Bundler +def run(args): + """ Run LM optimization with BAL input data and report resulting error """ + # Find default file, but if an argument is given, try loading a file + if args.input_file: + input_file = args.input_file + else: + input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + + pdb.set_trace() + # # Load the SfM data from file + mydata = readBal(input_file) + logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") + + # # Create a factor graph + graph = gtsam.NonlinearFactorGraph() + + # # We share *one* noiseModel between all projection factors + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Add measurements to the factor graph + j = 0 + pdb.set_trace() + for t_idx in range(mydata.number_tracks()): + track = mydata.track(t_idx) # SfmTrack + # retrieve the SfmMeasurement objects + for m_idx in range(track.number_measurements()): + # i represents the camera index, and uv is the 2d measurement + i, uv = track.measurement(m_idx) + #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P + #graph.add + j += 1 + pdb.set_trace() + + # # Add a prior on pose x1. This indirectly specifies where the origin is. + # # and a prior on the position of the first landmark to fix the scale + graph.push_back(gtsam.PriorFactorVector( + C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) + graph.push_back(gtsam.PriorFactorVector( + P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) + + # # Create initial estimate + initial = gtsam.Values() + + i = 0 + # add each SfmCamera + for cam_idx in range(mydata.number_cameras()): + camera = mydata.camera(cam_idx) + initial.insert(C(i), camera) + i += 1 + + j = 0 + # add each SfmTrack + for t_idx in range(mydata.number_tracks()): + track = mydata.track(t_idx) + initial.insert(P(j), track.point3()) + j += 1 + + # Optimize the graph and print results + try: + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("ERROR") + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = lm.optimize() + except Exception as e: + logging.exception("LM Optimization failed") + return + + logging.info(f"final error: {graph.error(result)}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('-i', '--input_file', type=str, default="", + help='Read SFM data from the specified BAL file') + run(parser.parse_args()) + + + + From 08636189fb57069ed0ec308b25a768f6cf0caf1d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 5 Oct 2020 14:35:27 -0400 Subject: [PATCH 02/10] add WIP PR --- gtsam/gtsam.i | 5 ++++- python/gtsam/examples/SFMExample_bal.py | 26 ++++++++++++++----------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index e5f323d72..1a239541d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1107,6 +1107,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +//typedef gtsam::PinholeCamera SfmCamera; #include class StereoCamera { @@ -2528,7 +2529,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2673,6 +2674,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; + template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 8efc8ec19..258ec8917 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -18,7 +18,9 @@ import numpy as np import gtsam from gtsam import ( + #GeneralSFMFactorCal3Bundler, PinholeCameraCal3Bundler, + PriorFactorSfmCamera, readBal, symbol_shorthand ) @@ -28,14 +30,10 @@ P = symbol_shorthand.P import pdb -#include - # We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration # and has a total of 9 free parameters -#typedef GeneralSFMFactor MyFactor; -PinholeCameraCal3Bundler def run(args): """ Run LM optimization with BAL input data and report resulting error """ # Find default file, but if an argument is given, try loading a file @@ -64,17 +62,23 @@ def run(args): for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) - #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P - #graph.add + # note use of shorthand symbols C and P + #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) j += 1 pdb.set_trace() # # Add a prior on pose x1. This indirectly specifies where the origin is. # # and a prior on the position of the first landmark to fix the scale - graph.push_back(gtsam.PriorFactorVector( - C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) - graph.push_back(gtsam.PriorFactorVector( - P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) + graph.push_back( + gtsam.PriorFactorSfmCamera( + C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) + ) + ) + graph.push_back( + gtsam.PriorFactorPoint3( + P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + ) + ) # # Create initial estimate initial = gtsam.Values() From 93825d0bc72a7d1764e1c9a5f3d8431132c8e63b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 8 Oct 2020 07:23:35 +0000 Subject: [PATCH 03/10] Wrapping SfmCamera to be used with GeneralSFMFactor --- gtsam/gtsam.i | 16 ++++++++-------- python/gtsam/examples/SFMExample_bal.py | 9 ++++----- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 1a239541d..6665a532e 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1106,8 +1106,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -//typedef gtsam::PinholeCamera SfmCamera; +typedef gtsam::PinholeCamera SfmCamera; #include class StereoCamera { @@ -2069,7 +2068,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template + template void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2163,7 +2162,7 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::SfmCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); @@ -2181,13 +2180,13 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::SfmCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double @@ -2491,7 +2490,8 @@ class ISAM2 { template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::SfmCamera, + Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -2674,7 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorSfmCamera; template diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 258ec8917..a5cbcf05f 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -18,8 +18,8 @@ import numpy as np import gtsam from gtsam import ( - #GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, + GeneralSFMFactorSfmCamera, + SfmCamera, PriorFactorSfmCamera, readBal, symbol_shorthand @@ -31,7 +31,7 @@ P = symbol_shorthand.P import pdb # We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration +# An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration # and has a total of 9 free parameters def run(args): @@ -63,7 +63,7 @@ def run(args): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) j += 1 pdb.set_trace() @@ -118,4 +118,3 @@ if __name__ == "__main__": - From c97af55c63534646f2bd026379d40a6155c2dcd6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Oct 2020 23:33:53 -0400 Subject: [PATCH 04/10] remove breakpoints --- python/gtsam/examples/SFMExample_bal.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index a5cbcf05f..169cf9f3c 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,11 +7,12 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: John Lambert + Author: Frank Dellaert (Python: Akshay Krishan, John Lambert) """ import argparse import logging +import sys import matplotlib.pyplot as plt import numpy as np @@ -28,7 +29,8 @@ from gtsam import ( C = symbol_shorthand.C P = symbol_shorthand.P -import pdb + +logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) # We will be using a projection factor that ties a SFM_Camera to a 3D point. # An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration @@ -42,7 +44,6 @@ def run(args): else: input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") - pdb.set_trace() # # Load the SfM data from file mydata = readBal(input_file) logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") @@ -55,7 +56,6 @@ def run(args): # Add measurements to the factor graph j = 0 - pdb.set_trace() for t_idx in range(mydata.number_tracks()): track = mydata.track(t_idx) # SfmTrack # retrieve the SfmMeasurement objects @@ -65,7 +65,6 @@ def run(args): # note use of shorthand symbols C and P graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) j += 1 - pdb.set_trace() # # Add a prior on pose x1. This indirectly specifies where the origin is. # # and a prior on the position of the first landmark to fix the scale From ac9077ff67f5ca36790203ac417982e9e88a52e4 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sat, 10 Oct 2020 08:57:09 +0000 Subject: [PATCH 05/10] Renaming SFMCamera to PinholeCameraCal3Bundler --- gtsam/gtsam.i | 17 ++++++++--------- python/gtsam/examples/SFMExample_bal.py | 16 ++++++---------- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6665a532e..2a513322d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1106,7 +1106,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera SfmCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; #include class StereoCamera { @@ -2068,7 +2068,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template + template, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2162,7 +2162,7 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::SfmCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); @@ -2180,13 +2180,13 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::SfmCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> T at(size_t j); /// version for double @@ -2490,7 +2490,7 @@ class ISAM2 { template , Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2529,7 +2529,7 @@ class NonlinearISAM { #include #include -template +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2674,8 +2674,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorSfmCamera; - +typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 169cf9f3c..a027dac89 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -19,9 +19,9 @@ import numpy as np import gtsam from gtsam import ( - GeneralSFMFactorSfmCamera, - SfmCamera, - PriorFactorSfmCamera, + GeneralSFMFactorCal3Bundler, + PinholeCameraCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, readBal, symbol_shorthand ) @@ -32,10 +32,6 @@ P = symbol_shorthand.P logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) -# We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SfmCamera is defined in dataset.h as a pinhole camera with unknown Cal3Bundler calibration -# and has a total of 9 free parameters - def run(args): """ Run LM optimization with BAL input data and report resulting error """ # Find default file, but if an argument is given, try loading a file @@ -63,13 +59,13 @@ def run(args): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P - graph.add(GeneralSFMFactorSfmCamera(uv, noise, C(i), P(j))) + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) j += 1 # # Add a prior on pose x1. This indirectly specifies where the origin is. # # and a prior on the position of the first landmark to fix the scale graph.push_back( - gtsam.PriorFactorSfmCamera( + gtsam.PriorFactorPinholeCameraCal3Bundler( C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) @@ -83,7 +79,7 @@ def run(args): initial = gtsam.Values() i = 0 - # add each SfmCamera + # add each PinholeCameraCal3Bundler for cam_idx in range(mydata.number_cameras()): camera = mydata.camera(cam_idx) initial.insert(C(i), camera) From 25d801bd15eeda8244b36ae384eb8c5d318966db Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 15:22:23 -0400 Subject: [PATCH 06/10] use argparse defaults --- python/gtsam/examples/SFMExample_bal.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index a027dac89..eda143011 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -34,11 +34,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) def run(args): """ Run LM optimization with BAL input data and report resulting error """ - # Find default file, but if an argument is given, try loading a file - if args.input_file: - input_file = args.input_file - else: - input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + input_file = gtsam.findExampleDataFile(args.input_file) # # Load the SfM data from file mydata = readBal(input_file) @@ -62,13 +58,13 @@ def run(args): graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) j += 1 - # # Add a prior on pose x1. This indirectly specifies where the origin is. - # # and a prior on the position of the first landmark to fix the scale + # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( gtsam.PriorFactorPinholeCameraCal3Bundler( C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) + # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) @@ -107,7 +103,7 @@ def run(args): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="", + parser.add_argument('-i', '--input_file', type=str, default="dubrovnik-3-7-pre", help='Read SFM data from the specified BAL file') run(parser.parse_args()) From c9d719cb1f2992c10d43afa48ebb5c2206e142f0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 16:03:14 -0400 Subject: [PATCH 07/10] make a note about how the eror drops --- python/gtsam/examples/SFMExample_bal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index eda143011..b5701669e 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -97,7 +97,7 @@ def run(args): except Exception as e: logging.exception("LM Optimization failed") return - + # Error drops from 2764.22 to 0.046 logging.info(f"final error: {graph.error(result)}") From b4bbad32df891a1f1c8178d2e5af38a3e8b85983 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 14 Oct 2020 21:33:13 -0400 Subject: [PATCH 08/10] fix typo --- python/gtsam/examples/SFMExample_bal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index b5701669e..77d79ca24 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,7 +7,7 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishan, John Lambert) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) """ import argparse From b1c53000f7a1466100086c230529b327f742017d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 17 Oct 2020 12:47:57 -0400 Subject: [PATCH 09/10] clean up comments --- python/gtsam/examples/SFMExample_bal.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77d79ca24..3ce465f6f 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -36,14 +36,14 @@ def run(args): """ Run LM optimization with BAL input data and report resulting error """ input_file = gtsam.findExampleDataFile(args.input_file) - # # Load the SfM data from file + # Load the SfM data from file mydata = readBal(input_file) logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") - # # Create a factor graph + # Create a factor graph graph = gtsam.NonlinearFactorGraph() - # # We share *one* noiseModel between all projection factors + # We share *one* noiseModel between all projection factors noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph @@ -71,7 +71,7 @@ def run(args): ) ) - # # Create initial estimate + # Create initial estimate initial = gtsam.Values() i = 0 @@ -97,7 +97,7 @@ def run(args): except Exception as e: logging.exception("LM Optimization failed") return - # Error drops from 2764.22 to 0.046 + # Error drops from ~2764.22 to ~0.046 logging.info(f"final error: {graph.error(result)}") @@ -107,5 +107,3 @@ if __name__ == "__main__": help='Read SFM data from the specified BAL file') run(parser.parse_args()) - - From 0d88438a2a47d2af2bc7914b662f8623ad8294dd Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 19 Oct 2020 09:08:54 -0400 Subject: [PATCH 10/10] renamed myData to scene_data, and explained BAL parameterization --- python/gtsam/examples/SFMExample_bal.py | 33 ++++++++++++++++--------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77d79ca24..34696700c 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -37,8 +37,8 @@ def run(args): input_file = gtsam.findExampleDataFile(args.input_file) # # Load the SfM data from file - mydata = readBal(input_file) - logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") + scene_data = readBal(input_file) + logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") # # Create a factor graph graph = gtsam.NonlinearFactorGraph() @@ -48,8 +48,8 @@ def run(args): # Add measurements to the factor graph j = 0 - for t_idx in range(mydata.number_tracks()): - track = mydata.track(t_idx) # SfmTrack + for t_idx in range(scene_data.number_tracks()): + track = scene_data.track(t_idx) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement @@ -61,13 +61,13 @@ def run(args): # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) + C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) ) ) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( gtsam.PriorFactorPoint3( - P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) ) ) @@ -76,15 +76,15 @@ def run(args): i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(mydata.number_cameras()): - camera = mydata.camera(cam_idx) + for cam_idx in range(scene_data.number_cameras()): + camera = scene_data.camera(cam_idx) initial.insert(C(i), camera) i += 1 j = 0 # add each SfmTrack - for t_idx in range(mydata.number_tracks()): - track = mydata.track(t_idx) + for t_idx in range(scene_data.number_tracks()): + track = scene_data.track(t_idx) initial.insert(P(j), track.point3()) j += 1 @@ -103,8 +103,17 @@ def run(args): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file') + parser.add_argument( + '-i', + '--input_file', + type=str, + default="dubrovnik-3-7-pre", + help='Read SFM data from the specified BAL file' + 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' + 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' + 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' + 'and (x,y,z) 3d point initializations.' + ) run(parser.parse_args())