diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..575a216d6 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -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::PinholeCameraCal3Bundler& 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::PinholeCameraCal3Bundler& 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,8 @@ class ISAM2 { template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera, + Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -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,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, 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 new file mode 100644 index 000000000..dfe8b523c --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,118 @@ +""" + 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: Frank Dellaert (Python: Akshay Krishnan, John Lambert) +""" + +import argparse +import logging +import sys + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +from gtsam import ( + GeneralSFMFactorCal3Bundler, + PinholeCameraCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, + readBal, + symbol_shorthand +) + +C = symbol_shorthand.C +P = symbol_shorthand.P + + +logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) + +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 + 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() + + # 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 + 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 + i, uv = track.measurement(m_idx) + # note use of shorthand symbols C and P + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + j += 1 + + # Add a prior on pose x1. This indirectly specifies where the origin is. + graph.push_back( + gtsam.PriorFactorPinholeCameraCal3Bundler( + 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), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + ) + ) + + # Create initial estimate + initial = gtsam.Values() + + i = 0 + # add each PinholeCameraCal3Bundler + 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(scene_data.number_tracks()): + track = scene_data.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 + # Error drops from ~2764.22 to ~0.046 + logging.info(f"final error: {graph.error(result)}") + + +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' + '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()) +