diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py new file mode 100644 index 000000000..50e450fea --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,111 @@ +""" + 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 gtsam +import matplotlib.pyplot as plt +import numpy as np + +from gtsam import symbol_shorthand +from gtsam import readBal + +L = symbol_shorthand.L +X = symbol_shorthand.X + +import pdb + +#include +#include +#include +#include +#include // for loading BAL datasets ! + + +# using symbol_shorthand::C; +# using symbol_shorthand::P; + +# 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; + + +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") + + # # 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 + for m_idx in range(track.number_measurements()): + # retrieve the SfmMeasurement + # i represents the camera index, and uv is the 2d measurement + i, uv = track.measurement(0) # + #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.addPrior(C(0), mydata.cameras[0], gtsam.noiseModel.Isotropic.Sigma(9, 0.1)) + # graph.addPrior(P(0), mydata.tracks[0].p, gtsam.noiseModel.Isotropic.Sigma(3, 0.1)) + + # # Create initial estimate + initial = gtsam.Values() + # i = 0 + # j = 0; + # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) + # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) + + # Optimize the graph and print results + # Values result; + 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()) + + + +