Add python equivalent for SFMExample_bal.cpp

gtsam/gtsam.i

@@ -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 {

python/gtsam/examples/SFMExample_bal.py

@@ -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())
+