From 80a052d048211f9f369ca9602db340508ec58b78 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 16 Jun 2023 10:38:31 -0400
Subject: [PATCH] fix templating TODOs

---
 gtsam/geometry/geometry.i   |  4 ++--
 gtsam/inference/inference.i | 10 +++------
 gtsam/sfm/sfm.i             | 43 +++++++++----------------------------
 timing/timeTest.cpp         |  1 -
 4 files changed, 15 insertions(+), 43 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 60d6a2bed..46172b774 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1181,8 +1181,8 @@ class TriangulationParameters {
                           const gtsam::SharedNoiseModel& noiseModel = nullptr);
 };
 
-// Templates appear not yet supported for free functions - issue raised at
-// borglab/wrap#14 to add support
+// Can be templated but overloaded for convenience.
+// We can now call `triangulatePoint3` with any template type.
 
 // Cal3_S2 versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index d4c90e356..b5413c747 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -176,13 +176,9 @@ class DotWriter {
 class VariableIndex {
   // Standard Constructors and Named Constructors
   VariableIndex();
-  // TODO: Templetize constructor when wrap supports it
-  // template<T = {gtsam::FactorGraph}>
-  // VariableIndex(const T& factorGraph, size_t nVariables);
-  // VariableIndex(const T& factorGraph);
-  VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
-  VariableIndex(const gtsam::GaussianFactorGraph& gfg);
-  VariableIndex(const gtsam::NonlinearFactorGraph& fg);
+  template <T = {gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph,
+                 gtsam::NonlinearFactorGraph}>
+  VariableIndex(const T& factorGraph);
   VariableIndex(const gtsam::VariableIndex& other);
 
   // Testable
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index a9deaa895..930a0dd46 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -126,39 +126,16 @@ class BinaryMeasurementsRot3 {
 #include <gtsam/slam/dataset.h>
 #include <gtsam/sfm/ShonanAveraging.h>
 
-// TODO(frank): copy/pasta below until we have integer template parameters in
-// wrap!
-
-class ShonanAveragingParameters2 {
-  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
-  ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm,
+template <d={2, 3}>
+class ShonanAveragingParameters {
+  ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm);
+  ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm,
                              string method);
   gtsam::LevenbergMarquardtParams getLMParams() const;
   void setOptimalityThreshold(double value);
   double getOptimalityThreshold() const;
-  void setAnchor(size_t index, const gtsam::Rot2& value);
-  pair<size_t, gtsam::Rot2> getAnchor();
-  void setAnchorWeight(double value);
-  double getAnchorWeight() const;
-  void setKarcherWeight(double value);
-  double getKarcherWeight() const;
-  void setGaugesWeight(double value);
-  double getGaugesWeight() const;
-  void setUseHuber(bool value);
-  bool getUseHuber() const;
-  void setCertifyOptimality(bool value);
-  bool getCertifyOptimality() const;
-};
-
-class ShonanAveragingParameters3 {
-  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
-  ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm,
-                             string method);
-  gtsam::LevenbergMarquardtParams getLMParams() const;
-  void setOptimalityThreshold(double value);
-  double getOptimalityThreshold() const;
-  void setAnchor(size_t index, const gtsam::Rot3& value);
-  pair<size_t, gtsam::Rot3> getAnchor();
+  void setAnchor(size_t index, const gtsam::This::Rot& value);
+  pair<size_t, gtsam::This::Rot> getAnchor();
   void setAnchorWeight(double value);
   double getAnchorWeight() const;
   void setKarcherWeight(double value);
@@ -171,6 +148,7 @@ class ShonanAveragingParameters3 {
   bool getCertifyOptimality() const;
 };
 
+// NOTE(Varun): Not templated because each class has specializations defined.
 class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
@@ -217,10 +195,9 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
-  ShonanAveraging3(
-      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
-      const gtsam::ShonanAveragingParameters3& parameters =
-          gtsam::ShonanAveragingParameters3());
+  ShonanAveraging3(const gtsam::This::Measurements& measurements,
+                   const gtsam::ShonanAveragingParameters3& parameters =
+                       gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);
diff --git a/timing/timeTest.cpp b/timing/timeTest.cpp
index 9da3cfb2e..c450b9cf0 100644
--- a/timing/timeTest.cpp
+++ b/timing/timeTest.cpp
@@ -21,7 +21,6 @@ using namespace gtsam;
 
 int main(int argc, char *argv[]) {
 
-  // FIXME: ticPush_ does not exist
   {
     gttic_(top1);
     gttic_(sub1);