Merge pull request #556 from johnwlambert/add_python_sfm_example_bal
Add python equivalent for SFMExample_bal.cpprelease/4.3a0
						commit
						5adf4dc50a
					
				|  | @ -2068,7 +2068,7 @@ class NonlinearFactorGraph { | |||
|   gtsam::KeySet keys() const; | ||||
|   gtsam::KeyVector keyVector() const; | ||||
| 
 | ||||
|   template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> | ||||
|   template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, 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<gtsam::Cal3Bundler>& 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<gtsam::Cal3Bundler>& 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<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> | ||||
|   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> | ||||
|   T at(size_t j); | ||||
| 
 | ||||
|   /// version for double | ||||
|  | @ -2490,7 +2490,8 @@ class ISAM2 { | |||
|   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, | ||||
|                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, | ||||
|                      gtsam::Cal3Bundler, gtsam::EssentialMatrix, | ||||
|                      gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}> | ||||
|                      gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,  | ||||
|                      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 <gtsam/geometry/StereoPoint2.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/PriorFactor.h> | ||||
| template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> | ||||
| template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}> | ||||
| 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<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; | ||||
| //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 | ||||
| //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; | ||||
| typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler; | ||||
| 
 | ||||
| template<CALIBRATION = {gtsam::Cal3_S2}> | ||||
| virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { | ||||
|  |  | |||
|  | @ -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()) | ||||
| 
 | ||||
		Loading…
	
		Reference in New Issue