2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
10 Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal)
18 from gtsam
import (GeneralSFMFactorCal3Bundler, SfmData,
19 PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
22 from matplotlib
import pyplot
as plt
24 logging.basicConfig(stream=sys.stdout, level=logging.INFO)
26 DEFAULT_BAL_DATASET =
"dubrovnik-3-7-pre"
30 """Plot the SFM results."""
32 for i
in range(scene_data.numberCameras()):
33 plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).
pose())
34 for j
in range(scene_data.numberTracks()):
35 plot_vals.insert(
P(j), result.atPoint3(
P(j)))
37 plot.plot_3d_points(0, plot_vals, linespec=
"g.")
38 plot.plot_trajectory(0, plot_vals, title=
"SFM results")
43 def run(args: argparse.Namespace) ->
None:
44 """ Run LM optimization with BAL input data and report resulting error """
45 input_file = args.input_file
48 scene_data = SfmData.FromBalFile(input_file)
49 logging.info(
"read %d tracks on %d cameras\n", scene_data.numberTracks(),
50 scene_data.numberCameras())
59 for j
in range(scene_data.numberTracks()):
60 track = scene_data.track(j)
62 for m_idx
in range(track.numberMeasurements()):
64 i, uv = track.measurement(m_idx)
70 PriorFactorPinholeCameraCal3Bundler(
71 0, scene_data.camera(0),
76 scene_data.track(0).
point3(),
84 for i
in range(scene_data.numberCameras()):
85 camera = scene_data.camera(i)
86 initial.insert(i, camera)
90 for j
in range(scene_data.numberTracks()):
91 track = scene_data.track(j)
92 initial.insert(
P(j), track.point3())
97 params.setVerbosityLM(
"ERROR")
99 result = lm.optimize()
101 logging.exception(
"LM Optimization failed")
105 logging.info(
"initial error: %f", graph.error(initial))
106 logging.info(
"final error: %f", graph.error(result))
113 parser = argparse.ArgumentParser()
114 parser.add_argument(
'-i',
118 help=
"""Read SFM data from the specified BAL file.
119 The data format is described here: https://grail.cs.washington.edu/projects/bal/.
120 BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples,
121 then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector
122 and (x,y,z) 3d point initializations.""")
123 run(parser.parse_args())
126 if __name__ ==
"__main__":