From 8fd2d9842426e452207b39daab0c8901243889d5 Mon Sep 17 00:00:00 2001
From: Sushmita <sushmi246@gmail.com>
Date: Tue, 1 Dec 2020 19:31:44 -0500
Subject: [PATCH] templated functions where possible

---
 gtsam/geometry/triangulation.h           |  7 +++--
 gtsam/gtsam.i                            | 34 +++++++++---------------
 python/CMakeLists.txt                    |  2 --
 python/gtsam/preamble.h                  |  4 +--
 python/gtsam/specializations.h           |  4 +--
 python/gtsam/tests/test_Triangulation.py |  2 +-
 6 files changed, 22 insertions(+), 31 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 23ea7e50b..617d09da7 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -497,8 +497,11 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 }
 
 // Vector of Cameras - used by the Python/MATLAB wrapper
-typedef CameraSet<PinholeCamera<Cal3Bundler>> CameraSetCal3Bundler;
-typedef CameraSet<PinholeCamera<Cal3_S2>> CameraSetCal3_S2;
+//typedef CameraSet<PinholeCamera<Cal3Bundler>> CameraSetCal3Bundler;
+//typedef CameraSet<PinholeCamera<Cal3_S2>> CameraSetCal3_S2;
+
+using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
+using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
 
 } // \namespace gtsam
 
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index f6c2da853..a8b1cf4f4 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -1109,31 +1109,22 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 
-class CameraSetCal3Bundler {
-  CameraSetCal3Bundler();
+template<T = {gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3_S2}>
+class CameraSet {
+  CameraSet();
 
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
+  // // common STL methods
+  // size_t size() const;
+  // bool empty() const;
+  // void clear();
 
-  // structure specific methods
-  gtsam::PinholeCameraCal3Bundler at(size_t i) const;
-  void push_back(const gtsam::PinholeCameraCal3Bundler& cam);
+  // // structure specific methods
+  // T at(size_t i) const;
+  void push_back(const T& cam);
 };
 
-class CameraSetCal3_S2 {
-  CameraSetCal3_S2();
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  // structure specific methods
-  gtsam::PinholeCameraCal3_S2 at(size_t i) const;
-  void push_back(const gtsam::PinholeCameraCal3_S2& cam);
-};
+typedef gtsam::CameraSet<gtsam::PinholeCameraCal3_S2> CameraSetCal3_S2;
+typedef gtsam::CameraSet<gtsam::PinholeCameraCal3Bundler> CameraSetCal3Bundler;
 
 #include <gtsam/geometry/StereoCamera.h>
 class StereoCamera {
@@ -1166,7 +1157,6 @@ class StereoCamera {
 
 #include <gtsam/geometry/triangulation.h>
 
-// Templates appear not yet supported for free functions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
     gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
     double rank_tol, bool optimize);
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index a318a483b..00b537340 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -38,8 +38,6 @@ set(ignore
     gtsam::Pose3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
-    gtsam::CameraSetCal3Bundler
-    gtsam::CameraSetCal3_S2
     gtsam::KeyPairDoubleMap)
 
 pybind_wrap(gtsam_py # target
diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h
index b56766c72..92ad14797 100644
--- a/python/gtsam/preamble.h
+++ b/python/gtsam/preamble.h
@@ -10,5 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
-PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
-PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
+//PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
+//PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
index 431697aac..8f97c2dae 100644
--- a/python/gtsam/specializations.h
+++ b/python/gtsam/specializations.h
@@ -13,5 +13,5 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "Bina
 py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
 py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
-py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
-py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
+// py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
+// py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py
index e57fbb6ab..a1adcd2c9 100644
--- a/python/gtsam/tests/test_Triangulation.py
+++ b/python/gtsam/tests/test_Triangulation.py
@@ -56,7 +56,7 @@ class TestVisualISAMExample(GtsamTestCase):
         rank_tol = 1e-9
 
         triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
-        self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9)
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
 
         # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
         measurements = Point2Vector()