SFMExample_bal.py
Go to the documentation of this file.
1 """
2  GTSAM Copyright 2010, Georgia Tech Research Corporation,
3  Atlanta, Georgia 30332-0415
4  All Rights Reserved
5  Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7  See LICENSE for the license information
8 
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)
11 """
12 
13 import argparse
14 import logging
15 import sys
16 
17 import gtsam
18 from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
19  PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
20 from gtsam.symbol_shorthand import P # type: ignore
21 from gtsam.utils import plot # type: ignore
22 from matplotlib import pyplot as plt
23 
24 logging.basicConfig(stream=sys.stdout, level=logging.INFO)
25 
26 DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
27 
28 
29 def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
30  """Plot the SFM results."""
31  plot_vals = gtsam.Values()
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)))
36 
37  plot.plot_3d_points(0, plot_vals, linespec="g.")
38  plot.plot_trajectory(0, plot_vals, title="SFM results")
39 
40  plt.show()
41 
42 
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
46 
47  # Load the SfM data from 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())
51 
52  # Create a factor graph
54 
55  # We share *one* noiseModel between all projection factors
56  noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
57 
58  # Add measurements to the factor graph
59  for j in range(scene_data.numberTracks()):
60  track = scene_data.track(j) # SfmTrack
61  # retrieve the SfmMeasurement objects
62  for m_idx in range(track.numberMeasurements()):
63  # i represents the camera index, and uv is the 2d measurement
64  i, uv = track.measurement(m_idx)
65  # note use of shorthand symbols C and P
66  graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
67 
68  # Add a prior on pose x1. This indirectly specifies where the origin is.
69  graph.push_back(
70  PriorFactorPinholeCameraCal3Bundler(
71  0, scene_data.camera(0),
73  # Also add a prior on the position of the first landmark to fix the scale
74  graph.push_back(
76  scene_data.track(0).point3(),
78 
79  # Create initial estimate
80  initial = gtsam.Values()
81 
82  i = 0
83  # add each PinholeCameraCal3Bundler
84  for i in range(scene_data.numberCameras()):
85  camera = scene_data.camera(i)
86  initial.insert(i, camera)
87  i += 1
88 
89  # add each SfmTrack
90  for j in range(scene_data.numberTracks()):
91  track = scene_data.track(j)
92  initial.insert(P(j), track.point3())
93 
94  # Optimize the graph and print results
95  try:
97  params.setVerbosityLM("ERROR")
98  lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
99  result = lm.optimize()
100  except RuntimeError:
101  logging.exception("LM Optimization failed")
102  return
103 
104  # Error drops from ~2764.22 to ~0.046
105  logging.info("initial error: %f", graph.error(initial))
106  logging.info("final error: %f", graph.error(result))
107 
108  plot_scene(scene_data, result)
109 
110 
111 def main() -> None:
112  """Main runner."""
113  parser = argparse.ArgumentParser()
114  parser.add_argument('-i',
115  '--input_file',
116  type=str,
117  default=gtsam.findExampleDataFile(DEFAULT_BAL_DATASET),
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())
124 
125 
126 if __name__ == "__main__":
127  main()
Point3_ point3(const Unit3_ &v)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Double_ range(const Point2_ &p, const Point2_ &q)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42