Merge branch 'develop' into boost-bind-warn

.github/workflows/build-linux.yml

@@ -57,7 +57,7 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

.github/workflows/build-python.yml

@@ -77,7 +77,7 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

.github/workflows/build-special.yml

@@ -64,7 +64,7 @@ jobs:
         run: |
           # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
           if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
-            gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

examples/CreateSFMExampleData.cpp

@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/slam/dataset.h>
 
 #include <boost/assign/std/vector.hpp>
 
@@ -26,22 +26,16 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 
-void createExampleBALFile(const string& filename, const vector<Point3>& P,
+void createExampleBALFile(const string& filename, const vector<Point3>& points,
-    const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
+                          const Pose3& pose1, const Pose3& pose2,
-        Cal3Bundler()) {
+                          const Cal3Bundler& K = Cal3Bundler()) {
-
   // Class that will gather all data
   SfmData data;
-
+  // Create two cameras and add them to data
-  // Create two cameras
-  Rot3 aRb = Rot3::Yaw(M_PI_2);
-  Point3 aTb(0.1, 0, 0);
-  Pose3 identity, aPb(aRb, aTb);
   data.cameras.push_back(SfmCamera(pose1, K));
   data.cameras.push_back(SfmCamera(pose2, K));
 
-  for(const Point3& p: P) {
+  for (const Point3& p : points) {
-
     // Create the track
     SfmTrack track;
     track.p = p;
@@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 /* ************************************************************************* */
 
 void create5PointExample1() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
+  vector<Point3> points = {
-  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
+      {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
-  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample1.txt";
+  const string filename = "../../examples/Data/5pointExample1.txt";
-  createExampleBALFile(filename, P, pose1, pose2);
+  createExampleBALFile(filename, points, pose1, pose2);
 }
 
 /* ************************************************************************* */
 
 void create5PointExample2() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
+  vector<Point3> points = {{0, 0, 100},  {-10, 0, 100}, {10, 0, 100},  //
-  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
+                           {0, 50, 50},  {0, -50, 50},  {-20, 0, 80},  //
-  Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
+                           {20, -50, 80}};
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample2.txt";
+  const string filename = "../../examples/Data/5pointExample2.txt";
   Cal3Bundler K(500, 0, 0);
-  createExampleBALFile(filename, P, pose1, pose2,K);
+  createExampleBALFile(filename, points, pose1, pose2, K);
 }
 
 /* ************************************************************************* */
 
+void create18PointExample1() {
+  // Create two cameras poses
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
+  Point3 aTb(0.1, 0, 0);
+  Pose3 pose1, pose2(aRb, aTb);
+
+  // Create test data, we need 15 points
+  vector<Point3> points = {
+      {0, 0, 1},         {-0.1, 0, 1},      {0.1, 0, 1},       //
+      {0, 0.5, 0.5},     {0, -0.5, 0.5},    {-1, -0.5, 2},     //
+      {-1, 0.5, 2},      {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5},  //
+      {-0.1, -0.5, 0.5}, {0.1, -0.5, 1},    {0.1, 0.5, 1},     //
+      {-0.1, 0, 0.5},    {-0.1, 0.5, 0.5},  {0, 0, 0.5},       //
+      {0.1, -0.5, 0.5},  {0.1, 0, 0.5},     {0.1, 0.5, 0.5}};
+
+  // Assumes example is run in ${GTSAM_TOP}/build/examples
+  const string filename = "../../examples/Data/18pointExample1.txt";
+  createExampleBALFile(filename, points, pose1, pose2);
+}
+
 int main(int argc, char* argv[]) {
   create5PointExample1();
   create5PointExample2();
+  create18PointExample1();
   return 0;
 }
 
 /* ************************************************************************* */
-

examples/Data/18pointExample1.txt

@@ -0,0 +1,131 @@
+2 18 36
+
+0 0 0 -0
+1 0 -6.123233995736766344e-18 -0.10000000000000000555
+0 1 -0.10000000000000000555 -0
+1 1 -1.2246467991473532688e-17 -0.2000000000000000111
+0 2 0.10000000000000000555 -0
+1 2 0 -0
+0 3 0 -1
+1 3 1 -0.20000000000000006661
+0 4 0 1
+1 4 -1 -0.19999999999999995559
+0 5 -0.5 0.25
+1 5 -0.25000000000000005551 -0.55000000000000004441
+0 6 -0.5 -0.25
+1 6 0.24999999999999997224 -0.55000000000000004441
+0 7 0.16666666666666665741 0.33333333333333331483
+1 7 -0.33333333333333331483 0.10000000000000000555
+0 8 0.16666666666666665741 -0.33333333333333331483
+1 8 0.33333333333333331483 0.099999999999999977796
+0 9 -0.2000000000000000111 1
+1 9 -1 -0.39999999999999996669
+0 10 0.10000000000000000555 0.5
+1 10 -0.5 3.0616169978683830179e-17
+0 11 0.10000000000000000555 -0.5
+1 11 0.5 -3.0616169978683830179e-17
+0 12 -0.2000000000000000111 -0
+1 12 -2.4492935982947065376e-17 -0.4000000000000000222
+0 13 -0.2000000000000000111 -1
+1 13 1 -0.40000000000000007772
+0 14 0 -0
+1 14 -1.2246467991473532688e-17 -0.2000000000000000111
+0 15 0.2000000000000000111 1
+1 15 -1 6.1232339957367660359e-17
+0 16 0.2000000000000000111 -0
+1 16 0 -0
+0 17 0.2000000000000000111 -1
+1 17 1 -6.1232339957367660359e-17
+
+3.141592653589793116
+                   0
+                   0
+-0
+0
+0
+1
+0
+0
+
+2.2214414690791830509
+2.2214414690791826068
+                    0
+-6.123233995736766344e-18
+-0.10000000000000000555
+0
+1
+0
+0
+
+0
+0
+1
+
+-0.10000000000000000555
+0
+1
+
+0.10000000000000000555
+0
+1
+
+0
+0.5
+0.5
+
+0
+-0.5
+0.5
+
+-1
+-0.5
+2
+
+-1
+0.5
+2
+
+0.25
+-0.5
+1.5
+
+0.25
+0.5
+1.5
+
+-0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+-0.5
+1
+
+0.10000000000000000555
+0.5
+1
+
+-0.10000000000000000555
+0
+0.5
+
+-0.10000000000000000555
+0.5
+0.5
+
+0
+0
+0.5
+
+0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+0
+0.5
+
+0.10000000000000000555
+0.5
+0.5
+

gtsam/base/treeTraversal-inst.h

@@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
   // Typedefs
   typedef typename FOREST::Node Node;
 
-  tbb::task::spawn_root_and_wait(
   internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
-          visitorPost, problemSizeThreshold));
+      visitorPost, problemSizeThreshold);
 #else
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 #endif

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -22,7 +22,7 @@
 #include <boost/make_shared.hpp>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/task.h>               // tbb::task, tbb::task_list
+#include <tbb/task_group.h>         // tbb::task_group
 #include <tbb/scalable_allocator.h> // tbb::scalable_allocator
 
 namespace gtsam {
@@ -34,7 +34,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class PreOrderTask : public tbb::task
+      class PreOrderTask
       {
       public:
         const boost::shared_ptr<NODE>& treeNode;
@@ -42,28 +42,30 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
+        tbb::task_group& tg;
         bool makeNewTasks;
 
-        bool isPostOrderPhase;
+        // Keep track of order phase across multiple calls to the same functor
+        mutable bool isPostOrderPhase;
 
         PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
                      VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
-                     bool makeNewTasks = true)
+                     tbb::task_group& tg, bool makeNewTasks = true)
             : treeNode(treeNode),
               myData(myData),
               visitorPre(visitorPre),
               visitorPost(visitorPost),
               problemSizeThreshold(problemSizeThreshold),
+              tg(tg),
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        tbb::task* execute() override
+        void operator()() const
         {
           if(isPostOrderPhase)
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
-            return nullptr;
           }
           else
           {
@@ -71,14 +73,10 @@ namespace gtsam {
             {
               if(!treeNode->children.empty())
               {
-                // Allocate post-order task as a continuation
-                isPostOrderPhase = true;
-                recycle_as_continuation();
-
                 bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
 
-                tbb::task* firstChild = 0;
+                // If we have child tasks, start subtasks and wait for them to complete
-                tbb::task_list childTasks;
+                tbb::task_group ctg;
                 for(const boost::shared_ptr<NODE>& child: treeNode->children)
                 {
                   // Process child in a subtask.  Important:  Run visitorPre before calling
@@ -86,37 +84,30 @@ namespace gtsam {
                   // allocated an extra child, this causes a TBB error.
                   boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
                       tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
-                  tbb::task* childTask =
+                  ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
-                      new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost,
+                      problemSizeThreshold, ctg, overThreshold));
-                                                          problemSizeThreshold, overThreshold);
-                  if (firstChild)
-                    childTasks.push_back(*childTask);
-                  else
-                    firstChild = childTask;
                 }
+                ctg.wait();
 
-                // If we have child tasks, start subtasks and wait for them to complete
+                // Allocate post-order task as a continuation
-                set_ref_count((int)treeNode->children.size());
+                isPostOrderPhase = true;
-                spawn(childTasks);
+                tg.run(*this);
-                return firstChild;
               }
               else
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
-                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
-              return nullptr;
             }
           }
         }
 
-        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
+        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) const
         {
           for(const boost::shared_ptr<NODE>& child: node->children)
           {
@@ -131,7 +122,7 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class RootTask : public tbb::task
+      class RootTask
       {
       public:
         const ROOTS& roots;
@@ -139,37 +130,30 @@ namespace gtsam {
         VISITOR_PRE& visitorPre;
         VISITOR_POST& visitorPost;
         int problemSizeThreshold;
+        tbb::task_group& tg;
         RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-          int problemSizeThreshold) :
+          int problemSizeThreshold, tbb::task_group& tg) :
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-          problemSizeThreshold(problemSizeThreshold) {}
+          problemSizeThreshold(problemSizeThreshold), tg(tg) {}
 
-        tbb::task* execute() override
+        void operator()() const
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children
-          tbb::task_list tasks;
           for(const boost::shared_ptr<NODE>& root: roots)
           {
             boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
-            tasks.push_back(*new(allocate_child())
+            tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
-              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
           }
-          // Set TBB ref count
-          set_ref_count(1 + (int) roots.size());
-          // Spawn tasks
-          spawn_and_wait_for_all(tasks);
-          // Return nullptr
-          return nullptr;
         }
       };
 
       template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
+      void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
-        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
       {
           typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
-          return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
+          tbb::task_group tg;
+          tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
       }
 
     }

gtsam/gtsam.i

@@ -2257,6 +2257,7 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
+  void insert(size_t j, double c);
 
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
@@ -2278,13 +2279,31 @@ class Values {
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
+  void update(size_t j, double c);
 
-  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
+  template <T = {gtsam::Point2,
+                 gtsam::Point3,
+                 gtsam::Rot2,
+                 gtsam::Pose2,
+                 gtsam::SO3,
+                 gtsam::SO4,
+                 gtsam::SOn,
+                 gtsam::Rot3,
+                 gtsam::Pose3,
+                 gtsam::Unit3,
+                 gtsam::Cal3_S2,
+                 gtsam::Cal3DS2,
+                 gtsam::Cal3Bundler,
+                 gtsam::EssentialMatrix,
+                 gtsam::PinholeCameraCal3_S2,
+                 gtsam::PinholeCamera<gtsam::Cal3Bundler>,
+                 gtsam::imuBias::ConstantBias,
+                 gtsam::NavState,
+                 Vector,
+                 Matrix,
+                 double}>
   T at(size_t j);
 
-  /// version for double
-  void insertDouble(size_t j, double c);
-  double atDouble(size_t j) const;
 };
 
 #include <gtsam/nonlinear/Marginals.h>

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -354,7 +354,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
     _LinearizeOneFactor(*this, linearizationPoint, *linearFG));
 
   // Linearize all non-sendable factors
-  for(int i = 0; i < size(); i++) {
+  for(size_t i = 0; i < size(); i++) {
     auto& factor = (*this)[i];
     if(factor && !(factor->sendable())) {
       (*linearFG)[i] = factor->linearize(linearizationPoint);

gtsam/slam/EssentialMatrixFactor.h

@@ -7,9 +7,10 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
 #include <iostream>
 
 namespace gtsam {
@@ -17,25 +18,24 @@ namespace gtsam {
 /**
  * Factor that evaluates epipolar error p'Ep for given essential matrix
  */
-class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
+class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
-
   Vector3 vA_, vB_;  ///< Homogeneous versions, in ideal coordinates
 
   typedef NoiseModelFactor1<EssentialMatrix> Base;
   typedef EssentialMatrixFactor This;
 
-public:
+ public:
-
   /**
    *  Constructor
    *  @param key Essential Matrix variable key
    *  @param pA point in first camera, in calibrated coordinates
    *  @param pB point in second camera, in calibrated coordinates
-   *  @param model noise model is about dot product in ideal, homogeneous coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous
+   * coordinates
    */
   EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
-      const SharedNoiseModel& model) :
+                        const SharedNoiseModel& model)
-      Base(model, key) {
+      : Base(model, key) {
     vA_ = EssentialMatrix::Homogeneous(pA);
     vB_ = EssentialMatrix::Homogeneous(pB);
   }
@@ -45,13 +45,15 @@ public:
    *  @param key Essential Matrix variable key
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
-   *  @param model noise model is about dot product in ideal, homogeneous coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous
+   * coordinates
    *  @param K calibration object, will be used only in constructor
    */
-  template<class CALIBRATION>
+  template <class CALIBRATION>
   EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
-      const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+                        const SharedNoiseModel& model,
-      Base(model, key) {
+                        boost::shared_ptr<CALIBRATION> K)
+      : Base(model, key) {
     assert(K);
     vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
     vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
@@ -64,7 +66,8 @@ public:
   }
 
   /// print
-  void print(const std::string& s = "",
+  void print(
+      const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor with measurements\n  ("
@@ -73,14 +76,15 @@ public:
   }
 
   /// vector of errors returns 1D vector
-  Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
+  Vector evaluateError(
-      boost::none) const override {
+      const EssentialMatrix& E,
+      boost::optional<Matrix&> H = boost::none) const override {
     Vector error(1);
     error << E.error(vA_, vB_, H);
     return error;
   }
 
-public:
+ public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
@@ -88,8 +92,8 @@ public:
  * Binary factor that optimizes for E and inverse depth d: assumes measurement
  * in image 2 is perfect, and returns re-projection error in image 1
  */
-class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
+class EssentialMatrixFactor2
-
+    : public NoiseModelFactor2<EssentialMatrix, double> {
   Point3 dP1_;  ///< 3D point corresponding to measurement in image 1
   Point2 pn_;   ///< Measurement in image 2, in ideal coordinates
   double f_;    ///< approximate conversion factor for error scaling
@@ -97,8 +101,7 @@ class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double>
   typedef NoiseModelFactor2<EssentialMatrix, double> Base;
   typedef EssentialMatrixFactor2 This;
 
-public:
+ public:
-
   /**
    *  Constructor
    *  @param key1 Essential Matrix variable key
@@ -108,8 +111,10 @@ public:
    *  @param model noise model should be in pixels, as well
    */
   EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const SharedNoiseModel& model) :
+                         const SharedNoiseModel& model)
-      Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) {
+      : Base(model, key1, key2),
+        dP1_(EssentialMatrix::Homogeneous(pA)),
+        pn_(pB) {
     f_ = 1.0;
   }
 
@@ -122,11 +127,13 @@ public:
    *  @param K calibration object, will be used only in constructor
    *  @param model noise model should be in pixels, as well
    */
-  template<class CALIBRATION>
+  template <class CALIBRATION>
   EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+                         const SharedNoiseModel& model,
-      Base(model, key1, key2), dP1_(
+                         boost::shared_ptr<CALIBRATION> K)
-          EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) {
+      : Base(model, key1, key2),
+        dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
+        pn_(K->calibrate(pB)) {
     f_ = 0.5 * (K->fx() + K->fy());
   }
 
@@ -137,12 +144,13 @@ public:
   }
 
   /// print
-  void print(const std::string& s = "",
+  void print(
+      const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
-        << dP1_.transpose() << ")' and (" << pn_.transpose()
+              << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
-        << ")'" << std::endl;
+              << std::endl;
   }
 
   /*
@@ -150,30 +158,28 @@ public:
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(const EssentialMatrix& E, const double& d,
+  Vector evaluateError(
-      boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
+      const EssentialMatrix& E, const double& d,
-          boost::none) const override {
+      boost::optional<Matrix&> DE = boost::none,
-
+      boost::optional<Matrix&> Dd = boost::none) const override {
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
     // We then convert to second camera by P2 = 1R2'*(P1-1T2)
     // The homogeneous coordinates of can be written as
     //   2R1*(P1-1T2) ==  2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
-    // where we multiplied with d which yields equivalent homogeneous coordinates.
+    // where we multiplied with d which yields equivalent homogeneous
-    // Note that this is just the homography 2R1 for d==0
+    // coordinates. Note that this is just the homography 2R1 for d==0 The point
-    // The point d*P1 = (x,y,1) is computed in constructor as dP1_
+    // d*P1 = (x,y,1) is computed in constructor as dP1_
 
     // Project to normalized image coordinates, then uncalibrate
-    Point2 pn(0,0);
+    Point2 pn(0, 0);
     if (!DE && !Dd) {
-
       Point3 _1T2 = E.direction().point3();
       Point3 d1T2 = d * _1T2;
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);  // 2R1*((x,y,1)-d*1T2)
       pn = PinholeBase::Project(dP2);
 
     } else {
-
       // Calculate derivatives. TODO if slow: optimize with Mathematica
       //     3*2        3*3       3*3
       Matrix D_1T2_dir, DdP2_rot, DP2_point;
@@ -194,13 +200,12 @@ public:
       if (Dd)  // efficient backwards computation:
         //      (2*3)    * (3*3)      * (3*1)
         *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
-
     }
     Point2 reprojectionError = pn - pn_;
     return f_ * reprojectionError;
   }
 
-public:
+ public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor2
@@ -210,15 +215,13 @@ public:
  * in image 2 is perfect, and returns re-projection error in image 1
  * This version takes an extrinsic rotation to allow for omni-directional rigs
  */
-class EssentialMatrixFactor3: public EssentialMatrixFactor2 {
+class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
-
   typedef EssentialMatrixFactor2 Base;
   typedef EssentialMatrixFactor3 This;
 
   Rot3 cRb_;  ///< Rotation from body to camera frame
 
-public:
+ public:
-
   /**
    *  Constructor
    *  @param key1 Essential Matrix variable key
@@ -229,9 +232,8 @@ public:
    *  @param model noise model should be in calibrated coordinates, as well
    */
   EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const Rot3& cRb, const SharedNoiseModel& model) :
+                         const Rot3& cRb, const SharedNoiseModel& model)
-      EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {
+      : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
-  }
 
   /**
    *  Constructor
@@ -242,12 +244,11 @@ public:
    *  @param K calibration object, will be used only in constructor
    *  @param model noise model should be in pixels, as well
    */
-  template<class CALIBRATION>
+  template <class CALIBRATION>
   EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
                          const Rot3& cRb, const SharedNoiseModel& model,
-      boost::shared_ptr<CALIBRATION> K) :
+                         boost::shared_ptr<CALIBRATION> K)
-      EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
+      : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
-  }
 
   /// @return a deep copy of this factor
   gtsam::NonlinearFactor::shared_ptr clone() const override {
@@ -256,7 +257,8 @@ public:
   }
 
   /// print
-  void print(const std::string& s = "",
+  void print(
+      const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
@@ -267,9 +269,10 @@ public:
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(const EssentialMatrix& E, const double& d,
+  Vector evaluateError(
-      boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
+      const EssentialMatrix& E, const double& d,
-          boost::none) const override {
+      boost::optional<Matrix&> DE = boost::none,
+      boost::optional<Matrix&> Dd = boost::none) const override {
     if (!DE) {
       // Convert E from body to camera frame
       EssentialMatrix cameraE = cRb_ * E;
@@ -285,10 +288,109 @@ public:
     }
   }
 
-public:
+ public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // EssentialMatrixFactor3
 
-}// gtsam
+/**
+ * Binary factor that optimizes for E and calibration K using the algebraic
+ * epipolar error (K^-1 pA)'E (K^-1 pB). The calibration is shared between two
+ * images.
+ *
+ * Note: As correspondences between 2d coordinates can only recover 7 DoF,
+ * this factor should always be used with a prior factor on calibration.
+ * Even with a prior, we can only optimize 2 DoF in the calibration. So the
+ * prior should have a noise model with very low sigma in the remaining
+ * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it
+ * works only with a strong prior (low sigma noisemodel) on all degrees of
+ * freedom.
+ */
+template <class CALIBRATION>
+class EssentialMatrixFactor4
+    : public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
+ private:
+  Point2 pA_, pB_;  ///< points in pixel coordinates
 
+  typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
+  typedef EssentialMatrixFactor4 This;
+
+  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
+  typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
+
+ public:
+  /**
+   *  Constructor
+   *  @param keyE Essential Matrix (from camera B to A) variable key
+   *  @param keyK Calibration variable key (common for both cameras)
+   *  @param pA point in first camera, in pixel coordinates
+   *  @param pB point in second camera, in pixel coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous
+   * coordinates
+   */
+  EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
+                         const SharedNoiseModel& model)
+      : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    Base::print(s);
+    std::cout << "  EssentialMatrixFactor4 with measurements\n  ("
+              << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
+              << std::endl;
+  }
+
+  /**
+   * @brief Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
+   *
+   * @param E essential matrix for key keyE
+   * @param K calibration (common for both images) for key keyK
+   * @param H1 optional jacobian of error w.r.t E
+   * @param H2 optional jacobian of error w.r.t K
+   * @return * Vector 1D vector of algebraic error
+   */
+  Vector evaluateError(
+      const EssentialMatrix& E, const CALIBRATION& K,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    // converting from pixel coordinates to normalized coordinates cA and cB
+    JacobianCalibration cA_H_K;  // dcA/dK
+    JacobianCalibration cB_H_K;  // dcB/dK
+    Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
+    Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
+
+    // convert to homogeneous coordinates
+    Vector3 vA = EssentialMatrix::Homogeneous(cA);
+    Vector3 vB = EssentialMatrix::Homogeneous(cB);
+
+    if (H2) {
+      // compute the jacobian of error w.r.t K
+
+      // error function f = vA.T * E * vB
+      // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
+      // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
+      // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
+      *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
+            vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
+    }
+
+    Vector error(1);
+    error << E.error(vA, vB, H1);
+
+    return error;
+  }
+
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+};
+// EssentialMatrixFactor4
+
+}  // namespace gtsam

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -5,28 +5,27 @@
  * @date December 17, 2013
  */
 
-#include <gtsam/slam/EssentialMatrixFactor.h>
+#include <CppUnitLite/TestHarness.h>
-
-#include <gtsam/slam/dataset.h>
-#include <gtsam/nonlinear/expressionTesting.h>
-#include <gtsam/nonlinear/ExpressionFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/CalibratedCamera.h>
-#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/slam/EssentialMatrixFactor.h>
+#include <gtsam/slam/dataset.h>
 
 #include <boost/bind/bind.hpp>
-#include <CppUnitLite/TestHarness.h>
 
 using namespace boost::placeholders;
 using namespace std;
 using namespace gtsam;
 
 // Noise model for first type of factor is evaluating algebraic error
-noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1,
+noiseModel::Isotropic::shared_ptr model1 =
-    0.01);
+    noiseModel::Isotropic::Sigma(1, 0.01);
 // Noise model for second type of factor is evaluating pixel coordinates
 noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
 
@@ -36,39 +35,33 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
 
 namespace example1 {
 
-const string filename = findExampleDataFile("5pointExample1.txt");
+const string filename = findExampleDataFile("18pointExample1.txt");
 SfmData data;
 bool readOK = readBAL(filename, data);
 Rot3 c1Rc2 = data.cameras[1].pose().rotation();
 Point3 c1Tc2 = data.cameras[1].pose().translation();
-PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
+// TODO: maybe default value not good; assert with 0th
+Cal3_S2 trueK = Cal3_S2();
+PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), trueK);
 Rot3 trueRotation(c1Rc2);
 Unit3 trueDirection(c1Tc2);
 EssentialMatrix trueE(trueRotation, trueDirection);
 double baseline = 0.1;  // actual baseline of the camera
 
-Point2 pA(size_t i) {
+Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
-  return data.tracks[i].measurements[0].second;
+Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; }
-}
+Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); }
-Point2 pB(size_t i) {
+Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); }
-  return data.tracks[i].measurements[1].second;
-}
-Vector vA(size_t i) {
-  return EssentialMatrix::Homogeneous(pA(i));
-}
-Vector vB(size_t i) {
-  return EssentialMatrix::Homogeneous(pB(i));
-}
 
 //*************************************************************************
-TEST (EssentialMatrixFactor, testData) {
+TEST(EssentialMatrixFactor, testData) {
   CHECK(readOK);
 
   // Check E matrix
   Matrix expected(3, 3);
   expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
-  Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
+  Matrix aEb_matrix =
-      * c1Rc2.matrix();
+      skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix();
   EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
 
   // Check some projections
@@ -90,7 +83,7 @@ TEST (EssentialMatrixFactor, testData) {
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor, factor) {
+TEST(EssentialMatrixFactor, factor) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
     EssentialMatrixFactor factor(key, pA(i), pB(i), model1);
@@ -98,19 +91,12 @@ TEST (EssentialMatrixFactor, factor) {
     // Check evaluation
     Vector expected(1);
     expected << 0;
-    Matrix Hactual;
+    Vector actual = factor.evaluateError(trueE);
-    Vector actual = factor.evaluateError(trueE, Hactual);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
-    // Use numerical derivatives to calculate the expected Jacobian
+    Values val;
-    Matrix Hexpected;
+    val.insert(key, trueE);
-    typedef Eigen::Matrix<double,1,1> Vector1;
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
-    Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
-        boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
-            boost::none), trueE);
-
-    // Verify the Jacobian is correct
-    EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
   }
 }
 
@@ -118,7 +104,7 @@ TEST (EssentialMatrixFactor, factor) {
 TEST(EssentialMatrixFactor, ExpressionFactor) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
-    boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f =
+    boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
         boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
     Expression<EssentialMatrix> E_(key);  // leaf expression
     Expression<double> expr(f, E_);       // unary expression
@@ -144,13 +130,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
 TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
   Key key(1);
   for (size_t i = 0; i < 5; i++) {
-    boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f =
+    boost::function<double(const EssentialMatrix &, OptionalJacobian<1, 5>)> f =
         boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
-    boost::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>,
+    boost::function<EssentialMatrix(const Rot3 &, const Unit3 &,
-                                    OptionalJacobian<5, 2>)> g;
+                                    OptionalJacobian<5, 3>,
+                                    OptionalJacobian<5, 2>)>
+        g;
     Expression<Rot3> R_(key);
     Expression<Unit3> d_(trueDirection);
-    Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection, R_, d_);
+    Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection,
+                                   R_, d_);
     Expression<double> expr(f, E_);
 
     // Test the derivatives using Paul's magic
@@ -171,7 +160,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor, minimization) {
+TEST(EssentialMatrixFactor, minimization) {
   // Here we want to optimize directly on essential matrix constraints
   // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
   // but GTSAM does the equivalent anyway, provided we give the right
@@ -190,8 +179,8 @@ TEST (EssentialMatrixFactor, minimization) {
 
   // Check error at initial estimate
   Values initial;
-  EssentialMatrix initialE = trueE.retract(
+  EssentialMatrix initialE =
-      (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
+      trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
   initial.insert(1, initialE);
 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
   EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
@@ -214,11 +203,10 @@ TEST (EssentialMatrixFactor, minimization) {
   // Check errors individually
   for (size_t i = 0; i < 5; i++)
     EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
-
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor2, factor) {
+TEST(EssentialMatrixFactor2, factor) {
   for (size_t i = 0; i < 5; i++) {
     EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
 
@@ -232,22 +220,15 @@ TEST (EssentialMatrixFactor2, factor) {
     Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
-    // Use numerical derivatives to calculate the expected Jacobian
+    Values val;
-    Matrix Hexpected1, Hexpected2;
+    val.insert(100, trueE);
-    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+    val.insert(i, d);
-        &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7);
-        boost::none);
-    Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
-    Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
-
-    // Verify the Jacobian is correct
-    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
-    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
   }
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor2, minimization) {
+TEST(EssentialMatrixFactor2, minimization) {
   // Here we want to optimize for E and inverse depths at the same time
 
   // We start with a factor graph and add constraints to it
@@ -290,8 +271,7 @@ TEST (EssentialMatrixFactor2, minimization) {
 EssentialMatrix bodyE = cRb.inverse() * trueE;
 
 //*************************************************************************
-TEST (EssentialMatrixFactor3, factor) {
+TEST(EssentialMatrixFactor3, factor) {
-
   for (size_t i = 0; i < 5; i++) {
     EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
 
@@ -305,28 +285,21 @@ TEST (EssentialMatrixFactor3, factor) {
     Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
-    // Use numerical derivatives to calculate the expected Jacobian
+    Values val;
-    Matrix Hexpected1, Hexpected2;
+    val.insert(100, bodyE);
-    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+    val.insert(i, d);
-        &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7);
-        boost::none);
-    Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
-    Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
-
-    // Verify the Jacobian is correct
-    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
-    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
   }
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor3, minimization) {
+TEST(EssentialMatrixFactor3, minimization) {
-
   // As before, we start with a factor graph and add constraints to it
   NonlinearFactorGraph graph;
   for (size_t i = 0; i < 5; i++)
     // but now we specify the rotation bRc
-    graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb, model2);
+    graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb,
+                                                 model2);
 
   // Check error at ground truth
   Values truth;
@@ -353,6 +326,213 @@ TEST (EssentialMatrixFactor3, minimization) {
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
+//*************************************************************************
+TEST(EssentialMatrixFactor4, factor) {
+  Key keyE(1);
+  Key keyK(2);
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor4<Cal3_S2> factor(keyE, keyK, pA(i), pB(i), model1);
+
+    // Check evaluation
+    Vector1 expected;
+    expected << 0;
+    Vector actual = factor.evaluateError(trueE, trueK);
+    EXPECT(assert_equal(expected, actual, 1e-7));
+
+    Values truth;
+    truth.insert(keyE, trueE);
+    truth.insert(keyK, trueK);
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
+  }
+}
+
+//*************************************************************************
+TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) {
+  Key keyE(1);
+  Key keyK(2);
+  // initialize essential matrix
+  Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9));
+  Unit3 t(Point3(2, -1, 0.5));
+  EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t);
+  Cal3_S2 K(200, 1, 1, 10, 10);
+  Values val;
+  val.insert(keyE, E);
+  val.insert(keyK, K);
+
+  Point2 pA(10.0, 20.0);
+  Point2 pB(12.0, 15.0);
+
+  EssentialMatrixFactor4<Cal3_S2> f(keyE, keyK, pA, pB, model1);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6);
+}
+
+//*************************************************************************
+TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) {
+  Key keyE(1);
+  Key keyK(2);
+  // initialize essential matrix
+  Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2));
+  Unit3 t(Point3(0.1, 0, 0));
+  EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t);
+  Cal3Bundler K;
+  Values val;
+  val.insert(keyE, E);
+  val.insert(keyK, K);
+
+  Point2 pA(-0.1, 0.5);
+  Point2 pB(-0.5, -0.2);
+
+  EssentialMatrixFactor4<Cal3Bundler> f(keyE, keyK, pA, pB, model1);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
+}
+
+//*************************************************************************
+TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) {
+  NonlinearFactorGraph graph;
+  for (size_t i = 0; i < 5; i++)
+    graph.emplace_shared<EssentialMatrixFactor4<Cal3_S2>>(1, 2, pA(i), pB(i),
+                                                          model1);
+
+  // Check error at ground truth
+  Values truth;
+  truth.insert(1, trueE);
+  truth.insert(2, trueK);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Initialization
+  Values initial;
+  EssentialMatrix initialE =
+      trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
+  initial.insert(1, initialE);
+  initial.insert(2, trueK);
+
+  // add prior factor for calibration
+  Vector5 priorNoiseModelSigma;
+  priorNoiseModelSigma << 10, 10, 10, 10, 10;
+  graph.emplace_shared<PriorFactor<Cal3_S2>>(
+      2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
+
+  LevenbergMarquardtOptimizer optimizer(graph, initial);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
+  Cal3_S2 actualK = result.at<Cal3_S2>(2);
+  EXPECT(assert_equal(trueE, actualE, 1e-1));
+  EXPECT(assert_equal(trueK, actualK, 1e-2));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+
+  // Check errors individually
+  for (size_t i = 0; i < 5; i++)
+    EXPECT_DOUBLES_EQUAL(
+        0,
+        actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
+                      EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
+        1e-6);
+}
+
+//*************************************************************************
+TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) {
+  // We need 7 points here as the prior on the focal length parameters is weak
+  // and the initialization is noisy. So in total we are trying to optimize 7
+  // DOF, with a strong prior on the remaining 3 DOF.
+  NonlinearFactorGraph graph;
+  for (size_t i = 0; i < 7; i++)
+    graph.emplace_shared<EssentialMatrixFactor4<Cal3_S2>>(1, 2, pA(i), pB(i),
+                                                          model1);
+
+  // Check error at ground truth
+  Values truth;
+  truth.insert(1, trueE);
+  truth.insert(2, trueK);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Initialization
+  Values initial;
+  EssentialMatrix initialE =
+      trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
+  Cal3_S2 initialK =
+      trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished());
+  initial.insert(1, initialE);
+  initial.insert(2, initialK);
+
+  // add prior factor for calibration
+  Vector5 priorNoiseModelSigma;
+  priorNoiseModelSigma << 20, 20, 1, 1, 1;
+  graph.emplace_shared<PriorFactor<Cal3_S2>>(
+      2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
+
+  LevenbergMarquardtOptimizer optimizer(graph, initial);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
+  Cal3_S2 actualK = result.at<Cal3_S2>(2);
+  EXPECT(assert_equal(trueE, actualE, 1e-1));
+  EXPECT(assert_equal(trueK, actualK, 1e-2));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+
+  // Check errors individually
+  for (size_t i = 0; i < 7; i++)
+    EXPECT_DOUBLES_EQUAL(
+        0,
+        actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
+                      EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
+        1e-5);
+}
+
+//*************************************************************************
+TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
+  NonlinearFactorGraph graph;
+  for (size_t i = 0; i < 5; i++)
+    graph.emplace_shared<EssentialMatrixFactor4<Cal3Bundler>>(1, 2, pA(i),
+                                                              pB(i), model1);
+  Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3);
+  // Check error at ground truth
+  Values truth;
+  truth.insert(1, trueE);
+  truth.insert(2, trueK);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Check error at initial estimate
+  Values initial;
+  EssentialMatrix initialE =
+      trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
+  Cal3Bundler initialK = trueK;
+  initial.insert(1, initialE);
+  initial.insert(2, initialK);
+
+  // add prior factor for calibration
+  Vector3 priorNoiseModelSigma;
+  priorNoiseModelSigma << 0.1, 0.1, 0.1;
+  graph.emplace_shared<PriorFactor<Cal3Bundler>>(
+      2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
+
+  LevenbergMarquardtOptimizer optimizer(graph, initial);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actualE = result.at<EssentialMatrix>(1);
+  Cal3Bundler actualK = result.at<Cal3Bundler>(2);
+  EXPECT(assert_equal(trueE, actualE, 1e-1));
+  EXPECT(assert_equal(trueK, actualK, 1e-2));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+
+  // Check errors individually
+  for (size_t i = 0; i < 5; i++)
+    EXPECT_DOUBLES_EQUAL(
+        0,
+        actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
+                      EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
+        1e-6);
+}
+
 }  // namespace example1
 
 //*************************************************************************
@@ -368,28 +548,24 @@ EssentialMatrix trueE(aRb, Unit3(aTb));
 
 double baseline = 10;  // actual baseline of the camera
 
-Point2 pA(size_t i) {
+Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
-  return data.tracks[i].measurements[0].second;
+Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; }
-}
-Point2 pB(size_t i) {
-  return data.tracks[i].measurements[1].second;
-}
 
-boost::shared_ptr<Cal3Bundler> //
+Cal3Bundler trueK = Cal3Bundler(500, 0, 0);
-K = boost::make_shared<Cal3Bundler>(500, 0, 0);
+boost::shared_ptr<Cal3Bundler> K = boost::make_shared<Cal3Bundler>(trueK);
-PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K);
+PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), trueK);
 
 Vector vA(size_t i) {
-  Point2 xy = K->calibrate(pA(i));
+  Point2 xy = trueK.calibrate(pA(i));
   return EssentialMatrix::Homogeneous(xy);
 }
 Vector vB(size_t i) {
-  Point2 xy = K->calibrate(pB(i));
+  Point2 xy = trueK.calibrate(pB(i));
   return EssentialMatrix::Homogeneous(xy);
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor, extraMinimization) {
+TEST(EssentialMatrixFactor, extraMinimization) {
   // Additional test with camera moving in positive X direction
 
   NonlinearFactorGraph graph;
@@ -403,8 +579,8 @@ TEST (EssentialMatrixFactor, extraMinimization) {
 
   // Check error at initial estimate
   Values initial;
-  EssentialMatrix initialE = trueE.retract(
+  EssentialMatrix initialE =
-      (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
+      trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
   initial.insert(1, initialE);
 
 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
@@ -428,11 +604,10 @@ TEST (EssentialMatrixFactor, extraMinimization) {
   // Check errors individually
   for (size_t i = 0; i < 5; i++)
     EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
-
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor2, extraTest) {
+TEST(EssentialMatrixFactor2, extraTest) {
   for (size_t i = 0; i < 5; i++) {
     EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K);
 
@@ -441,34 +616,27 @@ TEST (EssentialMatrixFactor2, extraTest) {
     const Point2 pi = camera2.project(P1);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(trueE, d);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
-    // Use numerical derivatives to calculate the expected Jacobian
+    Values val;
-    Matrix Hexpected1, Hexpected2;
+    val.insert(100, trueE);
-    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+    val.insert(i, d);
-        &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
-        boost::none);
-    Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
-    Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
-
-    // Verify the Jacobian is correct
-    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
-    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
   }
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor2, extraMinimization) {
+TEST(EssentialMatrixFactor2, extraMinimization) {
   // Additional test with camera moving in positive X direction
 
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1, assuming pixel measurements
   NonlinearFactorGraph graph;
   for (size_t i = 0; i < data.number_tracks(); i++)
-    graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2, K);
+    graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
+                                                 K);
 
   // Check error at ground truth
   Values truth;
@@ -496,8 +664,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor3, extraTest) {
+TEST(EssentialMatrixFactor3, extraTest) {
-
   // The "true E" in the body frame is
   EssentialMatrix bodyE = cRb.inverse() * trueE;
 
@@ -509,22 +676,14 @@ TEST (EssentialMatrixFactor3, extraTest) {
     const Point2 pi = camera2.project(P1);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(bodyE, d);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
-    // Use numerical derivatives to calculate the expected Jacobian
+    Values val;
-    Matrix Hexpected1, Hexpected2;
+    val.insert(100, bodyE);
-    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+    val.insert(i, d);
-        &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
+    EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
-        boost::none);
-    Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
-    Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
-
-    // Verify the Jacobian is correct
-    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
-    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
   }
 }
 
@@ -536,4 +695,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam_unstable/timing/timeShonanAveraging.cpp

@@ -52,7 +52,7 @@ void saveResult(string name, const Values& values) {
     myfile.open("shonan_result_of_" + name + ".dat");
     size_t nrSO3 = values.count<SO3>();
     myfile << "#Type SO3 Number " << nrSO3 << "\n";
-    for (int i = 0; i < nrSO3; ++i) {
+    for (size_t i = 0; i < nrSO3; ++i) {
         Matrix R = values.at<SO3>(i).matrix();
         // Check if the result of R.Transpose*R satisfy orthogonal constraint
         checkR(R);
@@ -72,7 +72,7 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
     ofstream myfile;
     myfile.open("shonan_result_of_" + name + ".g2o");
     size_t nrSO3 = values.count<SO3>();
-    for (int i = 0; i < nrSO3; ++i) {
+    for (size_t i = 0; i < nrSO3; ++i) {
         Matrix R = values.at<SO3>(i).matrix();
         // Check if the result of R.Transpose*R satisfy orthogonal constraint
         checkR(R);
@@ -92,7 +92,7 @@ void saveResultQuat(const Values& values) {
     ofstream myfile;
     myfile.open("shonan_result.dat");
     size_t nrSOn = values.count<SOn>();
-    for (int i = 0; i < nrSOn; ++i) {
+    for (size_t i = 0; i < nrSOn; ++i) {
         GTSAM_PRINT(values.at<SOn>(i));
         Rot3 R = Rot3(values.at<SOn>(i).matrix());
         float x = R.toQuaternion().x();

python/CMakeLists.txt

@@ -142,3 +142,13 @@ add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
         COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install
         DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
         WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
+
+# Custom make command to run all GTSAM Python tests
+add_custom_target(
+        python-test
+        COMMAND
+          ${CMAKE_COMMAND} -E env # add package to python path so no need to install
+          "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
+          ${PYTHON_EXECUTABLE} -m unittest discover
+          DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
+        WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests)

python/README.md

@@ -35,12 +35,8 @@ For instructions on updating the version of the [wrap library](https://github.co
 ## Unit Tests
 
 The Python toolbox also has a small set of unit tests located in the
-test directory. To run them:
+test directory.
-
+To run them, use `make python-test`.
-  ```bash
-  cd <GTSAM_SOURCE_DIRECTORY>/python/gtsam/tests
-  python -m unittest discover
-  ```
 
 ## Utils
 

timing/timeLago.cpp

@@ -41,11 +41,11 @@ int main(int argc, char *argv[]) {
 
   // add noise to create initial estimate
   Values initial;
-  Sampler sampler(42u);
   Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
   SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
+  Sampler sampler(noise);
   for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
-    initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
+    initial.insert(it.key, it.value.retract(sampler.sample()));
 
   // Add prior on the pose having index (key) = 0
   noiseModel::Diagonal::shared_ptr priorModel = //

wrap/.github/workflows/linux-ci.yml

@@ -10,7 +10,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        python-version: [3.6, 3.7, 3.8, 3.9]
+        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]
 
     steps:
       - name: Checkout

wrap/.github/workflows/macos-ci.yml

@@ -10,7 +10,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        python-version: [3.6, 3.7, 3.8, 3.9]
+        python-version: [3.5, 3.6, 3.7, 3.8, 3.9]
 
     steps:
       - name: Checkout

wrap/DOCS.md

@@ -100,7 +100,8 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the
         ```
 
 - Using classes defined in other modules
-    - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. `OtherClass` should be in the same project.
+    - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error.
+    - `OtherClass` may not be in the same project. If this is the case, include the header for the appropriate project `#include <other_project/OtherClass.h>`.
 
 - Virtual inheritance
     - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace.

wrap/gtwrap/interface_parser/classes.py

@@ -189,7 +189,7 @@ class Operator:
 
         # Check to ensure arg and return type are the same.
         if len(args) == 1 and self.operator not in ("()", "[]"):
-            assert args.args_list[0].ctype.typename.name == return_type.type1.typename.name, \
+            assert args.list()[0].ctype.typename.name == return_type.type1.typename.name, \
                 "Mixed type overloading not supported. Both arg and return type must be the same."
 
     def __repr__(self) -> str:

wrap/gtwrap/interface_parser/declaration.py

@@ -15,6 +15,7 @@ from pyparsing import CharsNotIn, Optional
 from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON,
                      VIRTUAL)
 from .type import Typename
+from .utils import collect_namespaces
 
 
 class Include:
@@ -42,11 +43,12 @@ class ForwardDeclaration:
                 t.name, t.parent_type, t.is_virtual))
 
     def __init__(self,
-                 name: Typename,
+                 typename: Typename,
                  parent_type: str,
                  is_virtual: str,
                  parent: str = ''):
-        self.name = name
+        self.name = typename.name
+        self.typename = typename
         if parent_type:
             self.parent_type = parent_type
         else:
@@ -55,6 +57,9 @@ class ForwardDeclaration:
         self.is_virtual = is_virtual
         self.parent = parent
 
+    def namespaces(self) -> list:
+        """Get the namespaces which this class is nested under as a list."""
+        return collect_namespaces(self)
+
     def __repr__(self) -> str:
-        return "ForwardDeclaration: {} {}({})".format(self.is_virtual,
+        return "ForwardDeclaration: {} {}".format(self.is_virtual, self.name)
-                                                      self.name, self.parent)

wrap/gtwrap/interface_parser/function.py

@@ -29,11 +29,13 @@ class Argument:
     void sayHello(/*`s` is the method argument with type `const string&`*/ const string& s);
     ```
     """
-    rule = ((Type.rule ^ TemplatedType.rule)("ctype") + IDENT("name") + \
+    rule = ((Type.rule ^ TemplatedType.rule)("ctype")  #
-            Optional(EQUAL + (DEFAULT_ARG ^ Type.rule ^ TemplatedType.rule) + \
+            + IDENT("name")  #
-                Optional(LPAREN + RPAREN)  # Needed to parse the parens for default constructors
+            + Optional(EQUAL + DEFAULT_ARG)("default")
-                )("default")
+            ).setParseAction(lambda t: Argument(
-            ).setParseAction(lambda t: Argument(t.ctype, t.name, t.default))
+                t.ctype,  #
+                t.name,  #
+                t.default[0] if isinstance(t.default, ParseResults) else None))
 
     def __init__(self,
                  ctype: Union[Type, TemplatedType],
@@ -44,18 +46,8 @@ class Argument:
         else:
             self.ctype = ctype
         self.name = name
-        # If the length is 1, it's a regular type,
-        if len(default) == 1:
-            default = default[0]
-        # This means a tuple has been passed so we convert accordingly
-        elif len(default) > 1:
-            default = tuple(default.asList())
-        else:
-            # set to None explicitly so we can support empty strings
-            default = None
         self.default = default
-
+        self.parent = None  # type: Union[ArgumentList, None]
-        self.parent: Union[ArgumentList, None] = None
 
     def __repr__(self) -> str:
         return self.to_cpp()
@@ -94,10 +86,14 @@ class ArgumentList:
     def __len__(self) -> int:
         return len(self.args_list)
 
-    def args_names(self) -> List[str]:
+    def names(self) -> List[str]:
         """Return a list of the names of all the arguments."""
         return [arg.name for arg in self.args_list]
 
+    def list(self) -> List[Argument]:
+        """Return a list of the names of all the arguments."""
+        return self.args_list
+
     def to_cpp(self, use_boost: bool) -> List[str]:
         """Generate the C++ code for wrapping."""
         return [arg.ctype.to_cpp(use_boost) for arg in self.args_list]

wrap/gtwrap/interface_parser/namespace.py

@@ -102,7 +102,7 @@ class Namespace:
         res = []
         for namespace in found_namespaces:
             classes_and_funcs = (c for c in namespace.content
-                                 if isinstance(c, (Class, GlobalFunction)))
+                                 if isinstance(c, (Class, GlobalFunction, ForwardDeclaration)))
             res += [c for c in classes_and_funcs if c.name == typename.name]
         if not res:
             raise ValueError("Cannot find class {} in module!".format(

wrap/gtwrap/interface_parser/tokens.py

@@ -10,9 +10,9 @@ All the token definitions.
 Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert
 """
 
-from pyparsing import (Keyword, Literal, Or, QuotedString, Suppress, Word,
+from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress,
-                       alphanums, alphas, delimitedList, nums,
+                       Word, alphanums, alphas, nestedExpr, nums,
-                       pyparsing_common)
+                       originalTextFor, printables)
 
 # rule for identifiers (e.g. variable names)
 IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums)
@@ -22,16 +22,20 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&")
 LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;")
 LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=")
 
-# Encapsulating type for numbers, and single and double quoted strings.
-# The pyparsing_common utilities ensure correct coversion to the corresponding type.
-# E.g. pyparsing_common.number will convert 3.1415 to a float type.
-NUMBER_OR_STRING = (pyparsing_common.number ^ QuotedString('"') ^ QuotedString("'"))
-
-# A python tuple, e.g. (1, 9, "random", 3.1415)
-TUPLE = (LPAREN + delimitedList(NUMBER_OR_STRING) + RPAREN)
-
 # Default argument passed to functions/methods.
-DEFAULT_ARG = (NUMBER_OR_STRING ^ pyparsing_common.identifier ^ TUPLE)
+# Allow anything up to ',' or ';' except when they
+# appear inside matched expressions such as
+# (a, b) {c, b} "hello, world", templates, initializer lists, etc.
+DEFAULT_ARG = originalTextFor(
+    OneOrMore(
+        QuotedString('"') ^  # parse double quoted strings
+        QuotedString("'") ^  # parse single quoted strings
+        Word(printables, excludeChars="(){}[]<>,;") ^  # parse arbitrary words
+        nestedExpr(opener='(', closer=')') ^  # parse expression in parentheses
+        nestedExpr(opener='[', closer=']') ^  # parse expression in brackets
+        nestedExpr(opener='{', closer='}') ^  # parse expression in braces
+        nestedExpr(opener='<', closer='>')  # parse template expressions
+    ))
 
 CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map(
     Keyword,

wrap/gtwrap/interface_parser/variable.py

@@ -12,7 +12,7 @@ Author: Varun Agrawal, Gerry Chen
 
 from pyparsing import Optional, ParseResults
 
-from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON, STATIC
+from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON
 from .type import TemplatedType, Type
 
 
@@ -32,10 +32,12 @@ class Variable:
     """
     rule = ((Type.rule ^ TemplatedType.rule)("ctype")  #
             + IDENT("name")  #
-            #TODO(Varun) Add support for non-basic types
+            + Optional(EQUAL + DEFAULT_ARG)("default")  #
-            + Optional(EQUAL + (DEFAULT_ARG))("default")  #
             + SEMI_COLON  #
-            ).setParseAction(lambda t: Variable(t.ctype, t.name, t.default))
+            ).setParseAction(lambda t: Variable(
+                t.ctype,  #
+                t.name,  #
+                t.default[0] if isinstance(t.default, ParseResults) else None))
 
     def __init__(self,
                  ctype: Type,
@@ -44,11 +46,7 @@ class Variable:
                  parent=''):
         self.ctype = ctype[0]  # ParseResult is a list
         self.name = name
-        if default:
+        self.default = default
-            self.default = default[0]
-        else:
-            self.default = None
-
         self.parent = parent
 
     def __repr__(self) -> str:

wrap/gtwrap/matlab_wrapper.py

@@ -57,7 +57,7 @@ class MatlabWrapper(object):
     # Methods that should be ignored
     ignore_methods = ['pickle']
     # Datatypes that do not need to be checked in methods
-    not_check_type: list = []
+    not_check_type = []  # type: list
     # Data types that are primitive types
     not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t']
     # Ignore the namespace for these datatypes
@@ -65,16 +65,18 @@ class MatlabWrapper(object):
     # The amount of times the wrapper has created a call to geometry_wrapper
     wrapper_id = 0
     # Map each wrapper id to what its collector function namespace, class, type, and string format
-    wrapper_map: dict = {}
+    wrapper_map = {}
     # Set of all the includes in the namespace
-    includes: Dict[parser.Include, int] = {}
+    includes = {}  # type: Dict[parser.Include, int]
     # Set of all classes in the namespace
-    classes: List[Union[parser.Class, instantiator.InstantiatedClass]] = []
+    classes = [
-    classes_elems: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] = {}
+    ]  # type: List[Union[parser.Class, instantiator.InstantiatedClass]]
+    classes_elems = {
+    }  # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int]
     # Id for ordering global functions in the wrapper
     global_function_id = 0
     # Files and their content
-    content: List[str] = []
+    content = []  # type: List[str]
 
     # Ensure the template file is always picked up from the correct directory.
     dir_path = osp.dirname(osp.realpath(__file__))
@@ -82,11 +84,9 @@ class MatlabWrapper(object):
         wrapper_file_header = f.read()
 
     def __init__(self,
-                 module,
                  module_name,
                  top_module_namespace='',
                  ignore_classes=()):
-        self.module = module
         self.module_name = module_name
         self.top_module_namespace = top_module_namespace
         self.ignore_classes = ignore_classes
@@ -374,14 +374,14 @@ class MatlabWrapper(object):
         """
         arg_wrap = ''
 
-        for i, arg in enumerate(args.args_list, 1):
+        for i, arg in enumerate(args.list(), 1):
             c_type = self._format_type_name(arg.ctype.typename,
                                             include_namespace=False)
 
             arg_wrap += '{c_type} {arg_name}{comma}'.format(
                 c_type=c_type,
                 arg_name=arg.name,
-                comma='' if i == len(args.args_list) else ', ')
+                comma='' if i == len(args.list()) else ', ')
 
         return arg_wrap
 
@@ -396,7 +396,7 @@ class MatlabWrapper(object):
         """
         var_arg_wrap = ''
 
-        for i, arg in enumerate(args.args_list, 1):
+        for i, arg in enumerate(args.list(), 1):
             name = arg.ctype.typename.name
             if name in self.not_check_type:
                 continue
@@ -442,7 +442,7 @@ class MatlabWrapper(object):
         var_list_wrap = ''
         first = True
 
-        for i in range(1, len(args.args_list) + 1):
+        for i in range(1, len(args.list()) + 1):
             if first:
                 var_list_wrap += 'varargin{{{}}}'.format(i)
                 first = False
@@ -462,9 +462,9 @@ class MatlabWrapper(object):
         if check_statement == '':
             check_statement = \
                 'if length(varargin) == {param_count}'.format(
-                    param_count=len(args.args_list))
+                    param_count=len(args.list()))
 
-        for _, arg in enumerate(args.args_list):
+        for _, arg in enumerate(args.list()):
             name = arg.ctype.typename.name
 
             if name in self.not_check_type:
@@ -516,7 +516,7 @@ class MatlabWrapper(object):
         params = ''
         body_args = ''
 
-        for arg in args.args_list:
+        for arg in args.list():
             if params != '':
                 params += ','
 
@@ -725,10 +725,10 @@ class MatlabWrapper(object):
             param_wrap += '      if' if i == 0 else '      elseif'
             param_wrap += ' length(varargin) == '
 
-            if len(overload.args.args_list) == 0:
+            if len(overload.args.list()) == 0:
                 param_wrap += '0\n'
             else:
-                param_wrap += str(len(overload.args.args_list)) \
+                param_wrap += str(len(overload.args.list())) \
                               + self._wrap_variable_arguments(overload.args, False) + '\n'
 
             # Determine format of return and varargout statements
@@ -825,14 +825,14 @@ class MatlabWrapper(object):
             methods_wrap += textwrap.indent(textwrap.dedent('''\
                 elseif nargin == {len}{varargin}
                   {ptr}{wrapper}({num}{comma}{var_arg});
-            ''').format(len=len(ctor.args.args_list),
+            ''').format(len=len(ctor.args.list()),
                         varargin=self._wrap_variable_arguments(
                             ctor.args, False),
                         ptr=wrapper_return,
                         wrapper=self._wrapper_name(),
                         num=self._update_wrapper_id(
                             (namespace_name, inst_class, 'constructor', ctor)),
-                        comma='' if len(ctor.args.args_list) == 0 else ', ',
+                        comma='' if len(ctor.args.list()) == 0 else ', ',
                         var_arg=self._wrap_list_variable_arguments(ctor.args)),
                                             prefix='    ')
 
@@ -938,7 +938,7 @@ class MatlabWrapper(object):
                            namespace_name,
                            inst_class,
                            methods,
-                           serialize=(False,)):
+                           serialize=(False, )):
         """Wrap the methods in the class.
 
         Args:
@@ -1075,7 +1075,7 @@ class MatlabWrapper(object):
                         static_overload.return_type,
                         include_namespace=True,
                         separator="."),
-                    length=len(static_overload.args.args_list),
+                    length=len(static_overload.args.list()),
                     var_args_list=self._wrap_variable_arguments(
                         static_overload.args),
                     check_statement=check_statement,
@@ -1097,7 +1097,8 @@ class MatlabWrapper(object):
 
             method_text += textwrap.indent(textwrap.dedent("""\
                                     end\n
-                """), prefix="  ")
+                """),
+                                           prefix="  ")
 
         if serialize:
             method_text += textwrap.indent(textwrap.dedent("""\
@@ -1349,14 +1350,14 @@ class MatlabWrapper(object):
 
         else:
             if isinstance(method.parent, instantiator.InstantiatedClass):
-                method_name = method.parent.cpp_class() + "::"
+                method_name = method.parent.to_cpp() + "::"
             else:
                 method_name = self._format_static_method(method, '::')
             method_name += method.name
 
         if "MeasureRange" in method_name:
             self._debug("method: {}, method: {}, inst: {}".format(
-                method_name, method.name, method.parent.cpp_class()))
+                method_name, method.name, method.parent.to_cpp()))
 
         obj = '  ' if return_1_name == 'void' else ''
         obj += '{}{}({})'.format(obj_start, method_name, params)
@@ -1447,7 +1448,7 @@ class MatlabWrapper(object):
             extra = collector_func[4]
 
             class_name = collector_func[0] + collector_func[1].name
-            class_name_separated = collector_func[1].cpp_class()
+            class_name_separated = collector_func[1].to_cpp()
             is_method = isinstance(extra, parser.Method)
             is_static_method = isinstance(extra, parser.StaticMethod)
 
@@ -1510,12 +1511,12 @@ class MatlabWrapper(object):
             elif extra == 'serialize':
                 body += self.wrap_collector_function_serialize(
                     collector_func[1].name,
-                    full_name=collector_func[1].cpp_class(),
+                    full_name=collector_func[1].to_cpp(),
                     namespace=collector_func[0])
             elif extra == 'deserialize':
                 body += self.wrap_collector_function_deserialize(
                     collector_func[1].name,
-                    full_name=collector_func[1].cpp_class(),
+                    full_name=collector_func[1].to_cpp(),
                     namespace=collector_func[0])
             elif is_method or is_static_method:
                 method_name = ''
@@ -1548,7 +1549,7 @@ class MatlabWrapper(object):
                     min1='-1' if is_method else '',
                     shared_obj=shared_obj,
                     method_name=method_name,
-                    num_args=len(extra.args.args_list),
+                    num_args=len(extra.args.list()),
                     body_args=body_args,
                     return_body=return_body)
 
@@ -1565,10 +1566,11 @@ class MatlabWrapper(object):
                   checkArguments("{function_name}",nargout,nargin,{len});
             ''').format(function_name=collector_func[1].name,
                         id=self.global_function_id,
-                        len=len(collector_func[1].args.args_list))
+                        len=len(collector_func[1].args.list()))
 
             body += self._wrapper_unwrap_arguments(collector_func[1].args)[1]
-            body += self.wrap_collector_function_return(collector_func[1]) + '\n}\n'
+            body += self.wrap_collector_function_return(
+                collector_func[1]) + '\n}\n'
 
             collector_function += body
 
@@ -1723,7 +1725,7 @@ class MatlabWrapper(object):
                     cls_insts += self._format_type_name(inst)
 
                 typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \
-                    .format(original_class_name=cls.cpp_class(),
+                    .format(original_class_name=cls.to_cpp(),
                             class_name_sep=cls.name)
 
                 class_name_sep = cls.name
@@ -1734,7 +1736,7 @@ class MatlabWrapper(object):
                     boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format(
                         class_name_sep, class_name)
             else:
-                class_name_sep = cls.cpp_class()
+                class_name_sep = cls.to_cpp()
                 class_name = self._format_class_name(cls)
 
                 if len(cls.original.namespaces()) > 1 and _has_serialization(
@@ -1780,7 +1782,7 @@ class MatlabWrapper(object):
 
             if queue_set_next_case:
                 ptr_ctor_frag += self.wrap_collector_function_upcast_from_void(
-                    id_val[1].name, idx, id_val[1].cpp_class())
+                    id_val[1].name, idx, id_val[1].to_cpp())
 
         wrapper_file += textwrap.dedent('''\
             {typedef_instances}
@@ -1872,10 +1874,14 @@ class MatlabWrapper(object):
                     namespace=namespace),
                                prefix='  ')
 
-    def wrap(self):
+    def wrap(self, content):
         """High level function to wrap the project."""
-        self.wrap_namespace(self.module)
+        # Parse the contents of the interface file
-        self.generate_wrapper(self.module)
+        parsed_result = parser.Module.parseString(content)
+        # Instantiate the module
+        module = instantiator.instantiate_namespace(parsed_result)
+        self.wrap_namespace(module)
+        self.generate_wrapper(module)
 
         return self.content
 

wrap/gtwrap/pybind_wrapper.py

@@ -23,48 +23,49 @@ class PybindWrapper:
     Class to generate binding code for Pybind11 specifically.
     """
     def __init__(self,
-                 module,
                  module_name,
                  top_module_namespaces='',
                  use_boost=False,
                  ignore_classes=(),
                  module_template=""):
-        self.module = module
         self.module_name = module_name
         self.top_module_namespaces = top_module_namespaces
         self.use_boost = use_boost
         self.ignore_classes = ignore_classes
         self._serializing_classes = list()
         self.module_template = module_template
-        self.python_keywords = ['print', 'lambda']
+        self.python_keywords = [
+            'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import',
+            'return', 'True', 'elif', 'in', 'try', 'and', 'else', 'is',
+            'while', 'as', 'except', 'lambda', 'with', 'assert', 'finally',
+            'nonlocal', 'yield', 'break', 'for', 'not', 'class', 'from', 'or',
+            'continue', 'global', 'pass'
+        ]
 
         # amount of indentation to add before each function/method declaration.
         self.method_indent = '\n' + (' ' * 8)
 
-    def _py_args_names(self, args_list):
+    def _py_args_names(self, args):
         """Set the argument names in Pybind11 format."""
-        names = args_list.args_names()
+        names = args.names()
         if names:
             py_args = []
-            for arg in args_list.args_list:
+            for arg in args.list():
-                if isinstance(arg.default, str) and arg.default is not None:
+                if arg.default is not None:
-                    # string default arg
+                    default = ' = {arg.default}'.format(arg=arg)
-                    arg.default = ' = "{arg.default}"'.format(arg=arg)
-                elif arg.default:  # Other types
-                    arg.default = ' = {arg.default}'.format(arg=arg)
                 else:
-                    arg.default = ''
+                    default = ''
                 argument = 'py::arg("{name}"){default}'.format(
-                    name=arg.name, default='{0}'.format(arg.default))
+                    name=arg.name, default='{0}'.format(default))
                 py_args.append(argument)
             return ", " + ", ".join(py_args)
         else:
             return ''
 
-    def _method_args_signature_with_names(self, args_list):
+    def _method_args_signature(self, args):
-        """Define the method signature types with the argument names."""
+        """Generate the argument types and names as per the method signature."""
-        cpp_types = args_list.to_cpp(self.use_boost)
+        cpp_types = args.to_cpp(self.use_boost)
-        names = args_list.args_names()
+        names = args.names()
         types_names = [
             "{} {}".format(ctype, name)
             for ctype, name in zip(cpp_types, names)
@@ -99,7 +100,8 @@ class PybindWrapper:
             serialize_method = self.method_indent + \
                 ".def(\"serialize\", []({class_inst} self){{ return gtsam::serialize(*self); }})".format(class_inst=cpp_class + '*')
             deserialize_method = self.method_indent + \
-                     ".def(\"deserialize\", []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg(\"serialized\"))" \
+                     '.def("deserialize", []({class_inst} self, string serialized)' \
+                     '{{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized"))' \
                        .format(class_inst=cpp_class + '*')
             return serialize_method + deserialize_method
 
@@ -112,20 +114,23 @@ class PybindWrapper:
             return pickle_method.format(cpp_class=cpp_class,
                                         indent=self.method_indent)
 
+        # Add underscore to disambiguate if the method name matches a python keyword
+        if py_method in self.python_keywords:
+            py_method = py_method + "_"
+
         is_method = isinstance(method, instantiator.InstantiatedMethod)
         is_static = isinstance(method, parser.StaticMethod)
         return_void = method.return_type.is_void()
-        args_names = method.args.args_names()
+        args_names = method.args.names()
         py_args_names = self._py_args_names(method.args)
-        args_signature_with_names = self._method_args_signature_with_names(
+        args_signature_with_names = self._method_args_signature(method.args)
-            method.args)
 
         caller = cpp_class + "::" if not is_method else "self->"
-        function_call = ('{opt_return} {caller}{function_name}'
+        function_call = ('{opt_return} {caller}{method_name}'
                          '({args_names});'.format(
                              opt_return='return' if not return_void else '',
                              caller=caller,
-                             function_name=cpp_method,
+                             method_name=cpp_method,
                              args_names=', '.join(args_names),
                          ))
 
@@ -136,8 +141,7 @@ class PybindWrapper:
                '{py_args_names}){suffix}'.format(
                    prefix=prefix,
                    cdef="def_static" if is_static else "def",
-                   py_method=py_method if not py_method in self.python_keywords
+                   py_method=py_method,
-                   else py_method + "_",
                    opt_self="{cpp_class}* self".format(
                        cpp_class=cpp_class) if is_method else "",
                    opt_comma=', ' if is_method and args_names else '',
@@ -181,15 +185,13 @@ class PybindWrapper:
                      suffix=''):
         """
         Wrap all the methods in the `cpp_class`.
-
-        This function is also used to wrap global functions.
         """
         res = ""
         for method in methods:
 
-            # To avoid type confusion for insert, currently unused
+            # To avoid type confusion for insert
             if method.name == 'insert' and cpp_class == 'gtsam::Values':
-                name_list = method.args.args_names()
+                name_list = method.args.names()
                 type_list = method.args.to_cpp(self.use_boost)
                 # inserting non-wrapped value types
                 if type_list[0].strip() == 'size_t':
@@ -214,7 +216,8 @@ class PybindWrapper:
                       module_var,
                       variable,
                       prefix='\n' + ' ' * 8):
-        """Wrap a variable that's not part of a class (i.e. global)
+        """
+        Wrap a variable that's not part of a class (i.e. global)
         """
         variable_value = ""
         if variable.default is None:
@@ -288,23 +291,20 @@ class PybindWrapper:
 
     def wrap_enums(self, enums, instantiated_class, prefix=' ' * 4):
         """Wrap multiple enums defined in a class."""
-        cpp_class = instantiated_class.cpp_class()
+        cpp_class = instantiated_class.to_cpp()
         module_var = instantiated_class.name.lower()
         res = ''
 
         for enum in enums:
             res += "\n" + self.wrap_enum(
-                enum,
+                enum, class_name=cpp_class, module=module_var, prefix=prefix)
-                class_name=cpp_class,
-                module=module_var,
-                prefix=prefix)
         return res
 
     def wrap_instantiated_class(
             self, instantiated_class: instantiator.InstantiatedClass):
         """Wrap the class."""
         module_var = self._gen_module_var(instantiated_class.namespaces())
-        cpp_class = instantiated_class.cpp_class()
+        cpp_class = instantiated_class.to_cpp()
         if cpp_class in self.ignore_classes:
             return ""
         if instantiated_class.parent_class:
@@ -356,10 +356,27 @@ class PybindWrapper:
                     wrapped_operators=self.wrap_operators(
                         instantiated_class.operators, cpp_class)))
 
+    def wrap_instantiated_declaration(
+            self, instantiated_decl: instantiator.InstantiatedDeclaration):
+        """Wrap the class."""
+        module_var = self._gen_module_var(instantiated_decl.namespaces())
+        cpp_class = instantiated_decl.to_cpp()
+        if cpp_class in self.ignore_classes:
+            return ""
+
+        res = (
+            '\n    py::class_<{cpp_class}, '
+            '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
+        ).format(shared_ptr_type=('boost' if self.use_boost else 'std'),
+                 cpp_class=cpp_class,
+                 class_name=instantiated_decl.name,
+                 module_var=module_var)
+        return res
+
     def wrap_stl_class(self, stl_class):
         """Wrap STL containers."""
         module_var = self._gen_module_var(stl_class.namespaces())
-        cpp_class = stl_class.cpp_class()
+        cpp_class = stl_class.to_cpp()
         if cpp_class in self.ignore_classes:
             return ""
 
@@ -385,6 +402,59 @@ class PybindWrapper:
                     stl_class.properties, cpp_class),
             ))
 
+    def wrap_functions(self,
+                       functions,
+                       namespace,
+                       prefix='\n' + ' ' * 8,
+                       suffix=''):
+        """
+        Wrap all the global functions.
+        """
+        res = ""
+        for function in functions:
+
+            function_name = function.name
+
+            # Add underscore to disambiguate if the function name matches a python keyword
+            python_keywords = self.python_keywords + ['print']
+            if function_name in python_keywords:
+                function_name = function_name + "_"
+
+            cpp_method = function.to_cpp()
+
+            is_static = isinstance(function, parser.StaticMethod)
+            return_void = function.return_type.is_void()
+            args_names = function.args.names()
+            py_args_names = self._py_args_names(function.args)
+            args_signature = self._method_args_signature(function.args)
+
+            caller = namespace + "::"
+            function_call = ('{opt_return} {caller}{function_name}'
+                             '({args_names});'.format(
+                                 opt_return='return'
+                                 if not return_void else '',
+                                 caller=caller,
+                                 function_name=cpp_method,
+                                 args_names=', '.join(args_names),
+                             ))
+
+            ret = ('{prefix}.{cdef}("{function_name}",'
+                   '[]({args_signature}){{'
+                   '{function_call}'
+                   '}}'
+                   '{py_args_names}){suffix}'.format(
+                       prefix=prefix,
+                       cdef="def_static" if is_static else "def",
+                       function_name=function_name,
+                       args_signature=args_signature,
+                       function_call=function_call,
+                       py_args_names=py_args_names,
+                       suffix=suffix))
+
+            res += ret
+
+        return res
+
     def _partial_match(self, namespaces1, namespaces2):
         for i in range(min(len(namespaces1), len(namespaces2))):
             if namespaces1[i] != namespaces2[i]:
@@ -460,6 +530,9 @@ class PybindWrapper:
                     wrapped += self.wrap_instantiated_class(element)
                     wrapped += self.wrap_enums(element.enums, element)
 
+                elif isinstance(element, instantiator.InstantiatedDeclaration):
+                    wrapped += self.wrap_instantiated_declaration(element)
+
                 elif isinstance(element, parser.Variable):
                     variable_namespace = self._add_namespaces('', namespaces)
                     wrapped += self.wrap_variable(namespace=variable_namespace,
@@ -476,7 +549,7 @@ class PybindWrapper:
                 if isinstance(func, (parser.GlobalFunction,
                                      instantiator.InstantiatedGlobalFunction))
             ]
-            wrapped += self.wrap_methods(
+            wrapped += self.wrap_functions(
                 all_funcs,
                 self._add_namespaces('', namespaces)[:-2],
                 prefix='\n' + ' ' * 4 + module_var,
@@ -484,9 +557,14 @@ class PybindWrapper:
             )
         return wrapped, includes
 
-    def wrap(self):
+    def wrap(self, content):
         """Wrap the code in the interface file."""
-        wrapped_namespace, includes = self.wrap_namespace(self.module)
+        # Parse the contents of the interface file
+        module = parser.Module.parseString(content)
+        # Instantiate all templates
+        module = instantiator.instantiate_namespace(module)
+
+        wrapped_namespace, includes = self.wrap_namespace(module)
 
         # Export classes for serialization.
         boost_class_export = ""

wrap/gtwrap/template_instantiator.py

@@ -95,10 +95,9 @@ def instantiate_args_list(args_list, template_typenames, instantiations,
     for arg in args_list:
         new_type = instantiate_type(arg.ctype, template_typenames,
                                     instantiations, cpp_typename)
-        default = [arg.default] if isinstance(arg, parser.Argument) else ''
+        instantiated_args.append(
-        instantiated_args.append(parser.Argument(name=arg.name,
+            parser.Argument(name=arg.name, ctype=new_type,
-                                                 ctype=new_type,
+                            default=arg.default))
-                                                 default=default))
     return instantiated_args
 
 
@@ -131,7 +130,6 @@ def instantiate_name(original_name, instantiations):
     TODO(duy): To avoid conflicts, we should include the instantiation's
     namespaces, but I find that too verbose.
     """
-    inst_name = ''
     instantiated_names = []
     for inst in instantiations:
         # Ensure the first character of the type is capitalized
@@ -172,7 +170,7 @@ class InstantiatedGlobalFunction(parser.GlobalFunction):
                 cpp_typename='',
             )
             instantiated_args = instantiate_args_list(
-                original.args.args_list,
+                original.args.list(),
                 self.original.template.typenames,
                 self.instantiations,
                 # Keyword type name `This` should already be replaced in the
@@ -206,7 +204,13 @@ class InstantiatedGlobalFunction(parser.GlobalFunction):
 
 class InstantiatedMethod(parser.Method):
     """
-    We can only instantiate template methods with a single template parameter.
+    Instantiate method with template parameters.
+
+    E.g.
+    class A {
+        template<X, Y>
+        void func(X x, Y y);
+    }
     """
     def __init__(self, original, instantiations: List[parser.Typename] = ''):
         self.original = original
@@ -216,7 +220,7 @@ class InstantiatedMethod(parser.Method):
         self.parent = original.parent
 
         # Check for typenames if templated.
-        # This way, we can gracefully handle bot templated and non-templated methois.
+        # This way, we can gracefully handle both templated and non-templated methods.
         typenames = self.original.template.typenames if self.original.template else []
         self.name = instantiate_name(original.name, self.instantiations)
         self.return_type = instantiate_return_type(
@@ -229,7 +233,7 @@ class InstantiatedMethod(parser.Method):
         )
 
         instantiated_args = instantiate_args_list(
-            original.args.args_list,
+            original.args.list(),
             typenames,
             self.instantiations,
             # Keyword type name `This` should already be replaced in the
@@ -342,7 +346,7 @@ class InstantiatedClass(parser.Class):
             "{ctors}\n{static_methods}\n{methods}".format(
                virtual="virtual" if self.is_virtual else '',
                name=self.name,
-               cpp_class=self.cpp_class(),
+               cpp_class=self.to_cpp(),
                parent_class=self.parent,
                ctors="\n".join([repr(ctor) for ctor in self.ctors]),
                methods="\n".join([repr(m) for m in self.methods]),
@@ -364,7 +368,7 @@ class InstantiatedClass(parser.Class):
 
         for ctor in self.original.ctors:
             instantiated_args = instantiate_args_list(
-                ctor.args.args_list,
+                ctor.args.list(),
                 typenames,
                 self.instantiations,
                 self.cpp_typename(),
@@ -389,7 +393,7 @@ class InstantiatedClass(parser.Class):
         instantiated_static_methods = []
         for static_method in self.original.static_methods:
             instantiated_args = instantiate_args_list(
-                static_method.args.args_list, typenames, self.instantiations,
+                static_method.args.list(), typenames, self.instantiations,
                 self.cpp_typename())
             instantiated_static_methods.append(
                 parser.StaticMethod(
@@ -426,7 +430,7 @@ class InstantiatedClass(parser.Class):
         class_instantiated_methods = []
         for method in self.original.methods:
             instantiated_args = instantiate_args_list(
-                method.args.args_list,
+                method.args.list(),
                 typenames,
                 self.instantiations,
                 self.cpp_typename(),
@@ -459,7 +463,7 @@ class InstantiatedClass(parser.Class):
         instantiated_operators = []
         for operator in self.original.operators:
             instantiated_args = instantiate_args_list(
-                operator.args.args_list,
+                operator.args.list(),
                 typenames,
                 self.instantiations,
                 self.cpp_typename(),
@@ -497,10 +501,6 @@ class InstantiatedClass(parser.Class):
         )
         return instantiated_properties
 
-    def cpp_class(self):
-        """Generate the C++ code for wrapping."""
-        return self.cpp_typename().to_cpp()
-
     def cpp_typename(self):
         """
         Return a parser.Typename including namespaces and cpp name of this
@@ -516,8 +516,53 @@ class InstantiatedClass(parser.Class):
         namespaces_name.append(name)
         return parser.Typename(namespaces_name)
 
+    def to_cpp(self):
+        """Generate the C++ code for wrapping."""
+        return self.cpp_typename().to_cpp()
 
-def instantiate_namespace_inplace(namespace):
+
+class InstantiatedDeclaration(parser.ForwardDeclaration):
+    """
+    Instantiate typedefs of forward declarations.
+    This is useful when we wish to typedef a templated class
+    which is not defined in the current project.
+
+    E.g.
+        class FactorFromAnotherMother;
+
+        typedef FactorFromAnotherMother<gtsam::Pose3> FactorWeCanUse;
+    """
+    def __init__(self, original, instantiations=(), new_name=''):
+        super().__init__(original.typename,
+                         original.parent_type,
+                         original.is_virtual,
+                         parent=original.parent)
+
+        self.original = original
+        self.instantiations = instantiations
+        self.parent = original.parent
+
+        self.name = instantiate_name(
+            original.name, instantiations) if not new_name else new_name
+
+    def to_cpp(self):
+        """Generate the C++ code for wrapping."""
+        instantiated_names = [
+            inst.qualified_name() for inst in self.instantiations
+        ]
+        name = "{}<{}>".format(self.original.name,
+                               ",".join(instantiated_names))
+        namespaces_name = self.namespaces()
+        namespaces_name.append(name)
+        # Leverage Typename to generate the fully qualified C++ name
+        return parser.Typename(namespaces_name).to_cpp()
+
+    def __repr__(self):
+        return "Instantiated {}".format(
+            super(InstantiatedDeclaration, self).__repr__())
+
+
+def instantiate_namespace(namespace):
     """
     Instantiate the classes and other elements in the `namespace` content and
     assign it back to the namespace content attribute.
@@ -567,7 +612,8 @@ def instantiate_namespace_inplace(namespace):
             original_element = top_level.find_class_or_function(
                 typedef_inst.typename)
 
-            # Check if element is a typedef'd class or function.
+            # Check if element is a typedef'd class, function or
+            # forward declaration from another project.
             if isinstance(original_element, parser.Class):
                 typedef_content.append(
                     InstantiatedClass(original_element,
@@ -578,12 +624,19 @@ def instantiate_namespace_inplace(namespace):
                     InstantiatedGlobalFunction(
                         original_element, typedef_inst.typename.instantiations,
                         typedef_inst.new_name))
+            elif isinstance(original_element, parser.ForwardDeclaration):
+                typedef_content.append(
+                    InstantiatedDeclaration(
+                        original_element, typedef_inst.typename.instantiations,
+                        typedef_inst.new_name))
 
         elif isinstance(element, parser.Namespace):
-            instantiate_namespace_inplace(element)
+            element = instantiate_namespace(element)
             instantiated_content.append(element)
         else:
             instantiated_content.append(element)
 
     instantiated_content.extend(typedef_content)
     namespace.content = instantiated_content
+
+    return namespace

wrap/scripts/matlab_wrap.py

@@ -8,9 +8,8 @@ and invoked during the wrapping by CMake.
 
 import argparse
 import os
+import sys
 
-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper import MatlabWrapper, generate_content
 
 if __name__ == "__main__":
@@ -51,18 +50,11 @@ if __name__ == "__main__":
     if not os.path.exists(args.src):
         os.mkdir(args.src)
 
-    module = parser.Module.parseString(content)
-
-    instantiator.instantiate_namespace_inplace(module)
-
-    import sys
-
     print("Ignoring classes: {}".format(args.ignore), file=sys.stderr)
-    wrapper = MatlabWrapper(module=module,
+    wrapper = MatlabWrapper(module_name=args.module_name,
-                            module_name=args.module_name,
                             top_module_namespace=top_module_namespaces,
                             ignore_classes=args.ignore)
 
-    cc_content = wrapper.wrap()
+    cc_content = wrapper.wrap(content)
 
     generate_content(cc_content, args.out)

wrap/scripts/pybind_wrap.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-
 """
 Helper script to wrap C++ to Python with Pybind.
 This script is installed via CMake to the user's binary directory
@@ -10,8 +9,6 @@ and invoked during the wrapping by CMake.
 
 import argparse
 
-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.pybind_wrapper import PybindWrapper
 
 
@@ -19,8 +16,7 @@ def main():
     """Main runner."""
     arg_parser = argparse.ArgumentParser(
         formatter_class=argparse.ArgumentDefaultsHelpFormatter)
-    arg_parser.add_argument(
+    arg_parser.add_argument("--src",
-        "--src",
                             type=str,
                             required=True,
                             help="Input interface .i/.h file")
@@ -62,7 +58,8 @@ def main():
         help="A space-separated list of classes to ignore. "
         "Class names must include their full namespaces.",
     )
-    arg_parser.add_argument("--template", type=str,
+    arg_parser.add_argument("--template",
+                            type=str,
                             help="The module template file")
     args = arg_parser.parse_args()
 
@@ -74,14 +71,10 @@ def main():
     with open(args.src, "r") as f:
         content = f.read()
 
-    module = parser.Module.parseString(content)
-    instantiator.instantiate_namespace_inplace(module)
-
     with open(args.template, "r") as f:
         template_content = f.read()
 
     wrapper = PybindWrapper(
-        module=module,
         module_name=args.module_name,
         use_boost=args.use_boost,
         top_module_namespaces=top_module_namespaces,
@@ -90,7 +83,7 @@ def main():
     )
 
     # Wrap the code and get back the cpp/cc code.
-    cc_content = wrapper.wrap()
+    cc_content = wrapper.wrap(content)
 
     # Generate the C++ code which Pybind11 will use.
     with open(args.out, "w") as f:

wrap/setup.py

@@ -10,7 +10,7 @@ packages = find_packages()
 setup(
     name='gtwrap',
     description='Library to wrap C++ with Python and Matlab',
-    version='1.1.0',
+    version='2.0.0',
     author="Frank Dellaert et. al.",
     author_email="dellaert@gatech.edu",
     license='BSD',

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(48, my_ptr);
+        class_wrapper(49, 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(49, obj.ptr_MultipleTemplatesIntDouble);
+      class_wrapper(50, 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(50, my_ptr);
+        class_wrapper(51, 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(51, obj.ptr_MultipleTemplatesIntFloat);
+      class_wrapper(52, 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(52, my_ptr);
+        class_wrapper(56, 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(53, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+        my_ptr = class_wrapper(57, 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(54, obj.ptr_MyFactorPosePoint2);
+      class_wrapper(58, 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(55, this, varargin{:});
+        class_wrapper(59, 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(45, my_ptr);
+        class_wrapper(46, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(46);
+        my_ptr = class_wrapper(47);
       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(47, obj.ptr_MyVector12);
+      class_wrapper(48, 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(42, my_ptr);
+        class_wrapper(43, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(43);
+        my_ptr = class_wrapper(44);
       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(44, obj.ptr_MyVector3);
+      class_wrapper(45, 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(38, my_ptr);
+        class_wrapper(39, my_ptr);
       elseif nargin == 0
-        my_ptr = class_wrapper(39);
+        my_ptr = class_wrapper(40);
       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(40, obj.ptr_PrimitiveRefDouble);
+      class_wrapper(41, 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(41, varargin{:});
+        varargout{1} = class_wrapper(42, varargin{:});
         return
       end
 

wrap/tests/expected/matlab/TemplatedFunctionRot3.m

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

wrap/tests/expected/matlab/Test.m

@@ -10,6 +10,7 @@
 %create_MixedPtrs() : returns pair< Test, Test >
 %create_ptrs() : returns pair< Test, Test >
 %get_container() : returns std::vector<testing::Test>
+%lambda() : returns void
 %print() : returns void
 %return_Point2Ptr(bool value) : returns Point2
 %return_Test(Test value) : returns Test
@@ -98,11 +99,21 @@ classdef Test < handle
       error('Arguments do not match any overload of function Test.get_container');
     end
 
+    function varargout = lambda(this, varargin)
+      % LAMBDA usage: lambda() : returns void
+      % Doxygen can be found at https://gtsam.org/doxygen/
+      if length(varargin) == 0
+        class_wrapper(18, this, varargin{:});
+        return
+      end
+      error('Arguments do not match any overload of function Test.lambda');
+    end
+
     function varargout = print(this, varargin)
       % PRINT usage: print() : returns void
       % Doxygen can be found at https://gtsam.org/doxygen/
       if length(varargin) == 0
-        class_wrapper(18, this, varargin{:});
+        class_wrapper(19, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.print');
@@ -112,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(19, this, varargin{:});
+        varargout{1} = class_wrapper(20, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_Point2Ptr');
@@ -122,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(20, this, varargin{:});
+        varargout{1} = class_wrapper(21, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_Test');
@@ -132,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(21, this, varargin{:});
+        varargout{1} = class_wrapper(22, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_TestPtr');
@@ -142,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(22, this, varargin{:});
+        varargout{1} = class_wrapper(23, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_bool');
@@ -152,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(23, this, varargin{:});
+        varargout{1} = class_wrapper(24, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_double');
@@ -162,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(24, this, varargin{:});
+        varargout{1} = class_wrapper(25, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_field');
@@ -172,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(25, this, varargin{:});
+        varargout{1} = class_wrapper(26, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_int');
@@ -182,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(26, this, varargin{:});
+        varargout{1} = class_wrapper(27, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_matrix1');
@@ -192,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(27, this, varargin{:});
+        varargout{1} = class_wrapper(28, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_matrix2');
@@ -202,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(28, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(29, 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(29, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_pair');
@@ -218,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(30, this, varargin{:});
+        [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_ptrs');
@@ -228,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(31, this, varargin{:});
+        varargout{1} = class_wrapper(32, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_size_t');
@@ -238,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(32, this, varargin{:});
+        varargout{1} = class_wrapper(33, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_string');
@@ -248,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(33, this, varargin{:});
+        varargout{1} = class_wrapper(34, this, varargin{:});
         return
       end
       error('Arguments do not match any overload of function Test.return_vector1');
@@ -258,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(34, this, varargin{:});
+        varargout{1} = class_wrapper(35, 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(35, 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')
@@ -283,6 +288,12 @@ classdef Test < handle
         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')
+        class_wrapper(38, this, varargin{:});
+        return
+      end
       error('Arguments do not match any overload of function Test.set_container');
     end
 

wrap/tests/expected/matlab/class_wrapper.cpp

@@ -33,6 +33,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 
@@ -90,6 +92,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -304,14 +312,21 @@ void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap_shared_ptr(boost::make_shared<std::vector<testing::Test>>(obj->get_container()),"std.vectorTest", false);
 }
 
-void Test_print_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_lambda_18(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_19(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_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Point2Ptr_20(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");
@@ -322,7 +337,7 @@ void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxA
   }
 }
 
-void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Test_21(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");
@@ -330,7 +345,7 @@ void Test_return_Test_20(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_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_TestPtr_22(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");
@@ -338,7 +353,7 @@ void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }
 
-void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_bool_23(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");
@@ -346,7 +361,7 @@ void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_bool(value));
 }
 
-void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_double_24(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");
@@ -354,7 +369,7 @@ void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< double >(obj->return_double(value));
 }
 
-void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_field_25(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");
@@ -362,7 +377,7 @@ void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_field(t));
 }
 
-void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_int_26(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");
@@ -370,7 +385,7 @@ void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *
   out[0] = wrap< int >(obj->return_int(value));
 }
 
-void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix1_27(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");
@@ -378,7 +393,7 @@ void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }
 
-void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix2_28(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");
@@ -386,7 +401,7 @@ void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }
 
-void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_29(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");
@@ -397,7 +412,7 @@ void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_30(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");
@@ -407,7 +422,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_ptrs_31(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");
@@ -418,7 +433,7 @@ void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_32(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");
@@ -426,7 +441,7 @@ void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< size_t >(obj->return_size_t(value));
 }
 
-void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_33(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");
@@ -434,7 +449,7 @@ void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< string >(obj->return_string(value));
 }
 
-void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_34(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");
@@ -442,7 +457,7 @@ void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector1(value));
 }
 
-void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_35(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");
@@ -450,14 +465,6 @@ void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector2(value));
 }
 
-void Test_set_container_35(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_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("set_container",nargout,nargin-1,1);
@@ -474,7 +481,15 @@ void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArra
   obj->set_container(*container);
 }
 
-void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_set_container_38(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_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@@ -483,7 +498,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[
   collector_PrimitiveRefDouble.insert(self);
 }
 
-void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
@@ -494,7 +509,7 @@ void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<PrimitiveRef<double>> Shared;
   checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1);
@@ -507,14 +522,14 @@ void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin
   }
 }
 
-void PrimitiveRefDouble_Brutal_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("PrimitiveRefDouble.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_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -523,7 +538,7 @@ void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int na
   collector_MyVector3.insert(self);
 }
 
-void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<3>> Shared;
@@ -534,7 +549,7 @@ void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxA
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<3>> Shared;
   checkArguments("delete_MyVector3",nargout,nargin,1);
@@ -547,7 +562,7 @@ void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const m
   }
 }
 
-void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -556,7 +571,7 @@ void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int n
   collector_MyVector12.insert(self);
 }
 
-void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyVector<12>> Shared;
@@ -567,7 +582,7 @@ void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mx
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MyVector<12>> Shared;
   checkArguments("delete_MyVector12",nargout,nargin,1);
@@ -580,7 +595,7 @@ void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const
   }
 }
 
-void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
@@ -589,7 +604,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArr
   collector_MultipleTemplatesIntDouble.insert(self);
 }
 
-void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MultipleTemplates<int, double>> Shared;
   checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1);
@@ -602,7 +617,7 @@ void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], in
   }
 }
 
-void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
@@ -611,7 +626,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArra
   collector_MultipleTemplatesIntFloat.insert(self);
 }
 
-void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<MultipleTemplates<int, float>> Shared;
   checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1);
@@ -624,7 +639,45 @@ void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int
   }
 }
 
-void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_ForwardKinematics.insert(self);
+}
+
+void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+
+  gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], "ptr_gtdynamicsRobot");
+  string& start_link_name = *unwrap_shared_ptr< string >(in[1], "ptr_string");
+  string& end_link_name = *unwrap_shared_ptr< string >(in[2], "ptr_string");
+  gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], "ptr_gtsamValues");
+  gtsam::Pose3& l2Tp = *unwrap_shared_ptr< gtsam::Pose3 >(in[4], "ptr_gtsamPose3");
+  Shared *self = new Shared(new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,l2Tp));
+  collector_ForwardKinematics.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<ForwardKinematics> Shared;
+  checkArguments("delete_ForwardKinematics",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_ForwardKinematics::iterator item;
+  item = collector_ForwardKinematics.find(self);
+  if(item != collector_ForwardKinematics.end()) {
+    delete self;
+    collector_ForwardKinematics.erase(item);
+  }
+}
+
+void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -633,7 +686,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[
   collector_MyFactorPosePoint2.insert(self);
 }
 
-void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>> Shared;
@@ -648,7 +701,7 @@ void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin,
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_deconstructor_58(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);
@@ -661,7 +714,7 @@ void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin
   }
 }
 
-void MyFactorPosePoint2_print_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void MyFactorPosePoint2_print_59(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");
@@ -737,58 +790,58 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       Test_get_container_17(nargout, out, nargin-1, in+1);
       break;
     case 18:
-      Test_print_18(nargout, out, nargin-1, in+1);
+      Test_lambda_18(nargout, out, nargin-1, in+1);
       break;
     case 19:
-      Test_return_Point2Ptr_19(nargout, out, nargin-1, in+1);
+      Test_print_19(nargout, out, nargin-1, in+1);
       break;
     case 20:
-      Test_return_Test_20(nargout, out, nargin-1, in+1);
+      Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1);
       break;
     case 21:
-      Test_return_TestPtr_21(nargout, out, nargin-1, in+1);
+      Test_return_Test_21(nargout, out, nargin-1, in+1);
       break;
     case 22:
-      Test_return_bool_22(nargout, out, nargin-1, in+1);
+      Test_return_TestPtr_22(nargout, out, nargin-1, in+1);
       break;
     case 23:
-      Test_return_double_23(nargout, out, nargin-1, in+1);
+      Test_return_bool_23(nargout, out, nargin-1, in+1);
       break;
     case 24:
-      Test_return_field_24(nargout, out, nargin-1, in+1);
+      Test_return_double_24(nargout, out, nargin-1, in+1);
       break;
     case 25:
-      Test_return_int_25(nargout, out, nargin-1, in+1);
+      Test_return_field_25(nargout, out, nargin-1, in+1);
       break;
     case 26:
-      Test_return_matrix1_26(nargout, out, nargin-1, in+1);
+      Test_return_int_26(nargout, out, nargin-1, in+1);
       break;
     case 27:
-      Test_return_matrix2_27(nargout, out, nargin-1, in+1);
+      Test_return_matrix1_27(nargout, out, nargin-1, in+1);
       break;
     case 28:
-      Test_return_pair_28(nargout, out, nargin-1, in+1);
+      Test_return_matrix2_28(nargout, out, nargin-1, in+1);
       break;
     case 29:
       Test_return_pair_29(nargout, out, nargin-1, in+1);
       break;
     case 30:
-      Test_return_ptrs_30(nargout, out, nargin-1, in+1);
+      Test_return_pair_30(nargout, out, nargin-1, in+1);
       break;
     case 31:
-      Test_return_size_t_31(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_31(nargout, out, nargin-1, in+1);
       break;
     case 32:
-      Test_return_string_32(nargout, out, nargin-1, in+1);
+      Test_return_size_t_32(nargout, out, nargin-1, in+1);
       break;
     case 33:
-      Test_return_vector1_33(nargout, out, nargin-1, in+1);
+      Test_return_string_33(nargout, out, nargin-1, in+1);
       break;
     case 34:
-      Test_return_vector2_34(nargout, out, nargin-1, in+1);
+      Test_return_vector1_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      Test_set_container_35(nargout, out, nargin-1, in+1);
+      Test_return_vector2_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
       Test_set_container_36(nargout, out, nargin-1, in+1);
@@ -797,58 +850,70 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       Test_set_container_37(nargout, out, nargin-1, in+1);
       break;
     case 38:
-      PrimitiveRefDouble_collectorInsertAndMakeBase_38(nargout, out, nargin-1, in+1);
+      Test_set_container_38(nargout, out, nargin-1, in+1);
       break;
     case 39:
-      PrimitiveRefDouble_constructor_39(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1);
       break;
     case 40:
-      PrimitiveRefDouble_deconstructor_40(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1);
       break;
     case 41:
-      PrimitiveRefDouble_Brutal_41(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1);
       break;
     case 42:
-      MyVector3_collectorInsertAndMakeBase_42(nargout, out, nargin-1, in+1);
+      PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1);
       break;
     case 43:
-      MyVector3_constructor_43(nargout, out, nargin-1, in+1);
+      MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1);
       break;
     case 44:
-      MyVector3_deconstructor_44(nargout, out, nargin-1, in+1);
+      MyVector3_constructor_44(nargout, out, nargin-1, in+1);
       break;
     case 45:
-      MyVector12_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1);
+      MyVector3_deconstructor_45(nargout, out, nargin-1, in+1);
       break;
     case 46:
-      MyVector12_constructor_46(nargout, out, nargin-1, in+1);
+      MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1);
       break;
     case 47:
-      MyVector12_deconstructor_47(nargout, out, nargin-1, in+1);
+      MyVector12_constructor_47(nargout, out, nargin-1, in+1);
       break;
     case 48:
-      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1);
+      MyVector12_deconstructor_48(nargout, out, nargin-1, in+1);
       break;
     case 49:
-      MultipleTemplatesIntDouble_deconstructor_49(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1);
       break;
     case 50:
-      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1);
       break;
     case 51:
-      MultipleTemplatesIntFloat_deconstructor_51(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1);
       break;
     case 52:
-      MyFactorPosePoint2_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1);
+      MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1);
       break;
     case 53:
-      MyFactorPosePoint2_constructor_53(nargout, out, nargin-1, in+1);
+      ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1);
       break;
     case 54:
-      MyFactorPosePoint2_deconstructor_54(nargout, out, nargin-1, in+1);
+      ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1);
       break;
     case 55:
-      MyFactorPosePoint2_print_55(nargout, out, nargin-1, in+1);
+      ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1);
+      break;
+    case 56:
+      MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1);
+      break;
+    case 57:
+      MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1);
+      break;
+    case 58:
+      MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1);
+      break;
+    case 59:
+      MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/expected/matlab/functions_wrapper.cpp

@@ -33,6 +33,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 
@@ -90,6 +92,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -198,9 +206,10 @@ void MultiTemplatedFunctionDoubleSize_tDouble_7(int nargout, mxArray *out[], int
 }
 void DefaultFuncInt_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("DefaultFuncInt",nargout,nargin,1);
+  checkArguments("DefaultFuncInt",nargout,nargin,2);
   int a = unwrap< int >(in[0]);
-  DefaultFuncInt(a);
+  int b = unwrap< int >(in[1]);
+  DefaultFuncInt(a,b);
 }
 void DefaultFuncString_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
@@ -215,7 +224,30 @@ void DefaultFuncObj_10(int nargout, mxArray *out[], int nargin, const mxArray *i
   gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[0], "ptr_gtsamKeyFormatter");
   DefaultFuncObj(keyFormatter);
 }
-void TemplatedFunctionRot3_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void DefaultFuncZero_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("DefaultFuncZero",nargout,nargin,5);
+  int a = unwrap< int >(in[0]);
+  int b = unwrap< int >(in[1]);
+  double c = unwrap< double >(in[2]);
+  bool d = unwrap< bool >(in[3]);
+  bool e = unwrap< bool >(in[4]);
+  DefaultFuncZero(a,b,c,d,e);
+}
+void DefaultFuncVector_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("DefaultFuncVector",nargout,nargin,2);
+  std::vector<int>& i = *unwrap_shared_ptr< std::vector<int> >(in[0], "ptr_stdvectorint");
+  std::vector<string>& s = *unwrap_shared_ptr< std::vector<string> >(in[1], "ptr_stdvectorstring");
+  DefaultFuncVector(i,s);
+}
+void setPose_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("setPose",nargout,nargin,1);
+  gtsam::Pose3& pose = *unwrap_shared_ptr< gtsam::Pose3 >(in[0], "ptr_gtsamPose3");
+  setPose(pose);
+}
+void TemplatedFunctionRot3_14(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("TemplatedFunctionRot3",nargout,nargin,1);
   gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3");
@@ -267,7 +299,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       DefaultFuncObj_10(nargout, out, nargin-1, in+1);
       break;
     case 11:
-      TemplatedFunctionRot3_11(nargout, out, nargin-1, in+1);
+      DefaultFuncZero_11(nargout, out, nargin-1, in+1);
+      break;
+    case 12:
+      DefaultFuncVector_12(nargout, out, nargin-1, in+1);
+      break;
+    case 13:
+      setPose_13(nargout, out, nargin-1, in+1);
+      break;
+    case 14:
+      TemplatedFunctionRot3_14(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/expected/matlab/geometry_wrapper.cpp

@@ -36,6 +36,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@@ -97,6 +99,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;

wrap/tests/expected/matlab/inheritance_wrapper.cpp

@@ -38,6 +38,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@@ -107,6 +109,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -564,12 +572,12 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx
   out[0] = wrap_shared_ptr(boost::make_shared<MyTemplate<Matrix>>(MyTemplate<gtsam::Matrix>::Level(K)),"MyTemplateMatrix", false);
 }
 
-void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  checkArguments("set_container",nargout,nargin-1,1);
+  checkArguments("return_vector2",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");
+  Vector value = unwrap< Vector >(in[1]);
-  obj->set_container(*container);
+  out[0] = wrap< Vector >(obj->return_vector2(value));
 }
 
 void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
@@ -716,7 +724,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      Test_set_container_35(nargout, out, nargin-1, in+1);
+      Test_return_vector2_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
       ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1);

wrap/tests/expected/matlab/namespaces_wrapper.cpp

@@ -8,6 +8,7 @@
 #include <folder/path/to/Test.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/Values.h>
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
 #include <path/to/ns2.h>
@@ -43,6 +44,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@@ -69,6 +72,8 @@ typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
 static Collector_ns2ClassC collector_ns2ClassC;
 typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
 static Collector_ClassD collector_ClassD;
+typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
+static Collector_gtsamValues collector_gtsamValues;
 
 void _deleteAllObjects()
 {
@@ -124,6 +129,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -202,6 +213,12 @@ void _deleteAllObjects()
     collector_ClassD.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin();
+      iter != collector_gtsamValues.end(); ) {
+    delete *iter;
+    collector_gtsamValues.erase(iter++);
+    anyDeleted = true;
+  } }
   if(anyDeleted)
     cout <<
       "WARNING:  Wrap modules with variables in the workspace have been reloaded due to\n"
@@ -491,6 +508,69 @@ void ClassD_deconstructor_25(int nargout, mxArray *out[], int nargin, const mxAr
   }
 }
 
+void gtsamValues_collectorInsertAndMakeBase_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));
+  collector_gtsamValues.insert(self);
+}
+
+void gtsamValues_constructor_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  Shared *self = new Shared(new gtsam::Values());
+  collector_gtsamValues.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamValues_constructor_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  mexAtExit(&_deleteAllObjects);
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+
+  gtsam::Values& other = *unwrap_shared_ptr< gtsam::Values >(in[0], "ptr_gtsamValues");
+  Shared *self = new Shared(new gtsam::Values(other));
+  collector_gtsamValues.insert(self);
+  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
+  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
+}
+
+void gtsamValues_deconstructor_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  typedef boost::shared_ptr<gtsam::Values> Shared;
+  checkArguments("delete_gtsamValues",nargout,nargin,1);
+  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));
+  Collector_gtsamValues::iterator item;
+  item = collector_gtsamValues.find(self);
+  if(item != collector_gtsamValues.end()) {
+    delete self;
+    collector_gtsamValues.erase(item);
+  }
+}
+
+void gtsamValues_insert_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("insert",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<gtsam::Values>(in[0], "ptr_gtsamValues");
+  size_t j = unwrap< size_t >(in[1]);
+  Vector vector = unwrap< Vector >(in[2]);
+  obj->insert(j,vector);
+}
+
+void gtsamValues_insert_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("insert",nargout,nargin-1,2);
+  auto obj = unwrap_shared_ptr<gtsam::Values>(in[0], "ptr_gtsamValues");
+  size_t j = unwrap< size_t >(in[1]);
+  Matrix matrix = unwrap< Matrix >(in[2]);
+  obj->insert(j,matrix);
+}
+
 
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
@@ -581,6 +661,24 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
     case 25:
       ClassD_deconstructor_25(nargout, out, nargin-1, in+1);
       break;
+    case 26:
+      gtsamValues_collectorInsertAndMakeBase_26(nargout, out, nargin-1, in+1);
+      break;
+    case 27:
+      gtsamValues_constructor_27(nargout, out, nargin-1, in+1);
+      break;
+    case 28:
+      gtsamValues_constructor_28(nargout, out, nargin-1, in+1);
+      break;
+    case 29:
+      gtsamValues_deconstructor_29(nargout, out, nargin-1, in+1);
+      break;
+    case 30:
+      gtsamValues_insert_30(nargout, out, nargin-1, in+1);
+      break;
+    case 31:
+      gtsamValues_insert_31(nargout, out, nargin-1, in+1);
+      break;
     }
   } catch(const std::exception& e) {
     mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());

wrap/tests/expected/matlab/special_cases_wrapper.cpp

@@ -9,6 +9,7 @@
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/nonlinear/Values.h>
 #include <path/to/ns1.h>
 #include <path/to/ns1/ClassB.h>
 #include <path/to/ns2.h>
@@ -46,6 +47,8 @@ typedef std::set<boost::shared_ptr<MultipleTemplatesIntDouble>*> Collector_Multi
 static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble;
 typedef std::set<boost::shared_ptr<MultipleTemplatesIntFloat>*> Collector_MultipleTemplatesIntFloat;
 static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat;
+typedef std::set<boost::shared_ptr<ForwardKinematics>*> Collector_ForwardKinematics;
+static Collector_ForwardKinematics collector_ForwardKinematics;
 typedef std::set<boost::shared_ptr<MyFactorPosePoint2>*> Collector_MyFactorPosePoint2;
 static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2;
 typedef std::set<boost::shared_ptr<gtsam::Point2>*> Collector_gtsamPoint2;
@@ -72,6 +75,8 @@ typedef std::set<boost::shared_ptr<ns2::ClassC>*> Collector_ns2ClassC;
 static Collector_ns2ClassC collector_ns2ClassC;
 typedef std::set<boost::shared_ptr<ClassD>*> Collector_ClassD;
 static Collector_ClassD collector_ClassD;
+typedef std::set<boost::shared_ptr<gtsam::Values>*> Collector_gtsamValues;
+static Collector_gtsamValues collector_gtsamValues;
 typedef std::set<boost::shared_ptr<gtsam::NonlinearFactorGraph>*> Collector_gtsamNonlinearFactorGraph;
 static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph;
 typedef std::set<boost::shared_ptr<gtsam::SfmTrack>*> Collector_gtsamSfmTrack;
@@ -135,6 +140,12 @@ void _deleteAllObjects()
     collector_MultipleTemplatesIntFloat.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin();
+      iter != collector_ForwardKinematics.end(); ) {
+    delete *iter;
+    collector_ForwardKinematics.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin();
       iter != collector_MyFactorPosePoint2.end(); ) {
     delete *iter;
@@ -213,6 +224,12 @@ void _deleteAllObjects()
     collector_ClassD.erase(iter++);
     anyDeleted = true;
   } }
+  { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin();
+      iter != collector_gtsamValues.end(); ) {
+    delete *iter;
+    collector_gtsamValues.erase(iter++);
+    anyDeleted = true;
+  } }
   { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin();
       iter != collector_gtsamNonlinearFactorGraph.end(); ) {
     delete *iter;

wrap/tests/expected/python/class_pybind.cpp

@@ -55,13 +55,14 @@ PYBIND11_MODULE(class_py, m_) {
         .def("create_ptrs",[](Test* self){return self->create_ptrs();})
         .def("create_MixedPtrs",[](Test* self){return self->create_MixedPtrs();})
         .def("return_ptrs",[](Test* self, std::shared_ptr<Test> p1, std::shared_ptr<Test> p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2"))
-        .def("print_",[](Test* self){ py::scoped_ostream_redirect output; self->print();})
+        .def("print",[](Test* self){ py::scoped_ostream_redirect output; self->print();})
         .def("__repr__",
                     [](const Test& self){
                         gtsam::RedirectCout redirect;
                         self.print();
                         return redirect.str();
                     })
+        .def("lambda_",[](Test* self){ self->lambda();})
         .def("set_container",[](Test* self, std::vector<testing::Test> container){ self->set_container(container);}, py::arg("container"))
         .def("set_container",[](Test* self, std::vector<std::shared_ptr<testing::Test>> container){ self->set_container(container);}, py::arg("container"))
         .def("set_container",[](Test* self, std::vector<testing::Test&> container){ self->set_container(container);}, py::arg("container"))
@@ -82,9 +83,12 @@ PYBIND11_MODULE(class_py, m_) {
 
     py::class_<MultipleTemplates<int, float>, std::shared_ptr<MultipleTemplates<int, float>>>(m_, "MultipleTemplatesIntFloat");
 
+    py::class_<ForwardKinematics, std::shared_ptr<ForwardKinematics>>(m_, "ForwardKinematics")
+        .def(py::init<const gtdynamics::Robot&, const string&, const string&, const gtsam::Values&, const gtsam::Pose3&>(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3());
+
     py::class_<MyFactor<gtsam::Pose2, gtsam::Matrix>, std::shared_ptr<MyFactor<gtsam::Pose2, gtsam::Matrix>>>(m_, "MyFactorPosePoint2")
         .def(py::init<size_t, size_t, double, const std::shared_ptr<gtsam::noiseModel::Base>>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel"))
-        .def("print_",[](MyFactor<gtsam::Pose2, gtsam::Matrix>* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter)
+        .def("print",[](MyFactor<gtsam::Pose2, gtsam::Matrix>* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter)
         .def("__repr__",
                     [](const MyFactor<gtsam::Pose2, gtsam::Matrix>& self, const string& s, const gtsam::KeyFormatter& keyFormatter){
                         gtsam::RedirectCout redirect;
@@ -92,6 +96,7 @@ PYBIND11_MODULE(class_py, m_) {
                         return redirect.str();
                     }, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);
 
+    py::class_<SuperCoolFactor<gtsam::Pose3>, std::shared_ptr<SuperCoolFactor<gtsam::Pose3>>>(m_, "SuperCoolFactorPose3")
 
 #include "python/specializations.h"
 

wrap/tests/expected/python/functions_pybind.cpp

@@ -30,9 +30,12 @@ PYBIND11_MODULE(functions_py, m_) {
     m_.def("overloadedGlobalFunction",[](int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b"));
     m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<string,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
     m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction<double,size_t,double>(x, y);}, py::arg("x"), py::arg("y"));
-    m_.def("DefaultFuncInt",[](int a){ ::DefaultFuncInt(a);}, py::arg("a") = 123);
+    m_.def("DefaultFuncInt",[](int a, int b){ ::DefaultFuncInt(a, b);}, py::arg("a") = 123, py::arg("b") = 0);
     m_.def("DefaultFuncString",[](const string& s, const string& name){ ::DefaultFuncString(s, name);}, py::arg("s") = "hello", py::arg("name") = "");
     m_.def("DefaultFuncObj",[](const gtsam::KeyFormatter& keyFormatter){ ::DefaultFuncObj(keyFormatter);}, py::arg("keyFormatter") = gtsam::DefaultKeyFormatter);
+    m_.def("DefaultFuncZero",[](int a, int b, double c, bool d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a") = 0, py::arg("b"), py::arg("c") = 0.0, py::arg("d") = false, py::arg("e"));
+    m_.def("DefaultFuncVector",[](const std::vector<int>& i, const std::vector<string>& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"});
+    m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3());
     m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction<Rot3>(t);}, py::arg("t"));
 
 #include "python/specializations.h"

wrap/tests/expected/python/namespaces_pybind.cpp

@@ -11,6 +11,7 @@
 #include "path/to/ns2.h"
 #include "path/to/ns2/ClassA.h"
 #include "path/to/ns3.h"
+#include "gtsam/nonlinear/Values.h"
 
 #include "wrap/serialization.h"
 #include <boost/serialization/export.hpp>
@@ -57,7 +58,16 @@ PYBIND11_MODULE(namespaces_py, m_) {
     py::class_<ClassD, std::shared_ptr<ClassD>>(m_, "ClassD")
         .def(py::init<>());
 
-    m_.attr("aGlobalVar") = aGlobalVar;
+    m_.attr("aGlobalVar") = aGlobalVar;    pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule");
+
+    py::class_<gtsam::Values, std::shared_ptr<gtsam::Values>>(m_gtsam, "Values")
+        .def(py::init<>())
+        .def(py::init<const gtsam::Values&>(), py::arg("other"))
+        .def("insert_vector",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector"))
+        .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector"))
+        .def("insert_matrix",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix"))
+        .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix"));
+
 
 #include "python/specializations.h"
 

wrap/tests/fixtures/class.i

@@ -61,7 +61,10 @@ class Test {
   pair<Test ,Test*> create_MixedPtrs () const;
   pair<Test*,Test*> return_ptrs (Test* p1, Test* p2) const;
 
+  // This should be callable as .print() in python
   void print() const;
+  // Since this is a reserved keyword, it should be updated to `lambda_`
+  void lambda() const;
 
   void set_container(std::vector<testing::Test> container);
   void set_container(std::vector<testing::Test*> container);
@@ -106,3 +109,14 @@ class MyVector {
 // Class with multiple instantiated templates
 template<T = {int}, U = {double, float}>
 class MultipleTemplates {};
+
+// Test for default args in constructor
+class ForwardKinematics {
+  ForwardKinematics(const gtdynamics::Robot& robot,
+                    const string& start_link_name, const string& end_link_name,
+                    const gtsam::Values& joint_angles,
+                    const gtsam::Pose3& l2Tp = gtsam::Pose3());
+};
+
+class SuperCoolFactor;
+typedef SuperCoolFactor<gtsam::Pose3> SuperCoolFactorPose3;

wrap/tests/fixtures/functions.i

@@ -28,6 +28,11 @@ void TemplatedFunction(const T& t);
 typedef TemplatedFunction<gtsam::Rot3> TemplatedFunctionRot3;
 
 // Check default arguments
-void DefaultFuncInt(int a = 123);
+void DefaultFuncInt(int a = 123, int b = 0);
 void DefaultFuncString(const string& s = "hello", const string& name = "");
 void DefaultFuncObj(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void DefaultFuncZero(int a = 0, int b, double c = 0.0, bool d = false, bool e);
+void DefaultFuncVector(const std::vector<int> &i = {1, 2, 3}, const std::vector<string> &s = {"borglab", "gtsam"});
+
+// Test for non-trivial default constructor
+void setPose(const gtsam::Pose3& pose = gtsam::Pose3());

wrap/tests/fixtures/namespaces.i

@@ -60,3 +60,14 @@ class ClassD {
 };
 
 int aGlobalVar;
+
+namespace gtsam {
+  #include <gtsam/nonlinear/Values.h>
+class Values {
+  Values();
+  Values(const gtsam::Values& other);
+
+  void insert(size_t j, Vector vector);
+  void insert(size_t j, Matrix matrix);
+};
+}

wrap/tests/test_docs.py

@@ -41,7 +41,6 @@ class TestDocument(unittest.TestCase):
     OUTPUT_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, OUTPUT_XML_DIR))
     EXPECTED_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, EXPECTED_XML_DIR))
 
-    @unittest.skip("DOC_DIR_PATH doesn't exist")
     def test_generate_xml(self):
         """Test parse_xml.generate_xml"""
         if path.exists(self.OUTPUT_XML_DIR_PATH):
@@ -65,7 +64,6 @@ class TestDocument(unittest.TestCase):
 
         self.assertTrue(not dircmp.diff_files and not dircmp.funny_files)
 
-    @unittest.skip("DOC_DIR_PATH doesn't exist")
     def test_parse(self):
         """Test the parsing of the XML generated by Doxygen."""
         docs = parser.ParseDoxygenXML(self.DOC_DIR_PATH,

wrap/tests/test_interface_parser.py

@@ -142,9 +142,9 @@ class TestInterfaceParser(unittest.TestCase):
             "const C6* c6"
         args = ArgumentList.rule.parseString(arg_string)[0]
 
-        self.assertEqual(7, len(args.args_list))
+        self.assertEqual(7, len(args.list()))
         self.assertEqual(['a', 'c1', 'c2', 'c3', 'c4', 'c5', 'c6'],
-                         args.args_names())
+                         args.names())
 
     def test_argument_list_qualifiers(self):
         """
@@ -153,7 +153,7 @@ class TestInterfaceParser(unittest.TestCase):
         """
         arg_string = "double x1, double* x2, double& x3, double@ x4, " \
             "const double x5, const double* x6, const double& x7, const double@ x8"
-        args = ArgumentList.rule.parseString(arg_string)[0].args_list
+        args = ArgumentList.rule.parseString(arg_string)[0].list()
         self.assertEqual(8, len(args))
         self.assertFalse(args[1].ctype.is_ptr and args[1].ctype.is_shared_ptr
                          and args[1].ctype.is_ref)
@@ -169,7 +169,7 @@ class TestInterfaceParser(unittest.TestCase):
         """Test arguments list where the arguments can be templated."""
         arg_string = "std::pair<string, double> steps, vector<T*> vector_of_pointers"
         args = ArgumentList.rule.parseString(arg_string)[0]
-        args_list = args.args_list
+        args_list = args.list()
         self.assertEqual(2, len(args_list))
         self.assertEqual("std::pair<string, double>",
                          args_list[0].ctype.to_cpp(False))
@@ -180,30 +180,62 @@ class TestInterfaceParser(unittest.TestCase):
 
     def test_default_arguments(self):
         """Tests any expression that is a valid default argument"""
-        args = ArgumentList.rule.parseString(
+        args = ArgumentList.rule.parseString("""
-            "string c = \"\", string s=\"hello\", int a=3, "
+            string c = "", int z = 0, double z2 = 0.0, bool f = false,
-            "int b, double pi = 3.1415, "
+            string s="hello"+"goodbye", char c='a', int a=3,
-            "gtsam::KeyFormatter kf = gtsam::DefaultKeyFormatter, "
+            int b, double pi = 3.1415""")[0].list()
-            "std::vector<size_t> p = std::vector<size_t>(), "
-            "std::vector<size_t> l = (1, 2, 'name', \"random\", 3.1415)"
-        )[0].args_list
 
         # Test for basic types
-        self.assertEqual(args[0].default, "")
+        self.assertEqual(args[0].default, '""')
-        self.assertEqual(args[1].default, "hello")
+        self.assertEqual(args[1].default, '0')
-        self.assertEqual(args[2].default, 3)
+        self.assertEqual(args[2].default, '0.0')
+        self.assertEqual(args[3].default, "false")
+        self.assertEqual(args[4].default, '"hello"+"goodbye"')
+        self.assertEqual(args[5].default, "'a'")
+        self.assertEqual(args[6].default, '3')
         # No default argument should set `default` to None
-        self.assertIsNone(args[3].default)
+        self.assertIsNone(args[7].default)
+        self.assertEqual(args[8].default, '3.1415')
 
-        self.assertEqual(args[4].default, 3.1415)
+        arg0 = 'gtsam::DefaultKeyFormatter'
+        arg1 = 'std::vector<size_t>()'
+        arg2 = '{1, 2}'
+        arg3 = '[&c1, &c2](string s=5, int a){return s+"hello"+a+c1+c2;}'
+        arg4 = 'gtsam::Pose3()'
+        arg5 = 'Factor<gtsam::Pose3, gtsam::Point3>()'
+        arg6 = 'gtsam::Point3(1, 2, 3)'
+        arg7 = 'ns::Class<T, U>(3, 2, 1, "name")'
+
+        argument_list = """
+            gtsam::KeyFormatter kf = {arg0},
+            std::vector<size_t> v = {arg1},
+            std::vector<size_t> l = {arg2},
+            gtsam::KeyFormatter lambda = {arg3},
+            gtsam::Pose3 p = {arg4},
+            Factor<gtsam::Pose3, gtsam::Point3> x = {arg5},
+            gtsam::Point3 x = {arg6},
+            ns::Class<T, U> obj = {arg7}
+            """.format(arg0=arg0,
+                       arg1=arg1,
+                       arg2=arg2,
+                       arg3=arg3,
+                       arg4=arg4,
+                       arg5=arg5,
+                       arg6=arg6,
+                       arg7=arg7)
+        args = ArgumentList.rule.parseString(argument_list)[0].list()
 
         # Test non-basic type
-        self.assertEqual(repr(args[5].default.typename),
+        self.assertEqual(args[0].default, arg0)
-                         'gtsam::DefaultKeyFormatter')
         # Test templated type
-        self.assertEqual(repr(args[6].default.typename), 'std::vector<size_t>')
+        self.assertEqual(args[1].default, arg1)
-        # Test for allowing list as default argument
+        self.assertEqual(args[2].default, arg2)
-        self.assertEqual(args[7].default, (1, 2, 'name', "random", 3.1415))
+        self.assertEqual(args[3].default, arg3)
+        self.assertEqual(args[4].default, arg4)
+        self.assertEqual(args[5].default, arg5)
+        self.assertEqual(args[6].default, arg6)
+        # Test for default argument with multiple templates and params
+        self.assertEqual(args[7].default, arg7)
 
     def test_return_type(self):
         """Test ReturnType"""
@@ -273,6 +305,15 @@ class TestInterfaceParser(unittest.TestCase):
         self.assertEqual("f", ret.name)
         self.assertEqual(3, len(ret.args))
 
+        ret = Constructor.rule.parseString(
+            """ForwardKinematics(const gtdynamics::Robot& robot,
+                    const string& start_link_name, const string& end_link_name,
+                    const gtsam::Values& joint_angles,
+                    const gtsam::Pose3& l2Tp = gtsam::Pose3());""")[0]
+        self.assertEqual("ForwardKinematics", ret.name)
+        self.assertEqual(5, len(ret.args))
+        self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default)
+
     def test_operator_overload(self):
         """Test for operator overloading."""
         # Unary operator
@@ -296,7 +337,7 @@ class TestInterfaceParser(unittest.TestCase):
                          ret.return_type.type1.typename.to_cpp())
         self.assertTrue(len(ret.args) == 1)
         self.assertEqual("const gtsam::Vector2 &",
-                         repr(ret.args.args_list[0].ctype))
+                         repr(ret.args.list()[0].ctype))
         self.assertTrue(not ret.is_unary)
 
     def test_typedef_template_instantiation(self):
@@ -392,6 +433,16 @@ class TestInterfaceParser(unittest.TestCase):
         self.assertEqual(0, len(ret.properties))
         self.assertTrue(not ret.is_virtual)
 
+    def test_templated_class(self):
+        """Test a templated class."""
+        ret = Class.rule.parseString("""
+        template<POSE, POINT>
+        class MyFactor {};
+        """)[0]
+
+        self.assertEqual("MyFactor", ret.name)
+        self.assertEqual("<POSE, POINT>", repr(ret.template))
+
     def test_class_inheritance(self):
         """Test for class inheritance."""
         ret = Class.rule.parseString("""
@@ -446,8 +497,7 @@ class TestInterfaceParser(unittest.TestCase):
         fwd = ForwardDeclaration.rule.parseString(
             "virtual class Test:gtsam::Point3;")[0]
 
-        fwd_name = fwd.name
+        self.assertEqual("Test", fwd.name)
-        self.assertEqual("Test", fwd_name.name)
         self.assertTrue(fwd.is_virtual)
 
     def test_function(self):
@@ -469,14 +519,26 @@ class TestInterfaceParser(unittest.TestCase):
         variable = Variable.rule.parseString("string kGravity = 9.81;")[0]
         self.assertEqual(variable.name, "kGravity")
         self.assertEqual(variable.ctype.typename.name, "string")
-        self.assertEqual(variable.default, 9.81)
+        self.assertEqual(variable.default, "9.81")
 
         variable = Variable.rule.parseString(
             "const string kGravity = 9.81;")[0]
         self.assertEqual(variable.name, "kGravity")
         self.assertEqual(variable.ctype.typename.name, "string")
         self.assertTrue(variable.ctype.is_const)
-        self.assertEqual(variable.default, 9.81)
+        self.assertEqual(variable.default, "9.81")
+
+        variable = Variable.rule.parseString(
+            "gtsam::Pose3 wTc = gtsam::Pose3();")[0]
+        self.assertEqual(variable.name, "wTc")
+        self.assertEqual(variable.ctype.typename.name, "Pose3")
+        self.assertEqual(variable.default, "gtsam::Pose3()")
+
+        variable = Variable.rule.parseString(
+            "gtsam::Pose3 wTc = gtsam::Pose3(1, 2, 0);")[0]
+        self.assertEqual(variable.name, "wTc")
+        self.assertEqual(variable.ctype.typename.name, "Pose3")
+        self.assertEqual(variable.default, "gtsam::Pose3(1, 2, 0)")
 
     def test_enumerator(self):
         """Test for enumerator."""

wrap/tests/test_matlab_wrapper.py

@@ -15,8 +15,6 @@ from loguru import logger
 
 sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
 
-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.matlab_wrapper import MatlabWrapper
 
 
@@ -117,19 +115,14 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         # Create MATLAB wrapper instance
         wrapper = MatlabWrapper(
-            module=module,
             module_name='geometry',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -148,18 +141,13 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         wrapper = MatlabWrapper(
-            module=module,
             module_name='functions',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -181,18 +169,13 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         wrapper = MatlabWrapper(
-            module=module,
             module_name='class',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -214,18 +197,13 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         wrapper = MatlabWrapper(
-            module=module,
             module_name='inheritance',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -237,7 +215,7 @@ class TestWrap(unittest.TestCase):
         for file in files:
             self.compare_and_diff(file)
 
-    def test_namspaces(self):
+    def test_namespaces(self):
         """
         Test interface file with full namespace definition.
         """
@@ -247,18 +225,13 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         wrapper = MatlabWrapper(
-            module=module,
             module_name='namespaces',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -282,18 +255,13 @@ class TestWrap(unittest.TestCase):
         if not osp.exists(self.MATLAB_ACTUAL_DIR):
             os.mkdir(self.MATLAB_ACTUAL_DIR)
 
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         wrapper = MatlabWrapper(
-            module=module,
             module_name='special_cases',
             top_module_namespace=['gtsam'],
             ignore_classes=[''],
         )
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         self.generate_content(cc_content)
 
@@ -306,5 +274,6 @@ class TestWrap(unittest.TestCase):
         for file in files:
             self.compare_and_diff(file)
 
+
 if __name__ == '__main__':
     unittest.main()

wrap/tests/test_pybind_wrapper.py

@@ -16,8 +16,6 @@ sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
 sys.path.append(
     osp.normpath(osp.abspath(osp.join(__file__, '../../../build/wrap'))))
 
-import gtwrap.interface_parser as parser
-import gtwrap.template_instantiator as instantiator
 from gtwrap.pybind_wrapper import PybindWrapper
 
 sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__))))
@@ -37,23 +35,18 @@ class TestWrap(unittest.TestCase):
         """
         Common function to wrap content.
         """
-        module = parser.Module.parseString(content)
-
-        instantiator.instantiate_namespace_inplace(module)
-
         with open(osp.join(self.TEST_DIR,
                            "pybind_wrapper.tpl")) as template_file:
             module_template = template_file.read()
 
         # Create Pybind wrapper instance
-        wrapper = PybindWrapper(module=module,
+        wrapper = PybindWrapper(module_name=module_name,
-                                module_name=module_name,
                                 use_boost=False,
                                 top_module_namespaces=[''],
                                 ignore_classes=[''],
                                 module_template=module_template)
 
-        cc_content = wrapper.wrap()
+        cc_content = wrapper.wrap(content)
 
         output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp")
 
@@ -165,10 +158,10 @@ class TestWrap(unittest.TestCase):
         with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f:
             content = f.read()
 
-        output = self.wrap_content(content, 'enum_py',
+        output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR)
-                                   self.PYTHON_ACTUAL_DIR)
 
         self.compare_and_diff('enum_pybind.cpp', output)
 
+
 if __name__ == '__main__':
     unittest.main()