parameters: (args: argparse.Namespace)
docstring: Run LM optimization with BAL input data and report resulting error
docstring_summary: Run LM optimization with BAL input data and report resulting error
docstring_tokens: Run LM optimization with BAL input data and report resulting error
function: def run(args: argparse.Namespace) -> None:
      """ Run LM optimization with BAL input data and report resulting error """
      input_file = args.input_file
  
      # Load the SfM data from file
      scene_data = SfmData.FromBalFile(input_file)
      logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
                   scene_data.numberCameras())
  
      # 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
      for j in range(scene_data.numberTracks()):
          track = scene_data.track(j)  # SfmTrack
          # retrieve the SfmMeasurement objects
          for m_idx in range(track.numberMeasurements()):
              # 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, i, P(j)))
  
      # Add a prior on pose x1. This indirectly specifies where the origin is.
      graph.push_back(
          PriorFactorPinholeCameraCal3Bundler(
              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(
          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 i in range(scene_data.numberCameras()):
          camera = scene_data.camera(i)
          initial.insert(i, camera)
          i += 1
  
      # add each SfmTrack
      for j in range(scene_data.numberTracks()):
          track = scene_data.track(j)
          initial.insert(P(j), track.point3())
  
      # Optimize the graph and print results
      try:
          params = gtsam.LevenbergMarquardtParams()
          params.setVerbosityLM("ERROR")
          lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
          result = lm.optimize()
      except RuntimeError:
          logging.exception("LM Optimization failed")
          return
  
      # Error drops from ~2764.22 to ~0.046
      logging.info("initial error: %f", graph.error(initial))
      logging.info("final error: %f", graph.error(result))
  
      plot_scene(scene_data, result)
function_tokens: def run ( args : argparse . Namespace ) -> None : input_file = args . input_file # Load the SfM data from file scene_data = SfmData . FromBalFile ( input_file ) logging . info ( "read %d tracks on %d cameras\n" , scene_data . numberTracks ( ) , scene_data . numberCameras ( ) ) # 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 for j in range ( scene_data . numberTracks ( ) ) : track = scene_data . track ( j ) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range ( track . numberMeasurements ( ) ) : # 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 , i , P ( j ) ) ) # Add a prior on pose x1. This indirectly specifies where the origin is. graph . push_back ( PriorFactorPinholeCameraCal3Bundler ( 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 ( 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 i in range ( scene_data . numberCameras ( ) ) : camera = scene_data . camera ( i ) initial . insert ( i , camera ) i += 1 # add each SfmTrack for j in range ( scene_data . numberTracks ( ) ) : track = scene_data . track ( j ) initial . insert ( P ( j ) , track . point3 ( ) ) # Optimize the graph and print results try : params = gtsam . LevenbergMarquardtParams ( ) params . setVerbosityLM ( "ERROR" ) lm = gtsam . LevenbergMarquardtOptimizer ( graph , initial , params ) result = lm . optimize ( ) except RuntimeError : logging . exception ( "LM Optimization failed" ) return # Error drops from ~2764.22 to ~0.046 logging . info ( "initial error: %f" , graph . error ( initial ) ) logging . info ( "final error: %f" , graph . error ( result ) ) plot_scene ( scene_data , result )