From 34f670e9d5ba6225b7ea8705d4c7ad310429169f Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Tue, 22 Sep 2020 15:44:31 -0400
Subject: [PATCH] remove BAL for now, and add get3dPoint() for p access

---
 gtsam/gtsam.i                           |   1 +
 gtsam/slam/dataset.h                    |   3 +
 python/gtsam/examples/SFMExample_bal.py | 117 ------------------------
 3 files changed, 4 insertions(+), 117 deletions(-)
 delete mode 100644 python/gtsam/examples/SFMExample_bal.py

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 52f5901ee..78e004390 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2847,6 +2847,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 
 #include <gtsam/slam/dataset.h>
 class SfmTrack {
+  Point3 get3dPoint() const;
   size_t number_measurements() const;
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 91ceaa5fd..5bb8063bd 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -233,6 +233,9 @@ struct SfmTrack {
   SiftIndex siftIndex(size_t idx) const {
     return siftIndices[idx];
   }
+  Point3 get3dPoint() const {
+    return p;
+  }
 };
 
 /// Define the structure for the camera poses
diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py
deleted file mode 100644
index ab3baa26c..000000000
--- a/python/gtsam/examples/SFMExample_bal.py
+++ /dev/null
@@ -1,117 +0,0 @@
-"""
-  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: John Lambert
-"""
-
-import argparse
-import logging
-
-import gtsam
-import matplotlib.pyplot as plt
-import numpy as np
-
-from gtsam import symbol_shorthand
-from gtsam import readBal
-
-C = symbol_shorthand.C
-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
-# and has a total of 9 free parameters
-#typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
-
-
-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
-    if args.input_file:
-        input_file = args.input_file
-    else:
-        input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre")
-
-    # # Load the SfM data from file
-    mydata = readBal(input_file)
-    logging.info(f"read {mydata.number_tracks()} tracks on {mydata.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
-    pdb.set_trace()
-    for t_idx in range(mydata.number_tracks()):
-        track = mydata.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(0) # 
-            #graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P
-            #graph.add
-        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.addPrior(P(0), mydata.track(0).p, 
-    # equivalent of addPrior
-    graph.push_back(gtsam.PriorFactorVector(
-            P(0), mydata.track(0)[1], gtsam.noiseModel.Isotropic.Sigma(3, 0.1)))
-
-    # # Create initial estimate
-    initial = gtsam.Values()
-    
-    i = 0
-    # add each SfmCamera
-    for cam_idx in range(mydata.number_cameras()):
-        camera = mydata.camera(0)
-        initial.insert(C(i), camera)
-        i += 1
-
-    j = 0
-    # add each SfmTrack
-    for t_idx in range(mydata.number_tracks()):
-        track = mydata.track(0)  
-        initial.insert(P(j), track.p)
-        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
-
-    logging.info(f"final error: {graph.error(result)}")
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument('-i', '--input_file', type=str, default="",
-                        help='Read SFM data from the specified BAL file')
-    run(parser.parse_args())
-
-
-
-