From 08636189fb57069ed0ec308b25a768f6cf0caf1d Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Mon, 5 Oct 2020 14:35:27 -0400
Subject: [PATCH] add WIP PR

---
 gtsam/gtsam.i                           |  5 ++++-
 python/gtsam/examples/SFMExample_bal.py | 26 ++++++++++++++-----------
 2 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index e5f323d72..1a239541d 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -1107,6 +1107,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfmCamera;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -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::SfmCamera}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2673,6 +2674,8 @@ 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::PinholeCameraCal3Bundler,gtsam::Point3> GeneralSFMFactorCal3Bundler;
+
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py
index 8efc8ec19..258ec8917 100644
--- a/python/gtsam/examples/SFMExample_bal.py
+++ b/python/gtsam/examples/SFMExample_bal.py
@@ -18,7 +18,9 @@ import numpy as np
 
 import gtsam
 from gtsam import (
+    #GeneralSFMFactorCal3Bundler,
     PinholeCameraCal3Bundler,
+    PriorFactorSfmCamera,
     readBal,
     symbol_shorthand
 )
@@ -28,14 +30,10 @@ P = symbol_shorthand.P
 
 import pdb
 
-#include <gtsam/slam/GeneralSFMFactor.h>
-
 # We will be using a projection factor that ties a SFM_Camera to a 3D point.
-# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+# An SFM_Camera is defined in dataset.h as a camera with unknown Cal3Bundler calibration
 # and has a total of 9 free parameters
-#typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
 
-PinholeCameraCal3Bundler
 def run(args):
     """ Run LM optimization with BAL input data and report resulting error """
     # Find default file, but if an argument is given, try loading a file
@@ -64,17 +62,23 @@ def run(args):
         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)
-            #graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P
-            #graph.add
+            # note use of shorthand symbols C and P
+            #graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
         j += 1
     pdb.set_trace()
 
     # # Add a prior on pose x1. This indirectly specifies where the origin is.
     # # and a prior on the position of the first landmark to fix the scale
-    graph.push_back(gtsam.PriorFactorVector(
-            C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
-    graph.push_back(gtsam.PriorFactorVector(
-            P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)))
+    graph.push_back(
+        gtsam.PriorFactorSfmCamera(
+            C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)
+        )
+    )
+    graph.push_back(
+        gtsam.PriorFactorPoint3(
+            P(0), mydata.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        )
+    )
 
     # # Create initial estimate
     initial = gtsam.Values()