added additional non-trivial functions to tests

varunagrawal · varunagrawal · commit 2a81d9260943 · 2025-11-25T02:35:52.000-05:00
diff --git a/tests/expected/matlab/EliminateDiscrete.m b/tests/expected/matlab/EliminateDiscrete.m
@@ -0,0 +1,7 @@
+function varargout = EliminateDiscrete(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'gtsam.DiscreteFactorGraph') && isa(varargin{2},'gtsam.Ordering')
+        [ varargout{1} varargout{2} ] = functions_wrapper(25, varargin{:});
+      else
+        error('Arguments do not match any overload of function EliminateDiscrete');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanPoint2.m b/tests/expected/matlab/FindKarcherMeanPoint2.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanPoint2(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Point2')
+        varargout{1} = functions_wrapper(28, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanPoint2');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanPoint3.m b/tests/expected/matlab/FindKarcherMeanPoint3.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanPoint3(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Point3')
+        varargout{1} = functions_wrapper(28, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanPoint3');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanPose2.m b/tests/expected/matlab/FindKarcherMeanPose2.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanPose2(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Pose2')
+        varargout{1} = functions_wrapper(30, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanPose2');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanPose3.m b/tests/expected/matlab/FindKarcherMeanPose3.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanPose3(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Pose3')
+        varargout{1} = functions_wrapper(31, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanPose3');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanRot2.m b/tests/expected/matlab/FindKarcherMeanRot2.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanRot2(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Rot2')
+        varargout{1} = functions_wrapper(29, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanRot2');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanRot3.m b/tests/expected/matlab/FindKarcherMeanRot3.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanRot3(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::Rot3')
+        varargout{1} = functions_wrapper(31, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanRot3');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanSO3.m b/tests/expected/matlab/FindKarcherMeanSO3.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanSO3(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::SO3')
+        varargout{1} = functions_wrapper(29, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanSO3');
+      end
+end
diff --git a/tests/expected/matlab/FindKarcherMeanSO4.m b/tests/expected/matlab/FindKarcherMeanSO4.m
@@ -0,0 +1,7 @@
+function varargout = FindKarcherMeanSO4(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorgtsam::SO4')
+        varargout{1} = functions_wrapper(30, varargin{:});
+      else
+        error('Arguments do not match any overload of function FindKarcherMeanSO4');
+      end
+end
diff --git a/tests/expected/matlab/MultiTemplatedFunctionDoubleSize_tDouble.m b/tests/expected/matlab/MultiTemplatedFunctionDoubleSize_tDouble.m
@@ -1,5 +1,5 @@
 function varargout = MultiTemplatedFunctionDoubleSize_tDouble(varargin)
-      if length(varargin) == 2 && isa(varargin{1},'T') && isa(varargin{2},'numeric')
+      if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'numeric')
         varargout{1} = functions_wrapper(7, varargin{:});
       else
         error('Arguments do not match any overload of function MultiTemplatedFunctionDoubleSize_tDouble');
diff --git a/tests/expected/matlab/MultiTemplatedFunctionStringSize_tDouble.m b/tests/expected/matlab/MultiTemplatedFunctionStringSize_tDouble.m
@@ -1,5 +1,5 @@
 function varargout = MultiTemplatedFunctionStringSize_tDouble(varargin)
-      if length(varargin) == 2 && isa(varargin{1},'T') && isa(varargin{2},'numeric')
+      if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'numeric')
         varargout{1} = functions_wrapper(6, varargin{:});
       else
         error('Arguments do not match any overload of function MultiTemplatedFunctionStringSize_tDouble');
diff --git a/tests/expected/matlab/TemplatedFunctionRot3.m b/tests/expected/matlab/TemplatedFunctionRot3.m
@@ -1,6 +1,6 @@
 function varargout = TemplatedFunctionRot3(varargin)
       if length(varargin) == 1 && isa(varargin{1},'gtsam.Rot3')
-        functions_wrapper(26, varargin{:});
+        functions_wrapper(32, varargin{:});
       else
         error('Arguments do not match any overload of function TemplatedFunctionRot3');
       end
diff --git a/tests/expected/matlab/functions_wrapper.cpp b/tests/expected/matlab/functions_wrapper.cpp
@@ -108,16 +108,16 @@ void overloadedGlobalFunction_5(int nargout, mxArray *out[], int nargin, const m
 void MultiTemplatedFunctionStringSize_tDouble_6(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("MultiTemplatedFunctionStringSize_tDouble",nargout,nargin,2);
-  T& x = *unwrap_shared_ptr< T >(in[0], "ptr_T");
+  string& x = *unwrap_shared_ptr< string >(in[0], "ptr_string");
   size_t y = unwrap< size_t >(in[1]);
-  out[0] = wrap< double >(MultiTemplatedFunctionStringSize_tDouble(x,y));
+  out[0] = wrap< double >(MultiTemplatedFunction<string,size_t,double>(x,y));
 }
 void MultiTemplatedFunctionDoubleSize_tDouble_7(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("MultiTemplatedFunctionDoubleSize_tDouble",nargout,nargin,2);
-  T& x = *unwrap_shared_ptr< T >(in[0], "ptr_T");
+  double x = unwrap< double >(in[0]);
   size_t y = unwrap< size_t >(in[1]);
-  out[0] = wrap< double >(MultiTemplatedFunctionDoubleSize_tDouble(x,y));
+  out[0] = wrap< double >(MultiTemplatedFunction<double,size_t,double>(x,y));
 }
 void DefaultFuncInt_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
@@ -238,11 +238,56 @@ void EliminateDiscrete_25(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap_shared_ptr(pairResult.first,"gtsam.DiscreteConditional", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.DecisionTreeFactor", false);
 }
-void TemplatedFunctionRot3_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void triangulatePoint3Cal3_S2_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("triangulatePoint3Cal3_S2",nargout,nargin,6);
+  gtsam::Pose3Vector& poses = *unwrap_shared_ptr< gtsam::Pose3Vector >(in[0], "ptr_gtsamPose3Vector");
+  std::shared_ptr<gtsam::Cal3_S2> sharedCal = unwrap_shared_ptr< gtsam::Cal3_S2 >(in[1], "ptr_gtsamCal3_S2");
+  gtsam::Point2Vector& measurements = *unwrap_shared_ptr< gtsam::Point2Vector >(in[2], "ptr_gtsamPoint2Vector");
+  double rank_tol = unwrap< double >(in[3]);
+  bool optimize = unwrap< bool >(in[4]);
+  gtsam::SharedNoiseModel& model = *unwrap_shared_ptr< gtsam::SharedNoiseModel >(in[5], "ptr_gtsamSharedNoiseModel");
+  out[0] = wrap< Point3 >(triangulatePoint3<gtsam::Cal3_S2>(poses,sharedCal,measurements,rank_tol,optimize,model));
+}
+void triangulatePoint3Cal3_S2_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("triangulatePoint3Cal3_S2",nargout,nargin,5);
+  gtsam::Pose3Vector& poses = *unwrap_shared_ptr< gtsam::Pose3Vector >(in[0], "ptr_gtsamPose3Vector");
+  std::shared_ptr<gtsam::Cal3_S2> sharedCal = unwrap_shared_ptr< gtsam::Cal3_S2 >(in[1], "ptr_gtsamCal3_S2");
+  gtsam::Point2Vector& measurements = *unwrap_shared_ptr< gtsam::Point2Vector >(in[2], "ptr_gtsamPoint2Vector");
+  double rank_tol = unwrap< double >(in[3]);
+  bool optimize = unwrap< bool >(in[4]);
+  out[0] = wrap< Point3 >(triangulatePoint3<gtsam::Cal3_S2>(poses,sharedCal,measurements,rank_tol,optimize,nullptr));
+}
+void FindKarcherMeanPoint3_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("FindKarcherMeanPoint3",nargout,nargin,1);
+  std::vector<gtsam::Point3>& elements = *unwrap_shared_ptr< std::vector<gtsam::Point3> >(in[0], "ptr_stdvectorgtsam::Point3");
+  out[0] = wrap< Point3 >(FindKarcherMean<gtsam::Point3>(elements));
+}
+void FindKarcherMeanSO3_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("FindKarcherMeanSO3",nargout,nargin,1);
+  std::vector<gtsam::SO3>& elements = *unwrap_shared_ptr< std::vector<gtsam::SO3> >(in[0], "ptr_stdvectorgtsam::SO3");
+  out[0] = wrap_shared_ptr(std::make_shared<gtsam::SO3>(FindKarcherMean<gtsam::SO3>(elements)),"gtsam.SO3", false);
+}
+void FindKarcherMeanSO4_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("FindKarcherMeanSO4",nargout,nargin,1);
+  std::vector<gtsam::SO4>& elements = *unwrap_shared_ptr< std::vector<gtsam::SO4> >(in[0], "ptr_stdvectorgtsam::SO4");
+  out[0] = wrap_shared_ptr(std::make_shared<gtsam::SO4>(FindKarcherMean<gtsam::SO4>(elements)),"gtsam.SO4", false);
+}
+void FindKarcherMeanPose3_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("FindKarcherMeanPose3",nargout,nargin,1);
+  std::vector<gtsam::Pose3>& elements = *unwrap_shared_ptr< std::vector<gtsam::Pose3> >(in[0], "ptr_stdvectorgtsam::Pose3");
+  out[0] = wrap_shared_ptr(std::make_shared<gtsam::Pose3>(FindKarcherMean<gtsam::Pose3>(elements)),"gtsam.Pose3", false);
+}
+void TemplatedFunctionRot3_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("TemplatedFunctionRot3",nargout,nargin,1);
   gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3");
-  TemplatedFunctionRot3(t);
+  TemplatedFunction<gtsam::Rot3>(t);
 }
 
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -335,7 +380,25 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       EliminateDiscrete_25(nargout, out, nargin-1, in+1);
       break;
     case 26:
-      TemplatedFunctionRot3_26(nargout, out, nargin-1, in+1);
+      triangulatePoint3Cal3_S2_26(nargout, out, nargin-1, in+1);
+      break;
+    case 27:
+      triangulatePoint3Cal3_S2_27(nargout, out, nargin-1, in+1);
+      break;
+    case 28:
+      FindKarcherMeanPoint3_28(nargout, out, nargin-1, in+1);
+      break;
+    case 29:
+      FindKarcherMeanSO3_29(nargout, out, nargin-1, in+1);
+      break;
+    case 30:
+      FindKarcherMeanSO4_30(nargout, out, nargin-1, in+1);
+      break;
+    case 31:
+      FindKarcherMeanPose3_31(nargout, out, nargin-1, in+1);
+      break;
+    case 32:
+      TemplatedFunctionRot3_32(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {
diff --git a/tests/expected/matlab/triangulatePoint3Cal3_S2.m b/tests/expected/matlab/triangulatePoint3Cal3_S2.m
@@ -0,0 +1,9 @@
+function varargout = triangulatePoint3Cal3_S2(varargin)
+      if length(varargin) == 6 && isa(varargin{1},'gtsam.Pose3Vector') && isa(varargin{2},'gtsam.Cal3_S2') && isa(varargin{3},'gtsam.Point2Vector') && isa(varargin{4},'double') && isa(varargin{5},'logical') && isa(varargin{6},'gtsam.SharedNoiseModel')
+        varargout{1} = functions_wrapper(26, varargin{:});
+      elseif length(varargin) == 5 && isa(varargin{1},'gtsam.Pose3Vector') && isa(varargin{2},'gtsam.Cal3_S2') && isa(varargin{3},'gtsam.Point2Vector') && isa(varargin{4},'double') && isa(varargin{5},'logical')
+        varargout{1} = functions_wrapper(27, varargin{:});
+      else
+        error('Arguments do not match any overload of function triangulatePoint3Cal3_S2');
+      end
+end
diff --git a/tests/expected/python/functions_pybind.cpp b/tests/expected/python/functions_pybind.cpp
@@ -22,15 +22,20 @@ PYBIND11_MODULE(functions_py, m_) {
     m_.def("aGlobalFunction",[](){return ::aGlobalFunction();});
     m_.def("overloadedGlobalFunction",[](int a){return ::overloadedGlobalFunction(a);}, py::arg("a"));
     m_.def("overloadedGlobalFunction",[](int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b"));
-    m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<string,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
-    m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<double,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
+    m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const string& x, size_t y){return ::MultiTemplatedFunction<string,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
+    m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const double& x, size_t y){return ::MultiTemplatedFunction<double,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
     m_.def("DefaultFuncInt",[](int a, int b){ ::DefaultFuncInt(a, b);}, py::arg("a") = 123, py::arg("b") = 0);
     m_.def("DefaultFuncString",[](const string& s, const string& name){ ::DefaultFuncString(s, name);}, py::arg("s") = "hello", py::arg("name") = "");
     m_.def("DefaultFuncObj",[](const gtsam::KeyFormatter& keyFormatter){ ::DefaultFuncObj(keyFormatter);}, py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);
     m_.def("DefaultFuncZero",[](int a, int b, double c, int d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a"), py::arg("b"), py::arg("c") = 0.0, py::arg("d") = 0, py::arg("e") = false);
     m_.def("DefaultFuncVector",[](const std::vector<int>& i, const std::vector<string>& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"});
     m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3());
     m_.def("EliminateDiscrete",[](const gtsam::DiscreteFactorGraph& factors, const gtsam::Ordering& frontalKeys){return ::EliminateDiscrete(factors, frontalKeys);}, py::arg("factors"), py::arg("frontalKeys"));
+    m_.def("triangulatePoint3Cal3_S2",[](const gtsam::Pose3Vector& poses, std::shared_ptr<gtsam::Cal3_S2> sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model){return ::triangulatePoint3<gtsam::Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize, model);}, py::arg("poses"), py::arg("sharedCal"), py::arg("measurements"), py::arg("rank_tol"), py::arg("optimize"), py::arg("model") = nullptr);
+    m_.def("FindKarcherMeanPoint3",[](const std::vector<gtsam::Point3>& elements){return ::FindKarcherMean<gtsam::Point3>(elements);}, py::arg("elements"));
+    m_.def("FindKarcherMeanSO3",[](const std::vector<gtsam::SO3>& elements){return ::FindKarcherMean<gtsam::SO3>(elements);}, py::arg("elements"));
+    m_.def("FindKarcherMeanSO4",[](const std::vector<gtsam::SO4>& elements){return ::FindKarcherMean<gtsam::SO4>(elements);}, py::arg("elements"));
+    m_.def("FindKarcherMeanPose3",[](const std::vector<gtsam::Pose3>& elements){return ::FindKarcherMean<gtsam::Pose3>(elements);}, py::arg("elements"));
     m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction<gtsam::Rot3>(t);}, py::arg("t"));
 
 #include "python/specializations.h"
diff --git a/tests/fixtures/functions.i b/tests/fixtures/functions.i
@@ -19,12 +19,11 @@ gtsam::Vector overloadedGlobalFunction(int a, double b);
 
 // A templated free/global function. Multiple templates supported.
 template<T1 = {string, double}, T2 = {size_t}, R = {double}>
-R MultiTemplatedFunction(const T& x, T2 y);
+R MultiTemplatedFunction(const T1& x, T2 y);
 
 // Check if we can typedef the templated function
 template<T>
 void TemplatedFunction(const T& t);
-
 typedef TemplatedFunction<gtsam::Rot3> TemplatedFunctionRot3;
 
 // Check default arguments
@@ -40,3 +39,14 @@ void setPose(const gtsam::Pose3& pose = gtsam::Pose3());
 std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
 EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
                   const gtsam::Ordering& frontalKeys);
+
+// Example of function where template is a shared pointer
+template <CAL = {gtsam::Cal3_S2}>
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, CAL* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+
+// Function which takes vector<T> as an argument
+template <T = {gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Pose3}>
+T FindKarcherMean(const std::vector<T>& elements);
diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py
@@ -105,6 +105,12 @@ def test_functions(self):
             'DefaultFuncVector.m',
             'DefaultFuncZero.m',
             'setPose.m',
+            'EliminateDiscrete.m',
+            'triangulatePoint3Cal3_S2.m',
+            'FindKarcherMeanPoint3.m',
+            'FindKarcherMeanSO3.m',
+            'FindKarcherMeanSO4.m',
+            'FindKarcherMeanPose3.m',
         ]
 
         for file in files:
