Merge branch 'develop' into feature/discrete_wrapper

.github/workflows/build-windows.yml

@@ -26,7 +26,11 @@ jobs:
             windows-2019-cl,
           ]
 
-        build_type: [Debug, Release]
+        build_type: [
+            Debug,
+            #TODO(Varun) The release build takes over 2.5 hours, need to figure out why.
+            # Release
+          ]
         build_unstable: [ON]
         include:
           #TODO This build fails, need to understand why.
@@ -90,13 +94,18 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
 
-      - name: Build
+      - name: Configuration
         run: |
           cmake -E remove_directory build
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
-          cmake --build build --config ${{ matrix.build_type }} --target gtsam
+
-          cmake --build build --config ${{ matrix.build_type }} --target gtsam_unstable
+      - name: Build
-          cmake --build build --config ${{ matrix.build_type }} --target wrap
+        run: |
-          cmake --build build --config ${{ matrix.build_type }} --target check.base
+          # Since Visual Studio is a multi-generator, we need to use --config
-          cmake --build build --config ${{ matrix.build_type }} --target check.base_unstable
+          # https://stackoverflow.com/a/24470998/1236990
-          cmake --build build --config ${{ matrix.build_type }} --target check.linear
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear

CMakeLists.txt

@@ -87,6 +87,13 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
         CACHE STRING "The Python version to use for wrapping")
     # Set the include directory for matlab.h
     set(GTWRAP_INCLUDE_NAME "wrap")
+
+    # Copy matlab.h to the correct folder.
+    configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
+               ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
+    # Add the include directories so that matlab.h can be found
+    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
+
     add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
 endif()

gtsam/base/base.i

@@ -38,7 +38,7 @@ class DSFMap {
   DSFMap();
   KEY find(const KEY& key) const;
   void merge(const KEY& x, const KEY& y);
-  std::map<KEY, Set> sets();
+  std::map<KEY, This::Set> sets();
 };
 
 class IndexPairSet {

gtsam/basis/basis.i

@@ -140,7 +140,7 @@ class FitBasis {
   static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
       const std::map<double, double>& sequence,
       const gtsam::noiseModel::Base* model, size_t N);
-  Parameters parameters() const;
+  This::Parameters parameters() const;
 };
 
 }  // namespace gtsam

gtsam/gtsam.i

@@ -165,6 +165,7 @@ gtsam::Values allPose2s(gtsam::Values& values);
 Matrix extractPose2(const gtsam::Values& values);
 gtsam::Values allPose3s(gtsam::Values& values);
 Matrix extractPose3(const gtsam::Values& values);
+Matrix extractVectors(const gtsam::Values& values, char c);
 void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u);
 void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR,
                   int seed = 42u);

gtsam/nonlinear/NonlinearFactor.h

@@ -282,7 +282,7 @@ public:
  * which are objects in non-linear manifolds (Lie groups).
  */
 template<class VALUE>
-class NoiseModelFactor1: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor1: public NoiseModelFactor {
 
 public:
 
@@ -366,7 +366,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 2
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2>
-class NoiseModelFactor2: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor2: public NoiseModelFactor {
 
 public:
 
@@ -441,7 +441,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 3
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3>
-class NoiseModelFactor3: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor3: public NoiseModelFactor {
 
 public:
 
@@ -518,7 +518,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 4
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
-class NoiseModelFactor4: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor4: public NoiseModelFactor {
 
 public:
 
@@ -599,7 +599,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 5
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
-class NoiseModelFactor5: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor5: public NoiseModelFactor {
 
 public:
 
@@ -684,7 +684,7 @@ private:
 /** A convenient base class for creating your own NoiseModelFactor with 6
  * variables.  To derive from this class, implement evaluateError(). */
 template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
-class NoiseModelFactor6: public NoiseModelFactor {
+class GTSAM_EXPORT NoiseModelFactor6: public NoiseModelFactor {
 
 public:
 

gtsam/nonlinear/tests/testUtilities.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/utilities.h>
 
 using namespace gtsam;
@@ -55,6 +54,26 @@ TEST(Utilities, ExtractPoint3) {
   EXPECT_LONGS_EQUAL(2, all_points.rows());
 }
 
+/* ************************************************************************* */
+TEST(Utilities, ExtractVector) {
+  // Test normal case with 3 vectors and 1 non-vector (ignore non-vector)
+  auto values = Values();
+  values.insert(X(0), (Vector(4) << 1, 2, 3, 4).finished());
+  values.insert(X(2), (Vector(4) << 13, 14, 15, 16).finished());
+  values.insert(X(1), (Vector(4) << 6, 7, 8, 9).finished());
+  values.insert(X(3), Pose3());
+  auto actual = utilities::extractVectors(values, 'x');
+  auto expected =
+      (Matrix(3, 4) << 1, 2, 3, 4, 6, 7, 8, 9, 13, 14, 15, 16).finished();
+  EXPECT(assert_equal(expected, actual));
+
+  // Check that mis-sized vectors fail
+  values.insert(X(4), (Vector(2) << 1, 2).finished());
+  THROWS_EXCEPTION(utilities::extractVectors(values, 'x'));
+  values.update(X(4), (Vector(6) << 1, 2, 3, 4, 5, 6).finished());
+  THROWS_EXCEPTION(utilities::extractVectors(values, 'x'));
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(nullptr));

gtsam/nonlinear/utilities.h

@@ -20,6 +20,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/linear/Sampler.h>
+#include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/Values.h>
@@ -162,6 +163,34 @@ Matrix extractPose3(const Values& values) {
   return result;
 }
 
+/// Extract all Vector values with a given symbol character into an mxn matrix,
+/// where m is the number of symbols that match the character and n is the
+/// dimension of the variables.  If not all variables have dimension n, then a
+/// runtime error will be thrown.  The order of returned values are sorted by
+/// the symbol.
+/// For example, calling extractVector(values, 'x'), where values contains 200
+/// variables x1, x2, ..., x200 of type Vector each 5-dimensional, will return a
+/// 200x5 matrix with row i containing xi.
+Matrix extractVectors(const Values& values, char c) {
+  Values::ConstFiltered<Vector> vectors =
+      values.filter<Vector>(Symbol::ChrTest(c));
+  if (vectors.size() == 0) {
+    return Matrix();
+  }
+  auto dim = vectors.begin()->value.size();
+  Matrix result(vectors.size(), dim);
+  Eigen::Index rowi = 0;
+  for (const auto& kv : vectors) {
+    if (kv.value.size() != dim) {
+      throw std::runtime_error(
+          "Tried to extract different-sized vectors into a single matrix");
+    }
+    result.row(rowi) = kv.value;
+    ++rowi;
+  }
+  return result;
+}
+
 /// Perturb all Point2 values using normally distributed noise
 void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
   noiseModel::Isotropic::shared_ptr model =

gtsam/sfm/MFAS.h

@@ -48,7 +48,7 @@ namespace gtsam {
   unit translations in a projection direction.
   @addtogroup SFM
 */
-class MFAS {
+class GTSAM_EXPORT MFAS {
  public:
   // used to represent edges between two nodes in the graph. When used in
   // translation averaging for global SfM

gtsam/slam/EssentialMatrixFactor.h

@@ -1,7 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2014, 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
- * @file EssentialMatrixFactor.cpp
+ * @file EssentialMatrixFactor.h
  * @brief EssentialMatrixFactor class
  * @author Frank Dellaert
+ * @author Ayush Baid
+ * @author Akshay Krishnan
  * @date December 17, 2013
  */
 

gtsam/slam/SmartFactorBase.h

@@ -47,7 +47,7 @@ namespace gtsam {
  * @tparam CAMERA should behave like a PinholeCamera.
  */
 template<class CAMERA>
-class SmartFactorBase: public NonlinearFactor {
+class GTSAM_EXPORT SmartFactorBase: public NonlinearFactor {
 
 private:
   typedef NonlinearFactor Base;

gtsam/slam/SmartProjectionPoseFactor.h

@@ -41,11 +41,10 @@ namespace gtsam {
  * If the calibration should be optimized, as well, use SmartProjectionFactor instead!
  * @addtogroup SLAM
  */
-template<class CALIBRATION>
+template <class CALIBRATION>
-class SmartProjectionPoseFactor: public SmartProjectionFactor<
+class GTSAM_EXPORT SmartProjectionPoseFactor
-    PinholePose<CALIBRATION> > {
+    : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
-
+ private:
-private:
   typedef PinholePose<CALIBRATION> Camera;
   typedef SmartProjectionFactor<Camera> Base;
   typedef SmartProjectionPoseFactor<CALIBRATION> This;
@@ -156,7 +155,6 @@ public:
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
-
 };
 // end of class declaration
 

gtsam/slam/slam.i

@@ -168,7 +168,7 @@ template <POSE>
 virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
   PoseTranslationPrior(size_t key, const POSE& pose_z,
                        const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Translation measured() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -185,7 +185,7 @@ template <POSE>
 virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
   PoseRotationPrior(size_t key, const POSE& pose_z,
                     const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Rotation measured() const;
 };
 
 typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
@@ -196,6 +196,21 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
   EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
                         const gtsam::Point2& pB,
                         const gtsam::noiseModel::Base* noiseModel);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
+  Vector evaluateError(const gtsam::EssentialMatrix& E) const;
+};
+
+#include <gtsam/slam/EssentialMatrixConstraint.h>
+virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
+  EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,
+                            const gtsam::noiseModel::Base *model);
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const;
+  Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const;
+  const gtsam::EssentialMatrix& measured() const;
 };
 
 #include <gtsam/slam/dataset.h>

gtsam/slam/tests/testEssentialMatrixConstraint.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  testEssentialMatrixConstraint.cpp
+ *  @file  TestEssentialMatrixConstraint.cpp
  *  @brief Unit tests for EssentialMatrixConstraint Class
  *  @author Frank Dellaert
  *  @author Pablo Alcantarilla

gtsam_unstable/slam/LocalOrientedPlane3Factor.h

@@ -9,6 +9,8 @@
 
 #include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam_unstable/dllexport.h>
+
 #include <string>
 
 namespace gtsam {
@@ -32,9 +34,9 @@ namespace gtsam {
  * a local linearisation point for the plane. The plane is representated and
  * optimized in x1 frame in the optimization.
  */
-class LocalOrientedPlane3Factor: public NoiseModelFactor3<Pose3, Pose3,
+class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
-                                                          OrientedPlane3> {
+    : public NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> {
-protected:
+ protected:
   OrientedPlane3 measured_p_;
   typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base;
 public:

gtsam_unstable/slam/PoseToPointFactor.h

@@ -1,11 +1,14 @@
 /**
  *  @file   PoseToPointFactor.hpp
- *  @brief  This factor can be used to track a 3D landmark over time by
+ *  @brief  This factor can be used to model relative position measurements
- *providing local measurements of its location.
+ *  from a (2D or 3D) pose to a landmark
  *  @author David Wisth
+ *  @author Luca Carlone
  **/
 #pragma once
 
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -17,12 +20,13 @@ namespace gtsam {
  * A class for a measurement between a pose and a point.
  * @addtogroup SLAM
  */
-class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
+template<typename POSE = Pose3, typename POINT = Point3>
+class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
  private:
   typedef PoseToPointFactor This;
-  typedef NoiseModelFactor2<Pose3, Point3> Base;
+  typedef NoiseModelFactor2<POSE, POINT> Base;
 
-  Point3 measured_; /** the point measurement in local coordinates */
+  POINT measured_; /** the point measurement in local coordinates */
 
  public:
   // shorthand for a smart pointer to a factor
@@ -32,7 +36,7 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
   PoseToPointFactor() {}
 
   /** Constructor */
-  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
+  PoseToPointFactor(Key key1, Key key2, const POINT& measured,
                     const SharedNoiseModel& model)
       : Base(model, key1, key2), measured_(measured) {}
 
@@ -41,8 +45,8 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-                                               DefaultKeyFormatter) const {
+                                       DefaultKeyFormatter) const override {
     std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
               << keyFormatter(this->key2()) << ")\n"
               << "  measured: " << measured_.transpose() << std::endl;
@@ -50,30 +54,31 @@ class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
   }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected,
+  bool equals(const NonlinearFactor& expected,
-                      double tol = 1e-9) const {
+              double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&expected);
     return e != nullptr && Base::equals(*e, tol) &&
-           traits<Point3>::Equals(this->measured_, e->measured_, tol);
+           traits<POINT>::Equals(this->measured_, e->measured_, tol);
   }
 
   /** implement functions needed to derive from Factor */
 
   /** vector of errors
-   * @brief Error = wTwi.inverse()*wPwp - measured_
+   * @brief Error = w_T_b.inverse()*w_P - measured_
-   * @param wTwi The pose of the sensor in world coordinates
+   * @param w_T_b The pose of the body in world coordinates
-   * @param wPwp The estimated point location in world coordinates
+   * @param w_P The estimated point location in world coordinates
    *
    * Note: measured_ and the error are in local coordiantes.
    */
-  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
+  Vector evaluateError(
-                       boost::optional<Matrix&> H1 = boost::none,
+      const POSE& w_T_b, const POINT& w_P,
-                       boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H1 = boost::none,
-    return wTwi.transformTo(wPwp, H1, H2) - measured_;
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    return w_T_b.transformTo(w_P, H1, H2) - measured_;
   }
 
   /** return the measured */
-  const Point3& measured() const { return measured_; }
+  const POINT& measured() const { return measured_; }
 
  private:
   /** Serialization function */

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -18,9 +18,11 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam_unstable/dllexport.h>
+
 #include <boost/optional.hpp>
 
 namespace gtsam {
@@ -30,28 +32,27 @@ namespace gtsam {
    * estimates the body pose, body-camera transform, 3D landmark, and calibration.
    * @addtogroup SLAM
    */
-  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
+template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
-  class ProjectionFactorPPPC: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
+class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
-  protected:
+    : public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
+ protected:
+  Point2 measured_;  ///< 2D measurement
 
-    Point2 measured_;                    ///< 2D measurement
+  // verbosity handling for Cheirality Exceptions
+  bool throwCheirality_;  ///< If true, rethrows Cheirality exceptions (default: false)
+  bool verboseCheirality_;  ///< If true, prints text for Cheirality exceptions (default: false)
 
-    // verbosity handling for Cheirality Exceptions
+ public:
-    bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+  /// shorthand for base class type
-    bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+  typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
 
-  public:
+  /// shorthand for this class
+  typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
 
-    /// shorthand for base class type
+  /// shorthand for a smart pointer to a factor
-    typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
+  typedef boost::shared_ptr<This> shared_ptr;
 
-    /// shorthand for this class
+  /// Default constructor
-    typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
-
-    /// shorthand for a smart pointer to a factor
-    typedef boost::shared_ptr<This> shared_ptr;
-
-    /// Default constructor
   ProjectionFactorPPPC() :
       measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
   }
@@ -168,7 +169,7 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
       ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
     }
-  };
+};
 
   /// traits
   template<class POSE, class LANDMARK, class CALIBRATION>

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -21,6 +21,7 @@
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam_unstable/dllexport.h>
 
 #include <boost/optional.hpp>
 
@@ -40,7 +41,7 @@ namespace gtsam {
  * @addtogroup SLAM
  */
 
-class ProjectionFactorRollingShutter
+class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
     : public NoiseModelFactor3<Pose3, Pose3, Point3> {
  protected:
   // Keep a copy of measurement and calibration for I/O

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam_unstable/dllexport.h>
 
 namespace gtsam {
 /**
@@ -41,7 +42,7 @@ namespace gtsam {
  * @addtogroup SLAM
  */
 template <class CAMERA>
-class SmartProjectionPoseFactorRollingShutter
+class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter
     : public SmartProjectionFactor<CAMERA> {
  private:
   typedef SmartProjectionFactor<CAMERA> Base;

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -20,18 +20,18 @@
 
 #pragma once
 
-#include <gtsam/slam/SmartFactorBase.h>
-#include <gtsam/slam/SmartFactorParams.h>
-
-#include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoCamera.h>
-#include <gtsam/slam/StereoFactor.h>
+#include <gtsam/geometry/triangulation.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/SmartFactorBase.h>
+#include <gtsam/slam/SmartFactorParams.h>
+#include <gtsam/slam/StereoFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam_unstable/dllexport.h>
 
-#include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/optional.hpp>
 #include <vector>
 
 namespace gtsam {
@@ -49,8 +49,9 @@ typedef SmartProjectionParams SmartStereoProjectionParams;
  * If you'd like to store poses in values instead of cameras, use
  * SmartStereoProjectionPoseFactor instead
 */
-class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
+class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactor
-private:
+    : public SmartFactorBase<StereoCamera> {
+ private:
 
   typedef SmartFactorBase<StereoCamera> Base;
 

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -40,7 +40,8 @@ namespace gtsam {
  * are Pose3 variables).
  * @addtogroup SLAM
  */
-class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
+class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
+    : public SmartStereoProjectionFactor {
  protected:
   /// shared pointer to calibration object (one for each camera)
   std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
@@ -294,7 +295,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     ar & BOOST_SERIALIZATION_NVP(K_all_);
   }
-
 };
 // end of class declaration
 

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -43,7 +43,8 @@ namespace gtsam {
  * This factor requires that values contains the involved poses (Pose3).
  * @addtogroup SLAM
  */
-class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor {
+class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
+    : public SmartStereoProjectionFactor {
  protected:
   /// shared pointer to calibration object (one for each camera)
   std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;

gtsam_unstable/slam/tests/testPoseToPointFactor.cpp

@@ -0,0 +1,161 @@
+/**
+ * @file   testPoseToPointFactor.cpp
+ * @brief
+ * @author David Wisth
+ * @author Luca Carlone
+ * @date   June 20, 2020
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam_unstable/slam/PoseToPointFactor.h>
+
+using namespace gtsam;
+using namespace gtsam::noiseModel;
+
+/* ************************************************************************* */
+// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless_2D) {
+  Pose2 pose = Pose2::identity();
+  Point2 point(1.0, 2.0);
+  Point2 noise(0.0, 0.0);
+  Point2 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(2, 0.05));
+  Vector expectedError = Vector2(0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise_2D) {
+  Pose2 pose = Pose2::identity();
+  Point2 point(1.0, 2.0);
+  Point2 noise(-1.0, 0.5);
+  Point2 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(2, 0.05));
+  Vector expectedError = -noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian_2D) {
+  // Measurement
+  gtsam::Point2 l_meas(1, 2);
+
+  // Linearisation point
+  gtsam::Point2 p_t(-5, 12);
+  gtsam::Rot2 p_R(1.5 * M_PI);
+  Pose2 p(p_R, p_t);
+
+  gtsam::Point2 l(3, 0);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
+  PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = std::bind(&PoseToPointFactor<Pose2,Point2>::evaluateError, factor,
+                     std::placeholders::_1, std::placeholders::_2, boost::none,
+                     boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+// Verify zero error when there is no noise
+TEST(PoseToPointFactor, errorNoiseless_3D) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(0.0, 0.0, 0.0);
+  Point3 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = Vector3(0.0, 0.0, 0.0);
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Verify expected error in test scenario
+TEST(PoseToPointFactor, errorNoise_3D) {
+  Pose3 pose = Pose3::identity();
+  Point3 point(1.0, 2.0, 3.0);
+  Point3 noise(-1.0, 0.5, 0.3);
+  Point3 measured = point + noise;
+
+  Key pose_key(1);
+  Key point_key(2);
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
+                           Isotropic::Sigma(3, 0.05));
+  Vector expectedError = -noise;
+  Vector actualError = factor.evaluateError(pose, point);
+  EXPECT(assert_equal(expectedError, actualError, 1E-5));
+}
+
+/* ************************************************************************* */
+// Check Jacobians are correct
+TEST(PoseToPointFactor, jacobian_3D) {
+  // Measurement
+  gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
+
+  // Linearisation point
+  gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
+  gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
+  Pose3 p(p_R, p_t);
+
+  gtsam::Point3 l = gtsam::Point3(3, 0, 5);
+
+  // Factor
+  Key pose_key(1);
+  Key point_key(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise);
+
+  // Calculate numerical derivatives
+  auto f = std::bind(&PoseToPointFactor<Pose3,Point3>::evaluateError, factor,
+                     std::placeholders::_1, std::placeholders::_2, boost::none,
+                     boost::none);
+  Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
+  Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
+
+  // Use the factor to calculate the derivative
+  Matrix actual_H1;
+  Matrix actual_H2;
+  factor.evaluateError(p, l, actual_H1, actual_H2);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
+  EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam_unstable/slam/tests/testPoseToPointFactor.h

@@ -1,86 +0,0 @@
-/**
- * @file   testPoseToPointFactor.cpp
- * @brief
- * @author David Wisth
- * @date   June 20, 2020
- */
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam_unstable/slam/PoseToPointFactor.h>
-
-using namespace gtsam;
-using namespace gtsam::noiseModel;
-
-/// Verify zero error when there is no noise
-TEST(PoseToPointFactor, errorNoiseless) {
-  Pose3 pose = Pose3::identity();
-  Point3 point(1.0, 2.0, 3.0);
-  Point3 noise(0.0, 0.0, 0.0);
-  Point3 measured = t + noise;
-
-  Key pose_key(1);
-  Key point_key(2);
-  PoseToPointFactor factor(pose_key, point_key, measured,
-                           Isotropic::Sigma(3, 0.05));
-  Vector expectedError = Vector3(0.0, 0.0, 0.0);
-  Vector actualError = factor.evaluateError(pose, point);
-  EXPECT(assert_equal(expectedError, actualError, 1E-5));
-}
-
-/// Verify expected error in test scenario
-TEST(PoseToPointFactor, errorNoise) {
-  Pose3 pose = Pose3::identity();
-  Point3 point(1.0, 2.0, 3.0);
-  Point3 noise(-1.0, 0.5, 0.3);
-  Point3 measured = t + noise;
-
-  Key pose_key(1);
-  Key point_key(2);
-  PoseToPointFactor factor(pose_key, point_key, measured,
-                           Isotropic::Sigma(3, 0.05));
-  Vector expectedError = noise;
-  Vector actualError = factor.evaluateError(pose, point);
-  EXPECT(assert_equal(expectedError, actualError, 1E-5));
-}
-
-/// Check Jacobians are correct
-TEST(PoseToPointFactor, jacobian) {
-  // Measurement
-  gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
-
-  // Linearisation point
-  gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
-  gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI);
-  Pose3 p(p_R, p_t);
-
-  gtsam::Point3 l = gtsam::Point3(3, 0, 5);
-
-  // Factor
-  Key pose_key(1);
-  Key point_key(2);
-  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
-  PoseToPointFactor factor(pose_key, point_key, l_meas, noise);
-
-  // Calculate numerical derivatives
-  auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2,
-                       boost::none, boost::none);
-  Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(f, p, l);
-  Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(f, p, l);
-
-  // Use the factor to calculate the derivative
-  Matrix actual_H1;
-  Matrix actual_H2;
-  factor.evaluateError(p, l, actual_H1, actual_H2);
-
-  // Verify we get the expected error
-  EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8));
-  EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8));
-}
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */

matlab/+gtsam/VisualISAMInitialize.m

@@ -12,11 +12,11 @@ end
 isam = ISAM2(params);
 
 %% Set Noise parameters
-noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]', true);
 %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]');
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]', true);
-noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1);
+noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1, true);
-noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
+noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0, true);
 
 %% Add constraints/priors
 % TODO: should not be from ground truth!

matlab/CMakeLists.txt

@@ -64,8 +64,21 @@ set(ignore
     gtsam::Point3
     gtsam::CustomFactor)
 
+set(interface_files
+    ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/base/base.i
+    ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
+    ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
+    ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sfm/sfm.i
+    ${GTSAM_SOURCE_DIR}/gtsam/navigation/navigation.i
+)
 # Wrap
-matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"
+matlab_wrap("${interface_files}" "gtsam" "${GTSAM_ADDITIONAL_LIBRARIES}"
             "" "${mexFlags}" "${ignore}")
 
 # Wrap version for gtsam_unstable

matlab/gtsam_tests/testJacobianFactor.m

@@ -32,7 +32,7 @@ x1 = 3;
 % the RHS
 b2=[-1;1.5;2;-1];
 sigmas = [1;1;1;1];
-model4 = noiseModel.Diagonal.Sigmas(sigmas);
+model4 = noiseModel.Diagonal.Sigmas(sigmas, true);
 combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
 
 % eliminate the first variable (x2) in the combined factor, destructive !
@@ -74,7 +74,7 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];
 
-model2 = noiseModel.Diagonal.Sigmas([1;1]);
+model2 = noiseModel.Diagonal.Sigmas([1;1], true);
 expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
 
 % check if the result matches the combined (reduced) factor

matlab/gtsam_tests/testKalmanFilter.m

@@ -23,13 +23,13 @@ import gtsam.*
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 Q = 0.01*eye(2,2);
 H = eye(2,2);
 z1 = [1.0, 0.0]';
 z2 = [2.0, 0.0]';
 z3 = [3.0, 0.0]';
-modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelR = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 R = 0.01*eye(2,2);
 
 %% Create the set of expected output TestValues

matlab/gtsam_tests/testLocalizationExample.m

@@ -17,7 +17,7 @@ graph = NonlinearFactorGraph;
 
 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 
@@ -27,7 +27,7 @@ groundTruth = Values;
 groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
 groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
 groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
-model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
+model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10], true);
 for i=1:3
     graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
 end

matlab/gtsam_tests/testOdometryExample.m

@@ -17,12 +17,12 @@ graph = NonlinearFactorGraph;
 
 %% Add a Gaussian prior on pose x_1
 priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); % 30cm std on x,y, 0.1 rad on theta
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -30,18 +30,18 @@ graph = NonlinearFactorGraph;
 
 %% Add prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 %% Add bearing/range measurement factors
 degrees = pi/180;
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -26,19 +26,19 @@ graph = NonlinearFactorGraph;
 %% Add prior
 % gaussian for prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
 
 %% Add pose constraint
-model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 %% Initialize to noisy points

matlab/gtsam_tests/testPose3SLAMExample.m

@@ -21,7 +21,7 @@ p1 = hexagon.atPose3(1);
 fg = NonlinearFactorGraph;
 fg.add(NonlinearEqualityPose3(0, p0));
 delta = p0.between(p1);
-covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180], true);
 fg.add(BetweenFactorPose3(0,1, delta, covariance));
 fg.add(BetweenFactorPose3(1,2, delta, covariance));
 fg.add(BetweenFactorPose3(2,3, delta, covariance));

matlab/gtsam_tests/testSFMExample.m

@@ -25,7 +25,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 graph = NonlinearFactorGraph;
 
 %% Add factors for all measurements
-measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
@@ -33,9 +33,9 @@ for i=1:length(data.Z)
     end
 end
 
-posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true);
 graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
-pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true);
 graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
 
 %% Initial estimate

matlab/gtsam_tests/testSerialization.m

@@ -45,30 +45,30 @@ graph = NonlinearFactorGraph;
 
 % Prior factor
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
 % Between Factors
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
  
 % Range Factors
-rNoise = noiseModel.Diagonal.Sigmas([0.2]);
+rNoise = noiseModel.Diagonal.Sigmas([0.2], true);
 graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
 graph.add(RangeFactor2D(i2, j1, 2, rNoise));
 graph.add(RangeFactor2D(i3, j2, 2, rNoise));
 
 % Bearing Factors
 degrees = pi/180;
-bNoise = noiseModel.Diagonal.Sigmas([0.1]);
+bNoise = noiseModel.Diagonal.Sigmas([0.1], true);
 graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
 graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
 graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
 
 % BearingRange Factors
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

matlab/gtsam_tests/testStereoVOExample.m

@@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
 K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0], true);
 
 %% Add measurements
 % pose 1

matlab/gtsam_tests/testUtilities.m

@@ -45,3 +45,12 @@ CHECK('KeySet', isa(actual,'gtsam.KeySet'));
 CHECK('size==3', actual.size==3);
 CHECK('actual.count(x1)', actual.count(x1));
 
+% test extractVectors
+values = Values();
+values.insert(symbol('x', 0), (1:6)');
+values.insert(symbol('x', 1), (7:12)');
+values.insert(symbol('x', 2), (13:18)');
+values.insert(symbol('x', 7), Pose3());
+actual = utilities.extractVectors(values, 'x');
+expected = reshape(1:18, 6, 3)';
+CHECK('extractVectors', all(actual == expected, 'all'));

python/gtsam/tests/testEssentialMatrixConstraint.py

@@ -0,0 +1,47 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+visual_isam unit tests.
+Author: Frank Dellaert & Pablo Alcantarilla
+"""
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import (EssentialMatrix, EssentialMatrixConstraint, Point3, Pose3,
+                   Rot3, Unit3, symbol)
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestVisualISAMExample(GtsamTestCase):
+    def test_VisualISAMExample(self):
+
+        # Create a factor
+        poseKey1 = symbol('x', 1)
+        poseKey2 = symbol('x', 2)
+        trueRotation = Rot3.RzRyRx(0.15, 0.15, -0.20)
+        trueTranslation = Point3(+0.5, -1.0, +1.0)
+        trueDirection = Unit3(trueTranslation)
+        E = EssentialMatrix(trueRotation, trueDirection)
+        model = gtsam.noiseModel.Isotropic.Sigma(5, 0.25)
+        factor = EssentialMatrixConstraint(poseKey1, poseKey2, E, model)
+
+        #  Create a linearization point at the zero-error point
+        pose1 = Pose3(Rot3.RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0))
+        pose2 = Pose3(
+            Rot3.RzRyRx(0.179693265735950, 0.002945368776519,
+                        0.102274823253840),
+            Point3(-3.37493895, 6.14660244, -8.93650986))
+
+        expected = np.zeros((5, 1))
+        actual = factor.evaluateError(pose1, pose2)
+        self.gtsamAssertEquals(actual, expected, 1e-8)
+
+
+if __name__ == "__main__":
+    unittest.main()

wrap/cmake/MatlabWrap.cmake

@@ -62,10 +62,10 @@ macro(find_and_configure_matlab)
 endmacro()
 
 # Consistent and user-friendly wrap function
-function(matlab_wrap interfaceHeader linkLibraries
+function(matlab_wrap interfaceHeader moduleName linkLibraries
          extraIncludeDirs extraMexFlags ignore_classes)
   find_and_configure_matlab()
-  wrap_and_install_library("${interfaceHeader}" "${linkLibraries}"
+  wrap_and_install_library("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                            "${extraIncludeDirs}" "${extraMexFlags}"
                            "${ignore_classes}")
 endfunction()
@@ -77,6 +77,7 @@ endfunction()
 # Arguments:
 #
 # interfaceHeader:  The relative path to the wrapper interface definition file.
+# moduleName:       The name of the wrapped module, e.g. gtsam
 # linkLibraries:    Any *additional* libraries to link.  Your project library
 # (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will
 # be linked automatically.  So normally, leave this empty.
@@ -85,15 +86,15 @@ endfunction()
 # extraMexFlags:    Any *additional* flags to pass to the compiler when building
 # the wrap code.  Normally, leave this empty.
 # ignore_classes:  List of classes to ignore in the wrapping.
-function(wrap_and_install_library interfaceHeader linkLibraries
+function(wrap_and_install_library interfaceHeader moduleName linkLibraries
          extraIncludeDirs extraMexFlags ignore_classes)
-  wrap_library_internal("${interfaceHeader}" "${linkLibraries}"
+  wrap_library_internal("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                         "${extraIncludeDirs}" "${mexFlags}")
-  install_wrapped_library_internal("${interfaceHeader}")
+  install_wrapped_library_internal("${moduleName}")
 endfunction()
 
 # Internal function that wraps a library and compiles the wrapper
-function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
+function(wrap_library_internal interfaceHeader moduleName linkLibraries extraIncludeDirs
          extraMexFlags)
   if(UNIX AND NOT APPLE)
     if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@@ -120,7 +121,6 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   # Extract module name from interface header file name
   get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
   get_filename_component(modulePath "${interfaceHeader}" PATH)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
 
   # Paths for generated files
   set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
@@ -136,8 +136,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   # explicit link libraries list so that the next block of code can unpack any
   # static libraries
   set(automaticDependencies "")
-  foreach(lib ${moduleName} ${linkLibraries})
+  foreach(lib ${module} ${linkLibraries})
-    # message("MODULE NAME: ${moduleName}")
     if(TARGET "${lib}")
       get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
       # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
@@ -176,7 +175,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   set(otherLibraryTargets "")
   set(otherLibraryNontargets "")
   set(otherSourcesAndObjects "")
-  foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
+  foreach(lib ${module} ${linkLibraries} ${automaticDependencies})
     if(TARGET "${lib}")
       if(WRAP_MEX_BUILD_STATIC_MODULE)
         get_target_property(target_sources ${lib} SOURCES)
@@ -250,7 +249,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
     COMMAND
       ${CMAKE_COMMAND} -E env
       "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader}
+      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src "${interfaceHeader}"
       --module_name ${moduleName} --out ${generated_files_path}
       --top_module_namespaces ${moduleName} --ignore ${ignore_classes}
     VERBATIM
@@ -324,8 +323,8 @@ endfunction()
 
 # Internal function that installs a wrap toolbox
 function(install_wrapped_library_internal interfaceHeader)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
+  get_filename_component(module "${interfaceHeader}" NAME_WE)
-  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
+  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${module}")
 
   # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash
   # on the directory name here prevents creating the top-level module name

wrap/gtwrap/interface_parser/type.py

@@ -53,6 +53,10 @@ class Typename:
         self.name = t[-1]  # the name is the last element in this list
         self.namespaces = t[:-1]
 
+        # If the first namespace is empty string, just get rid of it.
+        if self.namespaces and self.namespaces[0] == '':
+            self.namespaces.pop(0)
+
         if instantiations:
             if isinstance(instantiations, Sequence):
                 self.instantiations = instantiations  # type: ignore
@@ -92,8 +96,8 @@ class Typename:
         else:
             cpp_name = self.name
         return '{}{}{}'.format(
-            "::".join(self.namespaces[idx:]),
+            "::".join(self.namespaces),
-            "::" if self.namespaces[idx:] else "",
+            "::" if self.namespaces else "",
             cpp_name,
         )
 

wrap/gtwrap/matlab_wrapper/mixins.py

@@ -108,7 +108,7 @@ class FormatMixin:
         elif is_method:
             formatted_type_name += self.data_type_param.get(name) or name
         else:
-            formatted_type_name += name
+            formatted_type_name += str(name)
 
         if separator == "::":  # C++
             templates = []
@@ -192,10 +192,9 @@ class FormatMixin:
         method = ''
 
         if isinstance(static_method, parser.StaticMethod):
-            method += "".join([separator + x for x in static_method.parent.namespaces()]) + \
+            method += static_method.parent.to_cpp() + separator
-                      separator + static_method.parent.name + separator
 
-        return method[2 * len(separator):]
+        return method
 
     def _format_global_function(self,
                                 function: Union[parser.GlobalFunction, Any],

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -239,18 +239,18 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
         return var_list_wrap
 
-    def _wrap_method_check_statement(self, args):
+    def _wrap_method_check_statement(self, args: parser.ArgumentList):
         """
         Wrap the given arguments into either just a varargout call or a
         call in an if statement that checks if the parameters are accurate.
+
+        TODO Update this method so that default arguments are supported.
         """
-        check_statement = ''
         arg_id = 1
 
-        if check_statement == '':
+        param_count = len(args)
-            check_statement = \
+        check_statement = 'if length(varargin) == {param_count}'.format(
-                'if length(varargin) == {param_count}'.format(
+            param_count=param_count)
-                    param_count=len(args.list()))
 
         for _, arg in enumerate(args.list()):
             name = arg.ctype.typename.name
@@ -809,7 +809,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
         for static_method in static_methods:
             format_name = list(static_method[0].name)
-            format_name[0] = format_name[0].upper()
+            format_name[0] = format_name[0]
 
             if static_method[0].name in self.ignore_methods:
                 continue
@@ -850,12 +850,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                     wrapper=self._wrapper_name(),
                     id=self._update_wrapper_id(
                         (namespace_name, instantiated_class,
-                         static_overload.name, static_overload)),
+                        static_overload.name, static_overload)),
                     class_name=instantiated_class.name,
                     end_statement=end_statement),
-                                               prefix='    ')
+                                            prefix='    ')
 
-            #TODO Figure out what is static_overload doing here.
+            # If the arguments don't match any of the checks above,
+            # throw an error with the class and method name.
             method_text += textwrap.indent(textwrap.dedent("""\
                     error('Arguments do not match any overload of function {class_name}.{method_name}');
                 """.format(class_name=class_name,
@@ -1081,7 +1082,6 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         obj_start = ''
 
         if isinstance(method, instantiator.InstantiatedMethod):
-            # method_name = method.original.name
             method_name = method.to_cpp()
             obj_start = 'obj->'
 
@@ -1090,6 +1090,10 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                 #     self._format_type_name(method.instantiations))
                 method = method.to_cpp()
 
+        elif isinstance(method, instantiator.InstantiatedStaticMethod):
+            method_name = self._format_static_method(method, '::')
+            method_name += method.original.name
+
         elif isinstance(method, parser.GlobalFunction):
             method_name = self._format_global_function(method, '::')
             method_name += method.name
@@ -1250,7 +1254,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                 method_name = ''
 
                 if is_static_method:
-                    method_name = self._format_static_method(extra) + '.'
+                    method_name = self._format_static_method(extra, '.')
 
                 method_name += extra.name
 
@@ -1567,23 +1571,23 @@ class MatlabWrapper(CheckMixin, FormatMixin):
 
     def wrap(self, files, path):
         """High level function to wrap the project."""
+        content = ""
         modules = {}
         for file in files:
             with open(file, 'r') as f:
-                content = f.read()
+                content += f.read()
 
-            # Parse the contents of the interface file
+        # Parse the contents of the interface file
-            parsed_result = parser.Module.parseString(content)
+        parsed_result = parser.Module.parseString(content)
-            # print(parsed_result)
 
-            # Instantiate the module
+        # Instantiate the module
-            module = instantiator.instantiate_namespace(parsed_result)
+        module = instantiator.instantiate_namespace(parsed_result)
 
-            if module.name in modules:
+        if module.name in modules:
-                modules[module.
+            modules[
-                        name].content[0].content += module.content[0].content
+                module.name].content[0].content += module.content[0].content
-            else:
+        else:
-                modules[module.name] = module
+            modules[module.name] = module
 
         for module in modules.values():
             # Wrap the full namespace

wrap/gtwrap/template_instantiator/helpers.py

@@ -55,16 +55,14 @@ def instantiate_type(
     # make a deep copy so that there is no overwriting of original template params
     ctype = deepcopy(ctype)
 
-    # Check if the return type has template parameters
+    # Check if the return type has template parameters as the typename's name
     if ctype.typename.instantiations:
         for idx, instantiation in enumerate(ctype.typename.instantiations):
             if instantiation.name in template_typenames:
                 template_idx = template_typenames.index(instantiation.name)
-                ctype.typename.instantiations[
+                ctype.typename.instantiations[idx].name =\
-                    idx] = instantiations[  # type: ignore
+                    instantiations[template_idx]
-                        template_idx]
 
-        return ctype
 
     str_arg_typename = str(ctype.typename)
 
@@ -125,9 +123,18 @@ def instantiate_type(
 
     # Case when 'This' is present in the type namespace, e.g `This::Subclass`.
     elif 'This' in str_arg_typename:
-        # Simply get the index of `This` in the namespace and replace it with the instantiated name.
+        # Check if `This` is in the namespaces
-        namespace_idx = ctype.typename.namespaces.index('This')
+        if 'This' in ctype.typename.namespaces:
-        ctype.typename.namespaces[namespace_idx] = cpp_typename.name
+            # Simply get the index of `This` in the namespace and
+            # replace it with the instantiated name.
+            namespace_idx = ctype.typename.namespaces.index('This')
+            ctype.typename.namespaces[namespace_idx] = cpp_typename.name
+        # Else check if it is in the template namespace, e.g vector<This::Value>
+        else:
+            for idx, instantiation in enumerate(ctype.typename.instantiations):
+                if 'This' in instantiation.namespaces:
+                    ctype.typename.instantiations[idx].namespaces = \
+                        cpp_typename.namespaces + [cpp_typename.name]
         return ctype
 
     else:

wrap/tests/expected/matlab/+gtsam/GeneralSFMFactorCal3Bundler.m

@@ -0,0 +1,31 @@
+%class GeneralSFMFactorCal3Bundler, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef GeneralSFMFactorCal3Bundler < handle
+  properties
+    ptr_gtsamGeneralSFMFactorCal3Bundler = 0
+  end
+  methods
+    function obj = GeneralSFMFactorCal3Bundler(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        special_cases_wrapper(7, my_ptr);
+      else
+        error('Arguments do not match any overload of gtsam.GeneralSFMFactorCal3Bundler constructor');
+      end
+      obj.ptr_gtsamGeneralSFMFactorCal3Bundler = my_ptr;
+    end
+
+    function delete(obj)
+      special_cases_wrapper(8, obj.ptr_gtsamGeneralSFMFactorCal3Bundler);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/+gtsam/Point3.m

@@ -78,7 +78,7 @@ classdef Point3 < handle
       error('Arguments do not match any overload of function Point3.StaticFunctionRet');
     end
 
-    function varargout = StaticFunction(varargin)
+    function varargout = staticFunction(varargin)
       % STATICFUNCTION usage: staticFunction() : returns double
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0

wrap/tests/expected/matlab/+gtsam/SfmTrack.m

@@ -0,0 +1,31 @@
+%class SfmTrack, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef SfmTrack < handle
+  properties
+    ptr_gtsamSfmTrack = 0
+  end
+  methods
+    function obj = SfmTrack(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        special_cases_wrapper(3, my_ptr);
+      else
+        error('Arguments do not match any overload of gtsam.SfmTrack constructor');
+      end
+      obj.ptr_gtsamSfmTrack = my_ptr;
+    end
+
+    function delete(obj)
+      special_cases_wrapper(4, obj.ptr_gtsamSfmTrack);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/+gtsam/Values.m

@@ -0,0 +1,59 @@
+%class Values, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%Values()
+%Values(Values other)
+%
+%-------Methods-------
+%insert(size_t j, Vector vector) : returns void
+%insert(size_t j, Matrix matrix) : returns void
+%
+classdef Values < handle
+  properties
+    ptr_gtsamValues = 0
+  end
+  methods
+    function obj = Values(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        namespaces_wrapper(26, my_ptr);
+      elseif nargin == 0
+        my_ptr = namespaces_wrapper(27);
+      elseif nargin == 1 && isa(varargin{1},'gtsam.Values')
+        my_ptr = namespaces_wrapper(28, varargin{1});
+      else
+        error('Arguments do not match any overload of gtsam.Values constructor');
+      end
+      obj.ptr_gtsamValues = my_ptr;
+    end
+
+    function delete(obj)
+      namespaces_wrapper(29, obj.ptr_gtsamValues);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = insert(this, varargin)
+      % INSERT usage: insert(size_t j, Vector vector) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') && size(varargin{2},2)==1
+        namespaces_wrapper(30, this, varargin{:});
+        return
+      end
+      % INSERT usage: insert(size_t j, Matrix matrix) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
+        namespaces_wrapper(31, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function gtsam.Values.insert');
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/+ns2/ClassA.m

@@ -74,7 +74,7 @@ classdef ClassA < handle
   end
 
   methods(Static = true)
-    function varargout = Afunction(varargin)
+    function varargout = afunction(varargin)
       % AFUNCTION usage: afunction() : returns double
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0

wrap/tests/expected/matlab/DefaultFuncInt.m

@@ -0,0 +1,6 @@
+function varargout = DefaultFuncInt(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric')
+        functions_wrapper(8, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncInt');
+      end

wrap/tests/expected/matlab/DefaultFuncObj.m

@@ -0,0 +1,6 @@
+function varargout = DefaultFuncObj(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.KeyFormatter')
+        functions_wrapper(10, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncObj');
+      end

wrap/tests/expected/matlab/DefaultFuncString.m

@@ -0,0 +1,6 @@
+function varargout = DefaultFuncString(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'char')
+        functions_wrapper(9, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncString');
+      end

wrap/tests/expected/matlab/DefaultFuncVector.m

@@ -0,0 +1,6 @@
+function varargout = DefaultFuncVector(varargin)
+      if length(varargin) == 2 && isa(varargin{1},'std.vectornumeric') && isa(varargin{2},'std.vectorchar')
+        functions_wrapper(12, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncVector');
+      end

wrap/tests/expected/matlab/DefaultFuncZero.m

@@ -0,0 +1,6 @@
+function varargout = DefaultFuncZero(varargin)
+      if length(varargin) == 5 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'logical') && isa(varargin{5},'logical')
+        functions_wrapper(11, varargin{:});
+      else
+        error('Arguments do not match any overload of function DefaultFuncZero');
+      end

wrap/tests/expected/matlab/ForwardKinematics.m

@@ -0,0 +1,36 @@
+%class ForwardKinematics, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%ForwardKinematics(Robot robot, string start_link_name, string end_link_name, Values joint_angles, Pose3 l2Tp)
+%
+classdef ForwardKinematics < handle
+  properties
+    ptr_ForwardKinematics = 0
+  end
+  methods
+    function obj = ForwardKinematics(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        class_wrapper(55, my_ptr);
+      elseif nargin == 5 && isa(varargin{1},'gtdynamics.Robot') && isa(varargin{2},'char') && isa(varargin{3},'char') && isa(varargin{4},'gtsam.Values') && isa(varargin{5},'gtsam.Pose3')
+        my_ptr = class_wrapper(56, varargin{1}, varargin{2}, varargin{3}, varargin{4}, varargin{5});
+      else
+        error('Arguments do not match any overload of ForwardKinematics constructor');
+      end
+      obj.ptr_ForwardKinematics = my_ptr;
+    end
+
+    function delete(obj)
+      class_wrapper(57, obj.ptr_ForwardKinematics);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/ForwardKinematicsFactor.m

@@ -0,0 +1,36 @@
+%class ForwardKinematicsFactor, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+classdef ForwardKinematicsFactor < gtsam.BetweenFactor<gtsam.Pose3>
+  properties
+    ptr_ForwardKinematicsFactor = 0
+  end
+  methods
+    function obj = ForwardKinematicsFactor(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = inheritance_wrapper(36, varargin{2});
+        end
+        base_ptr = inheritance_wrapper(35, my_ptr);
+      else
+        error('Arguments do not match any overload of ForwardKinematicsFactor constructor');
+      end
+      obj = obj@gtsam.BetweenFactorPose3(uint64(5139824614673773682), base_ptr);
+      obj.ptr_ForwardKinematicsFactor = my_ptr;
+    end
+
+    function delete(obj)
+      inheritance_wrapper(37, obj.ptr_ForwardKinematicsFactor);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/FunDouble.m

@@ -3,6 +3,7 @@
 %
 %-------Methods-------
 %multiTemplatedMethodStringSize_t(double d, string t, size_t u) : returns Fun<double>
+%sets() : returns std::map<double,Fun<double>::double>
 %templatedMethodString(double d, string t) : returns Fun<double>
 %
 %-------Static Methods-------
@@ -46,11 +47,21 @@ classdef FunDouble < handle
       error('Arguments do not match any overload of function FunDouble.multiTemplatedMethodStringSize_t');
     end
 
+    function varargout = sets(this, varargin)
+      % SETS usage: sets() : returns std.mapdoubledouble
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        varargout{1} = class_wrapper(8, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function FunDouble.sets');
+    end
+
     function varargout = templatedMethodString(this, varargin)
       % TEMPLATEDMETHODSTRING usage: templatedMethodString(double d, string t) : returns Fun<double>
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'char')
-        varargout{1} = class_wrapper(8, this, varargin{:});
+        varargout{1} = class_wrapper(9, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function FunDouble.templatedMethodString');
@@ -59,22 +70,22 @@ classdef FunDouble < handle
   end
 
   methods(Static = true)
-    function varargout = StaticMethodWithThis(varargin)
+    function varargout = staticMethodWithThis(varargin)
       % STATICMETHODWITHTHIS usage: staticMethodWithThis() : returns Fundouble
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        varargout{1} = class_wrapper(9, varargin{:});
+        varargout{1} = class_wrapper(10, varargin{:});
         return
       end
 
       error('Arguments do not match any overload of function FunDouble.staticMethodWithThis');
     end
 
-    function varargout = TemplatedStaticMethodInt(varargin)
+    function varargout = templatedStaticMethodInt(varargin)
       % TEMPLATEDSTATICMETHODINT usage: templatedStaticMethodInt(int m) : returns double
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(10, varargin{:});
+        varargout{1} = class_wrapper(11, varargin{:});
         return
       end
 

wrap/tests/expected/matlab/FunRange.m

@@ -52,7 +52,7 @@ classdef FunRange < handle
   end
 
   methods(Static = true)
-    function varargout = Create(varargin)
+    function varargout = create(varargin)
       % CREATE usage: create() : returns FunRange
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0

wrap/tests/expected/matlab/MultipleTemplatesIntDouble.m

@@ -9,7 +9,7 @@ classdef MultipleTemplatesIntDouble < handle
     function obj = MultipleTemplatesIntDouble(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(50, my_ptr);
+        class_wrapper(51, my_ptr);
       else
         error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor');
       end
@@ -17,7 +17,7 @@ classdef MultipleTemplatesIntDouble < handle
     end
 
     function delete(obj)
-      class_wrapper(51, obj.ptr_MultipleTemplatesIntDouble);
+      class_wrapper(52, obj.ptr_MultipleTemplatesIntDouble);
     end
 
     function display(obj), obj.print(''); end

wrap/tests/expected/matlab/MultipleTemplatesIntFloat.m

@@ -9,7 +9,7 @@ classdef MultipleTemplatesIntFloat < handle
     function obj = MultipleTemplatesIntFloat(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(52, my_ptr);
+        class_wrapper(53, my_ptr);
       else
         error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor');
       end
@@ -17,7 +17,7 @@ classdef MultipleTemplatesIntFloat < handle
     end
 
     function delete(obj)
-      class_wrapper(53, obj.ptr_MultipleTemplatesIntFloat);
+      class_wrapper(54, obj.ptr_MultipleTemplatesIntFloat);
     end
 
     function display(obj), obj.print(''); end

wrap/tests/expected/matlab/MyFactorPosePoint2.m

@@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle
     function obj = MyFactorPosePoint2(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(63, my_ptr);
+        class_wrapper(64, my_ptr);
       elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
-        my_ptr = class_wrapper(64, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = class_wrapper(65, varargin{1}, varargin{2}, varargin{3}, varargin{4});
       else
         error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
       end
@@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle
     end
 
     function delete(obj)
-      class_wrapper(65, obj.ptr_MyFactorPosePoint2);
+      class_wrapper(66, obj.ptr_MyFactorPosePoint2);
     end
 
     function display(obj), obj.print(''); end
@@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle
       % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter')
-        class_wrapper(66, this, varargin{:});
+        class_wrapper(67, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function MyFactorPosePoint2.print');

wrap/tests/expected/matlab/MyVector12.m

@@ -12,9 +12,9 @@ classdef MyVector12 < handle
     function obj = MyVector12(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(47, my_ptr);
+        class_wrapper(48, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(48);
+        my_ptr = class_wrapper(49);
       else
         error('Arguments do not match any overload of MyVector12 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyVector12 < handle
     end
 
     function delete(obj)
-      class_wrapper(49, obj.ptr_MyVector12);
+      class_wrapper(50, obj.ptr_MyVector12);
     end
 
     function display(obj), obj.print(''); end

wrap/tests/expected/matlab/MyVector3.m

@@ -12,9 +12,9 @@ classdef MyVector3 < handle
     function obj = MyVector3(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(44, my_ptr);
+        class_wrapper(45, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(45);
+        my_ptr = class_wrapper(46);
       else
         error('Arguments do not match any overload of MyVector3 constructor');
       end
@@ -22,7 +22,7 @@ classdef MyVector3 < handle
     end
 
     function delete(obj)
-      class_wrapper(46, obj.ptr_MyVector3);
+      class_wrapper(47, obj.ptr_MyVector3);
     end
 
     function display(obj), obj.print(''); end

wrap/tests/expected/matlab/PrimitiveRefDouble.m

@@ -19,9 +19,9 @@ classdef PrimitiveRefDouble < handle
     function obj = PrimitiveRefDouble(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(40, my_ptr);
+        class_wrapper(41, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(41);
+        my_ptr = class_wrapper(42);
       else
         error('Arguments do not match any overload of PrimitiveRefDouble constructor');
       end
@@ -29,7 +29,7 @@ classdef PrimitiveRefDouble < handle
     end
 
     function delete(obj)
-      class_wrapper(42, obj.ptr_PrimitiveRefDouble);
+      class_wrapper(43, obj.ptr_PrimitiveRefDouble);
     end
 
     function display(obj), obj.print(''); end
@@ -43,7 +43,7 @@ classdef PrimitiveRefDouble < handle
       % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(43, varargin{:});
+        varargout{1} = class_wrapper(44, varargin{:});
         return
       end
 

wrap/tests/expected/matlab/ScopedTemplateResult.m

@@ -0,0 +1,36 @@
+%class ScopedTemplateResult, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%ScopedTemplateResult(Result::Value arg)
+%
+classdef ScopedTemplateResult < handle
+  properties
+    ptr_ScopedTemplateResult = 0
+  end
+  methods
+    function obj = ScopedTemplateResult(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        template_wrapper(6, my_ptr);
+      elseif nargin == 1 && isa(varargin{1},'Result::Value')
+        my_ptr = template_wrapper(7, varargin{1});
+      else
+        error('Arguments do not match any overload of ScopedTemplateResult constructor');
+      end
+      obj.ptr_ScopedTemplateResult = my_ptr;
+    end
+
+    function delete(obj)
+      template_wrapper(8, obj.ptr_ScopedTemplateResult);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/TemplatedConstructor.m

@@ -0,0 +1,45 @@
+%class TemplatedConstructor, see Doxygen page for details
+%at https://gtsam.org/doxygen/
+%
+%-------Constructors-------
+%TemplatedConstructor()
+%TemplatedConstructor(string arg)
+%TemplatedConstructor(int arg)
+%TemplatedConstructor(double arg)
+%
+classdef TemplatedConstructor < handle
+  properties
+    ptr_TemplatedConstructor = 0
+  end
+  methods
+    function obj = TemplatedConstructor(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        template_wrapper(0, my_ptr);
+      elseif nargin == 0
+        my_ptr = template_wrapper(1);
+      elseif nargin == 1 && isa(varargin{1},'char')
+        my_ptr = template_wrapper(2, varargin{1});
+      elseif nargin == 1 && isa(varargin{1},'numeric')
+        my_ptr = template_wrapper(3, varargin{1});
+      elseif nargin == 1 && isa(varargin{1},'double')
+        my_ptr = template_wrapper(4, varargin{1});
+      else
+        error('Arguments do not match any overload of TemplatedConstructor constructor');
+      end
+      obj.ptr_TemplatedConstructor = my_ptr;
+    end
+
+    function delete(obj)
+      template_wrapper(5, obj.ptr_TemplatedConstructor);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected/matlab/Test.m

@@ -40,11 +40,11 @@ classdef Test < handle
     function obj = Test(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        class_wrapper(11, my_ptr);
+        class_wrapper(12, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(12);
+        my_ptr = class_wrapper(13);
       elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = class_wrapper(13, varargin{1}, varargin{2});
+        my_ptr = class_wrapper(14, varargin{1}, varargin{2});
       else
         error('Arguments do not match any overload of Test constructor');
       end
@@ -52,7 +52,7 @@ classdef Test < handle
     end
 
     function delete(obj)
-      class_wrapper(14, obj.ptr_Test);
+      class_wrapper(15, obj.ptr_Test);
     end
 
     function display(obj), obj.print(''); end
@@ -63,7 +63,7 @@ classdef Test < handle
       % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        class_wrapper(15, this, varargin{:});
+        class_wrapper(16, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.arg_EigenConstRef');
@@ -73,7 +73,7 @@ classdef Test < handle
       % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = class_wrapper(16, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.create_MixedPtrs');
@@ -83,7 +83,7 @@ classdef Test < handle
       % CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = class_wrapper(17, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(18, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.create_ptrs');
@@ -93,7 +93,7 @@ classdef Test < handle
       % GET_CONTAINER usage: get_container() : returns std.vectorTest
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        varargout{1} = class_wrapper(18, this, varargin{:});
+        varargout{1} = class_wrapper(19, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.get_container');
@@ -103,7 +103,7 @@ classdef Test < handle
       % LAMBDA usage: lambda() : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        class_wrapper(19, this, varargin{:});
+        class_wrapper(20, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.lambda');
@@ -113,7 +113,7 @@ classdef Test < handle
       % PRINT usage: print() : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        class_wrapper(20, this, varargin{:});
+        class_wrapper(21, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.print');
@@ -123,7 +123,7 @@ classdef Test < handle
       % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = class_wrapper(21, this, varargin{:});
+        varargout{1} = class_wrapper(22, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_Point2Ptr');
@@ -133,7 +133,7 @@ classdef Test < handle
       % RETURN_TEST usage: return_Test(Test value) : returns Test
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(22, this, varargin{:});
+        varargout{1} = class_wrapper(23, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_Test');
@@ -143,7 +143,7 @@ classdef Test < handle
       % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(23, this, varargin{:});
+        varargout{1} = class_wrapper(24, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_TestPtr');
@@ -153,7 +153,7 @@ classdef Test < handle
       % RETURN_BOOL usage: return_bool(bool value) : returns bool
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = class_wrapper(24, this, varargin{:});
+        varargout{1} = class_wrapper(25, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_bool');
@@ -163,7 +163,7 @@ classdef Test < handle
       % RETURN_DOUBLE usage: return_double(double value) : returns double
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(25, this, varargin{:});
+        varargout{1} = class_wrapper(26, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_double');
@@ -173,7 +173,7 @@ classdef Test < handle
       % RETURN_FIELD usage: return_field(Test t) : returns bool
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = class_wrapper(26, this, varargin{:});
+        varargout{1} = class_wrapper(27, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_field');
@@ -183,7 +183,7 @@ classdef Test < handle
       % RETURN_INT usage: return_int(int value) : returns int
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(27, this, varargin{:});
+        varargout{1} = class_wrapper(28, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_int');
@@ -193,7 +193,7 @@ classdef Test < handle
       % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(28, this, varargin{:});
+        varargout{1} = class_wrapper(29, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_matrix1');
@@ -203,7 +203,7 @@ classdef Test < handle
       % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = class_wrapper(29, this, varargin{:});
+        varargout{1} = class_wrapper(30, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_matrix2');
@@ -213,13 +213,13 @@ classdef Test < handle
       % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:});
         return
       end
       % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix >
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_pair');
@@ -229,7 +229,7 @@ classdef Test < handle
       % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
-        [ varargout{1} varargout{2} ] = class_wrapper(32, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(33, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_ptrs');
@@ -239,7 +239,7 @@ classdef Test < handle
       % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = class_wrapper(33, this, varargin{:});
+        varargout{1} = class_wrapper(34, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_size_t');
@@ -249,7 +249,7 @@ classdef Test < handle
       % RETURN_STRING usage: return_string(string value) : returns string
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'char')
-        varargout{1} = class_wrapper(34, this, varargin{:});
+        varargout{1} = class_wrapper(35, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_string');
@@ -259,7 +259,7 @@ classdef Test < handle
       % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = class_wrapper(35, this, varargin{:});
+        varargout{1} = class_wrapper(36, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_vector1');
@@ -269,19 +269,13 @@ classdef Test < handle
       % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
-        varargout{1} = class_wrapper(36, this, varargin{:});
+        varargout{1} = class_wrapper(37, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_vector2');
     end
 
     function varargout = set_container(this, varargin)
-      % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
-      % Doxygen can be found at https://gtsam.org/doxygen/
-      if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
-        class_wrapper(37, this, varargin{:});
-        return
-      end
       % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
@@ -294,6 +288,12 @@ classdef Test < handle
         class_wrapper(39, this, varargin{:});
         return
       end
+      % SET_CONTAINER usage: set_container(vector<Test> container) : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 1 && isa(varargin{1},'std.vectorTest')
+        class_wrapper(40, this, varargin{:});
+        return
+      end
       error('Arguments do not match any overload of function Test.set_container');
     end
 

wrap/tests/expected/matlab/class_wrapper.cpp

@@ -231,7 +231,14 @@ void FunDouble_multiTemplatedMethod_7(int nargout, mxArray *out[], int nargin, c
   out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(obj->multiTemplatedMethod<string,size_t>(d,t,u)),"Fun<double>", false);
 }
 
-void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_sets_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("sets",nargout,nargin-1,0);
+  auto obj = unwrap_shared_ptr<Fun<double>>(in[0], "ptr_FunDouble");
+  out[0] = wrap_shared_ptr(boost::make_shared<std::map<double,Fun<double>::double>>(obj->sets()),"std.mapdoubledouble", false);
+}
+
+void FunDouble_templatedMethod_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("templatedMethodString",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<Fun<double>>(in[0], "ptr_FunDouble");
@@ -240,20 +247,20 @@ void FunDouble_templatedMethod_8(int nargout, mxArray *out[], int nargin, const
   out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(obj->templatedMethod<string>(d,t)),"Fun<double>", false);
 }
 
-void FunDouble_staticMethodWithThis_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_staticMethodWithThis_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("FunDouble.staticMethodWithThis",nargout,nargin,0);
+  checkArguments("Fun<double>.staticMethodWithThis",nargout,nargin,0);
   out[0] = wrap_shared_ptr(boost::make_shared<Fun<double>>(Fun<double>::staticMethodWithThis()),"Fundouble", false);
 }
 
-void FunDouble_templatedStaticMethodInt_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void FunDouble_templatedStaticMethodInt_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("FunDouble.templatedStaticMethodInt",nargout,nargin,1);
+  checkArguments("Fun<double>.templatedStaticMethodInt",nargout,nargin,1);
   int m = unwrap< int >(in[0]);
-  out[0] = wrap< double >(Fun<double>::templatedStaticMethodInt(m));
+  out[0] = wrap< double >(Fun<double>::templatedStaticMethod(m));
 }
 
-void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_collectorInsertAndMakeBase_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -262,7 +269,7 @@ void Test_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin,
   collector_Test.insert(self);
 }
 
-void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -273,7 +280,7 @@ void Test_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -286,7 +293,7 @@ void Test_constructor_13(int nargout, mxArray *out[], int nargin, const mxArray
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_deconstructor_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("delete_Test",nargout,nargin,1);
@@ -299,7 +306,7 @@ void Test_deconstructor_14(int nargout, mxArray *out[], int nargin, const mxArra
   }
 }
 
-void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_arg_EigenConstRef_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -307,7 +314,7 @@ void Test_arg_EigenConstRef_15(int nargout, mxArray *out[], int nargin, const mx
   obj->arg_EigenConstRef(value);
 }
 
-void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_MixedPtrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -316,7 +323,7 @@ void Test_create_MixedPtrs_16(int nargout, mxArray *out[], int nargin, const mxA
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_ptrs_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("create_ptrs",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -325,28 +332,28 @@ void Test_create_ptrs_17(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_get_container_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_get_container_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("get_container",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   out[0] = wrap_shared_ptr(boost::make_shared<std::vector<testing::Test>>(obj->get_container()),"std.vectorTest", false);
 }
 
-void Test_lambda_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_lambda_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("lambda",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   obj->lambda();
 }
 
-void Test_print_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_print_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("print",nargout,nargin-1,0);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   obj->print();
 }
 
-void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Point2Ptr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Point2Ptr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -357,7 +364,7 @@ void Test_return_Point2Ptr_21(int nargout, mxArray *out[], int nargin, const mxA
   }
 }
 
-void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Test_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_Test",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -365,7 +372,7 @@ void Test_return_Test_22(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap_shared_ptr(boost::make_shared<Test>(obj->return_Test(value)),"Test", false);
 }
 
-void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_TestPtr_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_TestPtr",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -373,7 +380,7 @@ void Test_return_TestPtr_23(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }
 
-void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_bool_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_bool",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -381,7 +388,7 @@ void Test_return_bool_24(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_bool(value));
 }
 
-void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_double_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_double",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -389,7 +396,7 @@ void Test_return_double_25(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< double >(obj->return_double(value));
 }
 
-void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_field_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_field",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -397,7 +404,7 @@ void Test_return_field_26(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_field(t));
 }
 
-void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_int_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_int",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -405,7 +412,7 @@ void Test_return_int_27(int nargout, mxArray *out[], int nargin, const mxArray *
   out[0] = wrap< int >(obj->return_int(value));
 }
 
-void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix1_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_matrix1",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -413,7 +420,7 @@ void Test_return_matrix1_28(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }
 
-void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix2_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_matrix2",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -421,7 +428,7 @@ void Test_return_matrix2_29(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }
 
-void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_pair",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -432,7 +439,7 @@ void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_pair",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -442,7 +449,7 @@ void Test_return_pair_31(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_ptrs_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_ptrs",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -453,7 +460,7 @@ void Test_return_ptrs_32(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_size_t",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -461,7 +468,7 @@ void Test_return_size_t_33(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< size_t >(obj->return_size_t(value));
 }
 
-void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_string",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -469,7 +476,7 @@ void Test_return_string_34(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< string >(obj->return_string(value));
 }
 
-void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_vector1",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -477,7 +484,7 @@ void Test_return_vector1_35(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector1(value));
 }
 
-void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("return_vector2",nargout,nargin-1,1);
   auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
@@ -485,14 +492,6 @@ void Test_return_vector2_36(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector2(value));
 }
 
-void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
-{
-  checkArguments("set_container",nargout,nargin-1,1);
-  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  boost::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], "ptr_stdvectorTest");
-  obj->set_container(*container);
-}
-
 void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("set_container",nargout,nargin-1,1);
@@ -509,7 +508,15 @@ void Test_set_container_39(int nargout, mxArray *out[], int nargin, const mxArra
   obj->set_container(*container);
 }
 
-void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_set_container_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("set_container",nargout,nargin-1,1);
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  boost::shared_ptr<std::vector<testing::Test>> container = unwrap_shared_ptr< std::vector<testing::Test> >(in[1], "ptr_stdvectorTest");
+  obj->set_container(*container);
+}
+
+void PrimitiveRefDouble_collectorInsertAndMakeBase_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@@ -518,7 +525,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_40(int nargout, mxArray *out[
   collector_PrimitiveRefDouble.insert(self);
 }
 
-void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_constructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@@ -529,7 +536,7 @@ void PrimitiveRefDouble_constructor_41(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_deconstructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
   checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1);
@@ -542,14 +549,14 @@ void PrimitiveRefDouble_deconstructor_42(int nargout, mxArray *out[], int nargin
   }
 }
 
-void PrimitiveRefDouble_Brutal_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_Brutal_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1);
+  checkArguments("PrimitiveRef<double>.Brutal",nargout,nargin,1);
   double t = unwrap< double >(in[0]);
   out[0] = wrap_shared_ptr(boost::make_shared<PrimitiveRef<double>>(PrimitiveRef<double>::Brutal(t)),"PrimitiveRefdouble", false);
 }
 
-void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -558,7 +565,7 @@ void MyVector3_collectorInsertAndMakeBase_44(int nargout, mxArray *out[], int na
   collector_MyVector3.insert(self);
 }
 
-void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -569,7 +576,7 @@ void MyVector3_constructor_45(int nargout, mxArray *out[], int nargin, const mxA
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<3>> Shared;
   checkArguments("delete_MyVector3",nargout,nargin,1);
@@ -582,7 +589,7 @@ void MyVector3_deconstructor_46(int nargout, mxArray *out[], int nargin, const m
   }
 }
 
-void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -591,7 +598,7 @@ void MyVector12_collectorInsertAndMakeBase_47(int nargout, mxArray *out[], int n
   collector_MyVector12.insert(self);
 }
 
-void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_constructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -602,7 +609,7 @@ void MyVector12_constructor_48(int nargout, mxArray *out[], int nargin, const mx
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<12>> Shared;
   checkArguments("delete_MyVector12",nargout,nargin,1);
@@ -615,7 +622,7 @@ void MyVector12_deconstructor_49(int nargout, mxArray *out[], int nargin, const
   }
 }
 
-void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
@@ -624,7 +631,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(int nargout, mxArr
   collector_MultipleTemplatesIntDouble.insert(self);
 }
 
-void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
   checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1);
@@ -637,7 +644,7 @@ void MultipleTemplatesIntDouble_deconstructor_51(int nargout, mxArray *out[], in
   }
 }
 
-void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
@@ -646,7 +653,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(int nargout, mxArra
   collector_MultipleTemplatesIntFloat.insert(self);
 }
 
-void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
   checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
@@ -659,7 +666,7 @@ void MultipleTemplatesIntFloat_deconstructor_53(int nargout, mxArray *out[], int
   }
 }
 
-void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_collectorInsertAndMakeBase_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<ForwardKinematics> Shared;
@@ -668,7 +675,7 @@ void ForwardKinematics_collectorInsertAndMakeBase_54(int nargout, mxArray *out[]
   collector_ForwardKinematics.insert(self);
 }
 
-void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_constructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<ForwardKinematics> Shared;
@@ -684,7 +691,7 @@ void ForwardKinematics_constructor_55(int nargout, mxArray *out[], int nargin, c
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_deconstructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<ForwardKinematics> Shared;
   checkArguments("delete_ForwardKinematics",nargout,nargin,1);
@@ -697,7 +704,7 @@ void ForwardKinematics_deconstructor_56(int nargout, mxArray *out[], int nargin,
   }
 }
 
-void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_collectorInsertAndMakeBase_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
@@ -706,7 +713,7 @@ void TemplatedConstructor_collectorInsertAndMakeBase_57(int nargout, mxArray *ou
   collector_TemplatedConstructor.insert(self);
 }
 
-void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
@@ -717,7 +724,7 @@ void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
@@ -729,7 +736,7 @@ void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
@@ -741,7 +748,7 @@ void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_constructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
@@ -753,7 +760,7 @@ void TemplatedConstructor_constructor_61(int nargout, mxArray *out[], int nargin
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void TemplatedConstructor_deconstructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<TemplatedConstructor> Shared;
   checkArguments("delete_TemplatedConstructor",nargout,nargin,1);
@@ -766,7 +773,7 @@ void TemplatedConstructor_deconstructor_62(int nargout, mxArray *out[], int narg
   }
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_collectorInsertAndMakeBase_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -775,7 +782,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_63(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -790,7 +797,7 @@ void MyFactorPosePoint2_constructor_64(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
   checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1);
@@ -803,7 +810,7 @@ void MyFactorPosePoint2_deconstructor_65(int nargout, mxArray *out[], int nargin
   }
 }
 
-void MyFactorPosePoint2_print_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_print_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("print",nargout,nargin-1,2);
   auto obj = unwrap_shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>(in[0], "ptr_MyFactorPosePoint2");
@@ -849,94 +856,94 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       FunDouble_multiTemplatedMethod_7(nargout, out, nargin-1, in+1);
       break;
     case 8:
-      FunDouble_templatedMethod_8(nargout, out, nargin-1, in+1);
+      FunDouble_sets_8(nargout, out, nargin-1, in+1);
       break;
     case 9:
-      FunDouble_staticMethodWithThis_9(nargout, out, nargin-1, in+1);
+      FunDouble_templatedMethod_9(nargout, out, nargin-1, in+1);
       break;
     case 10:
-      FunDouble_templatedStaticMethodInt_10(nargout, out, nargin-1, in+1);
+      FunDouble_staticMethodWithThis_10(nargout, out, nargin-1, in+1);
       break;
     case 11:
-      Test_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1);
+      FunDouble_templatedStaticMethodInt_11(nargout, out, nargin-1, in+1);
       break;
     case 12:
-      Test_constructor_12(nargout, out, nargin-1, in+1);
+      Test_collectorInsertAndMakeBase_12(nargout, out, nargin-1, in+1);
       break;
     case 13:
       Test_constructor_13(nargout, out, nargin-1, in+1);
       break;
     case 14:
-      Test_deconstructor_14(nargout, out, nargin-1, in+1);
+      Test_constructor_14(nargout, out, nargin-1, in+1);
       break;
     case 15:
-      Test_arg_EigenConstRef_15(nargout, out, nargin-1, in+1);
+      Test_deconstructor_15(nargout, out, nargin-1, in+1);
       break;
     case 16:
-      Test_create_MixedPtrs_16(nargout, out, nargin-1, in+1);
+      Test_arg_EigenConstRef_16(nargout, out, nargin-1, in+1);
       break;
     case 17:
-      Test_create_ptrs_17(nargout, out, nargin-1, in+1);
+      Test_create_MixedPtrs_17(nargout, out, nargin-1, in+1);
       break;
     case 18:
-      Test_get_container_18(nargout, out, nargin-1, in+1);
+      Test_create_ptrs_18(nargout, out, nargin-1, in+1);
       break;
     case 19:
-      Test_lambda_19(nargout, out, nargin-1, in+1);
+      Test_get_container_19(nargout, out, nargin-1, in+1);
       break;
     case 20:
-      Test_print_20(nargout, out, nargin-1, in+1);
+      Test_lambda_20(nargout, out, nargin-1, in+1);
       break;
     case 21:
-      Test_return_Point2Ptr_21(nargout, out, nargin-1, in+1);
+      Test_print_21(nargout, out, nargin-1, in+1);
       break;
     case 22:
-      Test_return_Test_22(nargout, out, nargin-1, in+1);
+      Test_return_Point2Ptr_22(nargout, out, nargin-1, in+1);
       break;
     case 23:
-      Test_return_TestPtr_23(nargout, out, nargin-1, in+1);
+      Test_return_Test_23(nargout, out, nargin-1, in+1);
       break;
     case 24:
-      Test_return_bool_24(nargout, out, nargin-1, in+1);
+      Test_return_TestPtr_24(nargout, out, nargin-1, in+1);
       break;
     case 25:
-      Test_return_double_25(nargout, out, nargin-1, in+1);
+      Test_return_bool_25(nargout, out, nargin-1, in+1);
       break;
     case 26:
-      Test_return_field_26(nargout, out, nargin-1, in+1);
+      Test_return_double_26(nargout, out, nargin-1, in+1);
       break;
     case 27:
-      Test_return_int_27(nargout, out, nargin-1, in+1);
+      Test_return_field_27(nargout, out, nargin-1, in+1);
       break;
     case 28:
-      Test_return_matrix1_28(nargout, out, nargin-1, in+1);
+      Test_return_int_28(nargout, out, nargin-1, in+1);
       break;
     case 29:
-      Test_return_matrix2_29(nargout, out, nargin-1, in+1);
+      Test_return_matrix1_29(nargout, out, nargin-1, in+1);
       break;
     case 30:
-      Test_return_pair_30(nargout, out, nargin-1, in+1);
+      Test_return_matrix2_30(nargout, out, nargin-1, in+1);
       break;
     case 31:
       Test_return_pair_31(nargout, out, nargin-1, in+1);
       break;
     case 32:
-      Test_return_ptrs_32(nargout, out, nargin-1, in+1);
+      Test_return_pair_32(nargout, out, nargin-1, in+1);
       break;
     case 33:
-      Test_return_size_t_33(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_33(nargout, out, nargin-1, in+1);
       break;
     case 34:
-      Test_return_string_34(nargout, out, nargin-1, in+1);
+      Test_return_size_t_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      Test_return_vector1_35(nargout, out, nargin-1, in+1);
+      Test_return_string_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
-      Test_return_vector2_36(nargout, out, nargin-1, in+1);
+      Test_return_vector1_36(nargout, out, nargin-1, in+1);
       break;
     case 37:
-      Test_set_container_37(nargout, out, nargin-1, in+1);
+      Test_return_vector2_37(nargout, out, nargin-1, in+1);
       break;
     case 38:
       Test_set_container_38(nargout, out, nargin-1, in+1);
@@ -945,61 +952,61 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       Test_set_container_39(nargout, out, nargin-1, in+1);
       break;
     case 40:
-      PrimitiveRefDouble_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1);
+      Test_set_container_40(nargout, out, nargin-1, in+1);
       break;
     case 41:
-      PrimitiveRefDouble_constructor_41(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_collectorInsertAndMakeBase_41(nargout, out, nargin-1, in+1);
       break;
     case 42:
-      PrimitiveRefDouble_deconstructor_42(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_constructor_42(nargout, out, nargin-1, in+1);
       break;
     case 43:
-      PrimitiveRefDouble_Brutal_43(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_deconstructor_43(nargout, out, nargin-1, in+1);
       break;
     case 44:
-      MyVector3_collectorInsertAndMakeBase_44(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_Brutal_44(nargout, out, nargin-1, in+1);
       break;
     case 45:
-      MyVector3_constructor_45(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1);
       break;
     case 46:
-      MyVector3_deconstructor_46(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_46(nargout, out, nargin-1, in+1);
       break;
     case 47:
-      MyVector12_collectorInsertAndMakeBase_47(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_47(nargout, out, nargin-1, in+1);
       break;
     case 48:
-      MyVector12_constructor_48(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1);
       break;
     case 49:
-      MyVector12_deconstructor_49(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_49(nargout, out, nargin-1, in+1);
       break;
     case 50:
-      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1);
+      MyVector12_deconstructor_50(nargout, out, nargin-1, in+1);
       break;
     case 51:
-      MultipleTemplatesIntDouble_deconstructor_51(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
       break;
     case 52:
-      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_deconstructor_52(nargout, out, nargin-1, in+1);
       break;
     case 53:
-      MultipleTemplatesIntFloat_deconstructor_53(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1);
       break;
     case 54:
-      ForwardKinematics_collectorInsertAndMakeBase_54(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_deconstructor_54(nargout, out, nargin-1, in+1);
       break;
     case 55:
-      ForwardKinematics_constructor_55(nargout, out, nargin-1, in+1);
+      ForwardKinematics_collectorInsertAndMakeBase_55(nargout, out, nargin-1, in+1);
       break;
     case 56:
-      ForwardKinematics_deconstructor_56(nargout, out, nargin-1, in+1);
+      ForwardKinematics_constructor_56(nargout, out, nargin-1, in+1);
       break;
     case 57:
-      TemplatedConstructor_collectorInsertAndMakeBase_57(nargout, out, nargin-1, in+1);
+      ForwardKinematics_deconstructor_57(nargout, out, nargin-1, in+1);
       break;
     case 58:
-      TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_collectorInsertAndMakeBase_58(nargout, out, nargin-1, in+1);
       break;
     case 59:
       TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1);
@@ -1011,19 +1018,22 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       TemplatedConstructor_constructor_61(nargout, out, nargin-1, in+1);
       break;
     case 62:
-      TemplatedConstructor_deconstructor_62(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_constructor_62(nargout, out, nargin-1, in+1);
       break;
     case 63:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_63(nargout, out, nargin-1, in+1);
+      TemplatedConstructor_deconstructor_63(nargout, out, nargin-1, in+1);
       break;
     case 64:
-      MyFactorPosePoint2_constructor_64(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_collectorInsertAndMakeBase_64(nargout, out, nargin-1, in+1);
       break;
     case 65:
-      MyFactorPosePoint2_deconstructor_65(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_constructor_65(nargout, out, nargin-1, in+1);
       break;
     case 66:
-      MyFactorPosePoint2_print_66(nargout, out, nargin-1, in+1);
+      MyFactorPosePoint2_deconstructor_66(nargout, out, nargin-1, in+1);
+      break;
+    case 67:
+      MyFactorPosePoint2_print_67(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/expected/matlab/geometry_wrapper.cpp

@@ -286,14 +286,14 @@ void gtsamPoint3_string_serialize_22(int nargout, mxArray *out[], int nargin, co
 }
 void gtsamPoint3_StaticFunctionRet_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
+  checkArguments("gtsam::Point3.StaticFunctionRet",nargout,nargin,1);
   double z = unwrap< double >(in[0]);
   out[0] = wrap< Point3 >(gtsam::Point3::StaticFunctionRet(z));
 }
 
 void gtsamPoint3_staticFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0);
+  checkArguments("gtsam::Point3.staticFunction",nargout,nargin,0);
   out[0] = wrap< double >(gtsam::Point3::staticFunction());
 }
 

wrap/tests/expected/matlab/inheritance_wrapper.cpp

@@ -289,7 +289,7 @@ void MyTemplatePoint2_templatedMethod_17(int nargout, mxArray *out[], int nargin
 
 void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("MyTemplatePoint2.Level",nargout,nargin,1);
+  checkArguments("MyTemplate<gtsam::Point2>.Level",nargout,nargin,1);
   Point2 K = unwrap< Point2 >(in[0]);
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Point2>>(MyTemplate<gtsam::Point2>::Level(K)),"MyTemplatePoint2", false);
 }
@@ -457,7 +457,7 @@ void MyTemplateMatrix_templatedMethod_33(int nargout, mxArray *out[], int nargin
 
 void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("MyTemplateMatrix.Level",nargout,nargin,1);
+  checkArguments("MyTemplate<gtsam::Matrix>.Level",nargout,nargin,1);
   Matrix K = unwrap< Matrix >(in[0]);
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }

wrap/tests/expected/matlab/namespaces_wrapper.cpp

@@ -248,7 +248,7 @@ void ns2ClassA_nsReturn_12(int nargout, mxArray *out[], int nargin, const mxArra
 
 void ns2ClassA_afunction_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("ns2ClassA.afunction",nargout,nargin,0);
+  checkArguments("ns2::ClassA.afunction",nargout,nargin,0);
   out[0] = wrap< double >(ns2::ClassA::afunction());
 }
 

wrap/tests/expected/matlab/setPose.m

@@ -0,0 +1,6 @@
+function varargout = setPose(varargin)
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Pose3')
+        functions_wrapper(13, varargin{:});
+      else
+        error('Arguments do not match any overload of function setPose');
+      end

wrap/tests/expected/python/class_pybind.cpp

@@ -31,6 +31,7 @@ PYBIND11_MODULE(class_py, m_) {
     py::class_<Fun<double>, std::shared_ptr<Fun<double>>>(m_, "FunDouble")
         .def("templatedMethodString",[](Fun<double>* self, double d, string t){return self->templatedMethod<string>(d, t);}, py::arg("d"), py::arg("t"))
         .def("multiTemplatedMethodStringSize_t",[](Fun<double>* self, double d, string t, size_t u){return self->multiTemplatedMethod<string,size_t>(d, t, u);}, py::arg("d"), py::arg("t"), py::arg("u"))
+        .def("sets",[](Fun<double>* self){return self->sets();})
         .def_static("staticMethodWithThis",[](){return Fun<double>::staticMethodWithThis();})
         .def_static("templatedStaticMethodInt",[](const int& m){return Fun<double>::templatedStaticMethod<int>(m);}, py::arg("m"));
 

wrap/tests/fixtures/class.i

@@ -18,6 +18,8 @@ class Fun {
 
   template<T={string}, U={size_t}>
   This multiTemplatedMethod(double d, T t, U u);
+
+  std::map<M, This::M> sets();
 };
 
 

wrap/tests/test_matlab_wrapper.py

@@ -92,10 +92,19 @@ class TestWrap(unittest.TestCase):
         wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
-            'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m',
+            'functions_wrapper.cpp',
+            'aGlobalFunction.m',
+            'load2D.m',
             'MultiTemplatedFunctionDoubleSize_tDouble.m',
             'MultiTemplatedFunctionStringSize_tDouble.m',
-            'overloadedGlobalFunction.m', 'TemplatedFunctionRot3.m'
+            'overloadedGlobalFunction.m',
+            'TemplatedFunctionRot3.m',
+            'DefaultFuncInt.m',
+            'DefaultFuncObj.m',
+            'DefaultFuncString.m',
+            'DefaultFuncVector.m',
+            'DefaultFuncZero.m',
+            'setPose.m',
         ]
 
         for file in files:
@@ -115,10 +124,17 @@ class TestWrap(unittest.TestCase):
         wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
-            'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m',
+            'class_wrapper.cpp',
-            'MultipleTemplatesIntDouble.m', 'MultipleTemplatesIntFloat.m',
+            'FunDouble.m',
-            'MyFactorPosePoint2.m', 'MyVector3.m', 'MyVector12.m',
+            'FunRange.m',
-            'PrimitiveRefDouble.m', 'Test.m'
+            'MultipleTemplatesIntDouble.m',
+            'MultipleTemplatesIntFloat.m',
+            'MyFactorPosePoint2.m',
+            'MyVector3.m',
+            'MyVector12.m',
+            'PrimitiveRefDouble.m',
+            'Test.m',
+            'ForwardKinematics.m',
         ]
 
         for file in files:
@@ -137,7 +153,10 @@ class TestWrap(unittest.TestCase):
 
         wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
-        files = ['template_wrapper.cpp']
+        files = [
+            'template_wrapper.cpp', 'ScopedTemplateResult.m',
+            'TemplatedConstructor.m'
+        ]
 
         for file in files:
             actual = osp.join(self.MATLAB_ACTUAL_DIR, file)
@@ -155,8 +174,11 @@ class TestWrap(unittest.TestCase):
         wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
-            'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m',
+            'inheritance_wrapper.cpp',
-            'MyTemplatePoint2.m'
+            'MyBase.m',
+            'MyTemplateMatrix.m',
+            'MyTemplatePoint2.m',
+            'ForwardKinematicsFactor.m',
         ]
 
         for file in files:
@@ -178,10 +200,17 @@ class TestWrap(unittest.TestCase):
         wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR)
 
         files = [
-            'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m',
+            'namespaces_wrapper.cpp',
-            '+ns1/ClassA.m', '+ns1/ClassB.m', '+ns2/+ns3/ClassB.m',
+            '+ns1/aGlobalFunction.m',
-            '+ns2/aGlobalFunction.m', '+ns2/ClassA.m', '+ns2/ClassC.m',
+            '+ns1/ClassA.m',
-            '+ns2/overloadedGlobalFunction.m', 'ClassD.m'
+            '+ns1/ClassB.m',
+            '+ns2/+ns3/ClassB.m',
+            '+ns2/aGlobalFunction.m',
+            '+ns2/ClassA.m',
+            '+ns2/ClassC.m',
+            '+ns2/overloadedGlobalFunction.m',
+            'ClassD.m',
+            '+gtsam/Values.m',
         ]
 
         for file in files:
@@ -203,8 +232,10 @@ class TestWrap(unittest.TestCase):
 
         files = [
             'special_cases_wrapper.cpp',
-            '+gtsam/PinholeCameraCal3Bundler.m',
+            '+gtsam/GeneralSFMFactorCal3Bundler.m',
             '+gtsam/NonlinearFactorGraph.m',
+            '+gtsam/PinholeCameraCal3Bundler.m',
+            '+gtsam/SfmTrack.m',
         ]
 
         for file in files: