Merge branch 'develop' into feature/BAD_using_charts

.cproject

@@ -2353,6 +2353,38 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSpirit.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSpirit.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.wrap" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testMethod.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testMethod.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testClass.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testClass.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -3222,22 +3254,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testSpirit.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check.wrap</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

CMakeLists.txt

@@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
 endif()
 
 # Set the version number for the library
-set (GTSAM_VERSION_MAJOR 3)
+set (GTSAM_VERSION_MAJOR 4)
-set (GTSAM_VERSION_MINOR 1)
+set (GTSAM_VERSION_MINOR 0)
 set (GTSAM_VERSION_PATCH 0)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")

examples/Data/pose3example-rewritten.txt

@@ -1,11 +0,0 @@
-VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
-VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
-VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
-VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
-VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
-EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.30338 -0.513226 0.542221 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

gtsam.h

@@ -156,8 +156,14 @@ virtual class Value {
   size_t dim() const;
 };
 
+class Vector3 {
+  Vector3(Vector v);
+};
+class Vector6 {
+  Vector6(Vector v);
+};
 #include <gtsam/base/LieScalar.h>
-virtual class LieScalar : gtsam::Value {
+class LieScalar {
   // Standard constructors
   LieScalar();
   LieScalar(double d);
@@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value {
 };
 
 #include <gtsam/base/LieVector.h>
-virtual class LieVector : gtsam::Value {
+class LieVector {
   // Standard constructors
   LieVector();
   LieVector(Vector v);
@@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value {
 };
 
 #include <gtsam/base/LieMatrix.h>
-virtual class LieMatrix : gtsam::Value {
+class LieMatrix {
   // Standard constructors
   LieMatrix();
   LieMatrix(Matrix v);
@@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value {
 // geometry
 //*************************************************************************
 
-virtual class Point2 : gtsam::Value {
+class Point2 {
   // Standard Constructors
   Point2();
   Point2(double x, double y);
@@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class StereoPoint2 : gtsam::Value {
+class StereoPoint2 {
   // Standard Constructors
   StereoPoint2();
   StereoPoint2(double uL, double uR, double v);
@@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Point3 : gtsam::Value {
+class Point3 {
   // Standard Constructors
   Point3();
   Point3(double x, double y, double z);
@@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Rot2 : gtsam::Value {
+class Rot2 {
   // Standard Constructors and Named Constructors
   Rot2();
   Rot2(double theta);
@@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Rot3 : gtsam::Value {
+class Rot3 {
   // Standard Constructors and Named Constructors
   Rot3();
   Rot3(Matrix R);
@@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Pose2 : gtsam::Value {
+class Pose2 {
   // Standard Constructor
   Pose2();
   Pose2(const gtsam::Pose2& pose);
@@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Pose3 : gtsam::Value {
+class Pose3 {
   // Standard Constructors
   Pose3();
   Pose3(const gtsam::Pose3& pose);
@@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value {
 };
 
 #include <gtsam/geometry/Unit3.h>
-virtual class Unit3 : gtsam::Value {
+class Unit3 {
   // Standard Constructors
   Unit3();
   Unit3(const gtsam::Point3& pose);
@@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value {
 };
 
 #include <gtsam/geometry/EssentialMatrix.h>
-virtual class EssentialMatrix : gtsam::Value {
+class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
@@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value {
   double error(Vector vA, Vector vB);
 };
 
-virtual class Cal3_S2 : gtsam::Value {
+class Cal3_S2 {
   // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
@@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value {
 };
 
 #include <gtsam/geometry/Cal3DS2.h>
-virtual class Cal3DS2 : gtsam::Value {
+class Cal3DS2 {
   // Standard Constructors
   Cal3DS2();
   Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@@ -699,7 +705,43 @@ class Cal3_S2Stereo {
   double baseline() const;
 };
 
-virtual class CalibratedCamera : gtsam::Value {
+#include <gtsam/geometry/Cal3Bundler.h>
+class Cal3Bundler {
+  // Standard Constructors
+  Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Bundler retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double k1() const;
+  double k2() const;
+  double u0() const;
+  double v0() const;
+  Vector vector() const;
+  Vector k() const;
+  //Matrix K() const; //FIXME: Uppercase
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+class CalibratedCamera {
   // Standard Constructors and Named Constructors
   CalibratedCamera();
   CalibratedCamera(const gtsam::Pose3& pose);
@@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value {
   void serialize() const;
 };
 
-virtual class SimpleCamera : gtsam::Value {
+class SimpleCamera {
   // Standard Constructors and Named Constructors
   SimpleCamera();
   SimpleCamera(const gtsam::Pose3& pose);
@@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value {
 };
 
 template<CALIBRATION = {gtsam::Cal3DS2}>
-virtual class PinholeCamera : gtsam::Value {
+class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
   PinholeCamera(const gtsam::Pose3& pose);
@@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value {
   void serialize() const;
 };
 
-virtual class StereoCamera : gtsam::Value {
+class StereoCamera {
   // Standard Constructors and Named Constructors
   StereoCamera();
   StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@@ -862,7 +904,7 @@ virtual class SymbolicFactor {
 };
 
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
-class SymbolicFactorGraph {
+virtual class SymbolicFactorGraph {
   SymbolicFactorGraph();
   SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
   SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@@ -1664,15 +1706,12 @@ class Values {
   void print(string s) const;
   bool equals(const gtsam::Values& other, double tol) const;
 
-  void insert(size_t j, const gtsam::Value& value);
   void insert(const gtsam::Values& values);
-  void update(size_t j, const gtsam::Value& val);
   void update(const gtsam::Values& values);
   void erase(size_t j);
   void swap(gtsam::Values& values);
 
   bool exists(size_t j) const;
-  gtsam::Value at(size_t j) const;
   gtsam::KeyList keys() const;
 
   gtsam::VectorValues zeroVectors() const;
@@ -1682,6 +1721,37 @@ class Values {
 
   // enabling serialization functionality
   void serialize() const;
+
+  // New in 4.0, we have to specialize every insert/update/at to generate wrappers
+  // Instead of the old:
+  // void insert(size_t j, const gtsam::Value& value);
+  // void update(size_t j, const gtsam::Value& val);
+  // gtsam::Value at(size_t j) const;
+  void insert(size_t j, const gtsam::Point2& t);
+  void insert(size_t j, const gtsam::Point3& t);
+  void insert(size_t j, const gtsam::Rot2& t);
+  void insert(size_t j, const gtsam::Pose2& t);
+  void insert(size_t j, const gtsam::Rot3& t);
+  void insert(size_t j, const gtsam::Pose3& t);
+  void insert(size_t j, const gtsam::Cal3_S2& t);
+  void insert(size_t j, const gtsam::Cal3DS2& t);
+  void insert(size_t j, const gtsam::Cal3Bundler& t);
+  void insert(size_t j, const gtsam::EssentialMatrix& t);
+
+  void update(size_t j, const gtsam::Point2& t);
+  void update(size_t j, const gtsam::Point3& t);
+  void update(size_t j, const gtsam::Rot2& t);
+  void update(size_t j, const gtsam::Pose2& t);
+  void update(size_t j, const gtsam::Rot3& t);
+  void update(size_t j, const gtsam::Pose3& t);
+  void update(size_t j, const gtsam::Cal3_S2& t);
+  void update(size_t j, const gtsam::Cal3DS2& t);
+  void update(size_t j, const gtsam::Cal3Bundler& t);
+  void update(size_t j, const gtsam::EssentialMatrix& t);
+
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
+      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2}>
+  T at(size_t j);
 };
 
 // Actually a FastList<Key>
@@ -2077,7 +2147,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/slam/PriorFactor.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/slam/BetweenFactor.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
   T measured() const;
@@ -2099,7 +2169,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2280,7 +2350,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
 namespace imuBias {
 #include <gtsam/navigation/ImuBias.h>
 
-virtual class ConstantBias : gtsam::Value {
+class ConstantBias {
   // Standard Constructor
   ConstantBias();
   ConstantBias(Vector biasAcc, Vector biasGyro);
@@ -2340,7 +2410,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+  void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
       const gtsam::imuBias::ConstantBias& bias,
       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis) const;
@@ -2383,7 +2453,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+  void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
       const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis) const;

gtsam/3rdparty/gtsam_eigen_includes.h.in

@@ -27,3 +27,7 @@
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>
+
+
+
+

gtsam/base/GenericValue.h

@@ -70,9 +70,9 @@ struct print<double> {
 };
 
 // equals for Matrix types
-template<int M, int N, int Options>
+template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct equals<Eigen::Matrix<double, M, N, Options> > {
+struct equals<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
-  typedef Eigen::Matrix<double, M, N, Options> type;
+  typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
   typedef bool result_type;
   bool operator()(const type& A, const type& B, double tol) {
     return equal_with_abs_tol(A, B, tol);
@@ -80,9 +80,9 @@ struct equals<Eigen::Matrix<double, M, N, Options> > {
 };
 
 // print for Matrix types
-template<int M, int N, int Options>
+template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct print<Eigen::Matrix<double, M, N, Options> > {
+struct print<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
-  typedef Eigen::Matrix<double, M, N, Options> type;
+  typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
   typedef void result_type;
   void operator()(const type& A, const std::string& str) {
     std::cout << str << ": " << A << std::endl;

gtsam/base/Manifold.h

@@ -253,10 +253,10 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "DefaultChart has not been implemented yet for dynamically sized matrices");
   static vector local(const T& origin, const T& other) {
-    return reshape<vector::RowsAtCompileTime, 1>(other) - reshape<vector::RowsAtCompileTime, 1>(origin);
+    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
   }
   static T retract(const T& origin, const vector& d) {
-    return origin + reshape<M, N>(d);
+    return origin + reshape<M, N, Options>(d);
   }
   static int getDimension(const T&origin) {
     return origin.rows() * origin.cols();

gtsam/base/Matrix.h

@@ -294,10 +294,10 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
 inline Matrix trans(const Matrix& A) { return A.transpose(); }
 
 /// Reshape functor
-template <int OutM, int OutN, int InM, int InN, int InOptions>
+template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
 struct Reshape {
   //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
-  typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN> > ReshapedType;
+  typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
   static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
     return in.data();
   }
@@ -305,7 +305,7 @@ struct Reshape {
 
 /// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output)
 template <int M, int InOptions>
-struct Reshape<M, M, M, M, InOptions> {
+struct Reshape<M, M, InOptions, M, M, InOptions> {
   typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
   static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
     return in;
@@ -314,7 +314,7 @@ struct Reshape<M, M, M, M, InOptions> {
 
 /// Reshape specialization that does nothing as shape stays the same
 template <int M, int N, int InOptions>
-struct Reshape<M, N, M, N, InOptions> {
+struct Reshape<M, N, InOptions, M, N, InOptions> {
   typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
   static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
     return in;
@@ -323,17 +323,17 @@ struct Reshape<M, N, M, N, InOptions> {
 
 /// Reshape specialization that does transpose
 template <int M, int N, int InOptions>
-struct Reshape<N, M, M, N, InOptions> {
+struct Reshape<N, M, InOptions, M, N, InOptions> {
   typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
   static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
     return in.transpose();
   }
 };
 
-template <int OutM, int OutN, int InM, int InN, int InOptions>
+template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
-inline typename Reshape<OutM, OutN, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
+inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
   BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
-  return Reshape<OutM, OutN, InM, InN, InOptions>::reshape(m);
+  return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
 }
 
 /**

gtsam/geometry/Cal3DS2.h

@@ -37,7 +37,7 @@ namespace gtsam {
  *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
-class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
+class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
 
   typedef Cal3DS2_Base Base;
 
@@ -87,21 +87,24 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return 9; }  //TODO: make a final dimension variable
 
-  /// @}
 
 private:
 
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
-    ar & boost::serialization::make_nvp("Cal3DS2",
-        boost::serialization::base_object<Value>(*this));
     ar & boost::serialization::make_nvp("Cal3DS2",
         boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
+  /// @}
+
 };
 
 // Define GTSAM traits

gtsam/geometry/Cal3DS2_Base.cpp

@@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx,
     double yy, double xy, double rr, double r4, double pnx, double pny,
-    const Eigen::Matrix<double, 2, 2>& DK) {
+    const Matrix2& DK) {
-  Eigen::Matrix<double, 2, 5> DR1;
+  Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
-  Eigen::Matrix<double, 2, 4> DR2;
+  Matrix24 DR2;
   DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
          y * rr, y * r4, rr + 2 * yy, 2 * xy;
-  Eigen::Matrix<double, 2, 9> D;
+  Matrix29 D;
   D << DR1, DK * DR2;
   return D;
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr,
     double g, double k1, double k2, double p1, double p2,
-    const Eigen::Matrix<double, 2, 2>& DK) {
+    const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
   const double dDydx = 2. * p2 * y + p1 * drdx;
   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
 
-  Eigen::Matrix<double, 2, 2> DR;
+  Matrix2 DR;
   DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
         y * dgdx + dDydx, g + y * dgdy + dDydy;
 
@@ -90,8 +90,9 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
-    boost::optional<Matrix&> H2) const {
+    boost::optional<Matrix29&> H1,
+    boost::optional<Matrix2&> H2) const {
 
   //  rr = x^2 + y^2;
   //  g = (1 + k(1)*rr + k(2)*rr^2);
@@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
 
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   if (H1 || H2) DK << fx_, s_, 0.0, fy_;
 
   // Derivative for calibration
@@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
+/* ************************************************************************* */
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
+    boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+
+  if (H1 || H2) {
+    Matrix29 D1;
+    Matrix2 D2;
+    Point2 pu = uncalibrate(p, D1, D2);
+    if (H1)
+      *H1 = D1;
+    if (H2)
+      *H2 = D2;
+    return pu;
+
+  } else {
+    return uncalibrate(p);
+  }
+}
+
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
   // Use the following fixed point iteration to invert the radial distortion.
@@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
   const double rr = xx + yy;
   const double r4 = rr * rr;
   const double g = (1 + k1_ * rr + k2_ * r4);
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
 }
@@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }

gtsam/geometry/Cal3DS2_Base.h

@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -114,9 +113,14 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
+
   Point2 uncalibrate(const Point2& p,
-      boost::optional<Matrix&> Dcal = boost::none,
+       boost::optional<Matrix29&> Dcal = boost::none,
-      boost::optional<Matrix&> Dp = boost::none) const ;
+       boost::optional<Matrix2&> Dp = boost::none) const ;
+
+  Point2 uncalibrate(const Point2& p,
+       boost::optional<Matrix&> Dcal,
+       boost::optional<Matrix&> Dp) const ;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@@ -127,7 +131,6 @@ public:
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix D2d_calibration(const Point2& p) const ;
 
-
 private:
 
   /// @}

gtsam/geometry/Cal3Unified.cpp

@@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
 }
 
 /* ************************************************************************* */
+// todo: make a fixed sized jacobian version of this
 Point2 Cal3Unified::uncalibrate(const Point2& p,
        boost::optional<Matrix&> H1,
        boost::optional<Matrix&> H2) const {
@@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
   Point2 m(x,y);
-  Matrix H1base, H2base;    // jacobians from Base class
+  Matrix29 H1base;
+  Matrix2 H2base;    // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
 
   // Inlined derivative for calibration
   if (H1) {
     // part1
-    Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2,
+    Vector2 DU;
-        -ys * sqrt_nx * xi_sqrt_nx2);
+    DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-    Matrix DDS2U = H2base * DU;
+        -ys * sqrt_nx * xi_sqrt_nx2;
 
-    *H1 = collect(2, &H1base, &DDS2U);
+    H1->resize(2,10);
+    H1->block<2,9>(0,0) = H1base;
+    H1->block<2,1>(0,9) = H2base * DU;
   }
+
   // Inlined derivative for points
   if (H2) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
     const double mid = -(xi * xs*ys) * denom;
-    Matrix DU = (Matrix(2, 2) <<
+    Matrix2 DU;
-        (sqrt_nx + xi*(ys*ys + 1)) * denom, mid,
+    DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
-        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom);
+        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
     *H2 = H2base * DU;
   }

gtsam/geometry/Cal3Unified.h

@@ -40,7 +40,7 @@ namespace gtsam {
  *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
-class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
+class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
 
   typedef Cal3Unified This;
   typedef Cal3DS2_Base Base;
@@ -129,8 +129,6 @@ private:
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
-    ar & boost::serialization::make_nvp("Cal3Unified",
-        boost::serialization::base_object<Value>(*this));
     ar & boost::serialization::make_nvp("Cal3Unified",
         boost::serialization::base_object<Cal3DS2_Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(xi_);

gtsam/geometry/EssentialMatrix.h

@@ -58,6 +58,8 @@ public:
     return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
   }
 
+  virtual ~EssentialMatrix() {}
+
   /// @}
 
   /// @name Testable

gtsam/linear/JacobianFactor.h

@@ -187,7 +187,7 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
-    /* ************************************************************************* */
+    /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const;
 
     /// Return the block diagonal of the Hessian for this factor

gtsam/linear/Preconditioner.cpp

@@ -13,6 +13,7 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <boost/shared_ptr.hpp>
 #include <boost/algorithm/string.hpp>
+#include <boost/range/adaptor/map.hpp>
 #include <iostream>
 #include <vector>
 
@@ -134,30 +135,16 @@ void BlockJacobiPreconditioner::build(
   size_t nnz = 0;
   for ( size_t i = 0 ; i < n ; ++i ) {
     const size_t dim = dims_[i];
-    blocks.push_back(Matrix::Zero(dim, dim));
+    // blocks.push_back(Matrix::Zero(dim, dim));
     // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
     nnz += dim*dim;
   }
 
-  /* compute the block diagonal by scanning over the factors */
+  /* getting the block diagonals over the factors */
   BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
-    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+    std::map<Key, Matrix> hessianMap = gf->hessianBlockDiagonal();
-      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+    BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values)
-        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+      blocks.push_back(hessian);
-        const Matrix &Ai = jf->getA(it);
-        blocks[entry.index()] += (Ai.transpose() * Ai);
-      }
-    }
-    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
-      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
-        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
-        const Matrix &Hii = hf->info(it, it).selfadjointView();
-        blocks[entry.index()] += Hii;
-      }
-    }
-    else {
-      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
-    }
   }
 
   /* if necessary, allocating the memory for cacheing the factorization results */

gtsam/navigation/ImuFactor.h

@@ -57,7 +57,7 @@ namespace gtsam {
     class PreintegratedMeasurements {
     public:
       imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
-      Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
+      Matrix measurementCovariance; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
       Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
       Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
@@ -216,11 +216,21 @@ namespace gtsam {
             H_vel_pos, H_vel_vel, H_vel_angles,
             H_angles_pos, H_angles_vel, H_angles_angles;
 
-
+        // first order uncertainty propagation:
-        // first order uncertainty propagation
         // the deltaT allows to pass from continuous time noise to discrete time noise
+        // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
+        // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
         PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
 
+        // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
+        //
+        //        Matrix G(9,9);
+        //        G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+        //            Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+        //            Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+        //
+        //        PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+
         // Update preintegrated measurements
         /* ----------------------------------------------------------------------------------------------------------------------- */
         if(!use2ndOrderIntegration_){

gtsam/nonlinear/Values-inl.h

@@ -215,7 +215,7 @@ namespace gtsam {
   Values::Values(const Values::ConstFiltered<ValueType>& view) {
     BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
       Key key = key_value.key;
-      insert<ValueType>(key, key_value.value);
+      insert(key, static_cast<const ValueType&>(key_value.value));
     }
   }
 

gtsam/nonlinear/tests/testValues.cpp

@@ -383,6 +383,12 @@ TEST(Values, filter) {
   expectedSubValues1.insert(3, pose3);
   EXPECT(assert_equal(expectedSubValues1, actualSubValues1));
 
+  // ConstFilter by Key
+  Values::ConstFiltered<Value> constfiltered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
+  EXPECT_LONGS_EQUAL(2, (long)constfiltered.size());
+  Values fromconstfiltered(constfiltered);
+  EXPECT(assert_equal(expectedSubValues1, fromconstfiltered));
+
   // Filter by type
   i = 0;
   Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();

gtsam/slam/RegularImplicitSchurFactor.h

@@ -1,5 +1,5 @@
 /**
- * @file    ImplicitSchurFactor.h
+ * @file    RegularImplicitSchurFactor.h
  * @brief   A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
  * @author  Frank Dellaert
  * @author  Luca Carlone
@@ -17,13 +17,13 @@
 namespace gtsam {
 
 /**
- * ImplicitSchurFactor
+ * RegularImplicitSchurFactor
  */
 template<size_t D> //
-class ImplicitSchurFactor: public GaussianFactor {
+class RegularImplicitSchurFactor: public GaussianFactor {
 
 public:
-  typedef ImplicitSchurFactor This; ///< Typedef to this class
+  typedef RegularImplicitSchurFactor This; ///< Typedef to this class
   typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
 protected:
@@ -40,11 +40,11 @@ protected:
 public:
 
   /// Constructor
-  ImplicitSchurFactor() {
+  RegularImplicitSchurFactor() {
   }
 
   /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
-  ImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+  RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
       const Matrix3& P, const Vector& b) :
       Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
     initKeys();
@@ -58,7 +58,7 @@ public:
   }
 
   /// Destructor
-  virtual ~ImplicitSchurFactor() {
+  virtual ~RegularImplicitSchurFactor() {
   }
 
   // Write access, only use for construction!
@@ -87,7 +87,7 @@ public:
   /// print
   void print(const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << " ImplicitSchurFactor " << std::endl;
+    std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
     std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
     std::cout << " E_ \n" << E_ << std::endl;
@@ -96,7 +96,7 @@ public:
 
   /// equals
   bool equals(const GaussianFactor& lf, double tol) const {
-    if (!dynamic_cast<const ImplicitSchurFactor*>(&lf))
+    if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
       return false;
     else {
       return false;
@@ -110,21 +110,21 @@ public:
 
   virtual Matrix augmentedJacobian() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::augmentedJacobian non implemented");
+        "RegularImplicitSchurFactor::augmentedJacobian non implemented");
     return Matrix();
   }
   virtual std::pair<Matrix, Vector> jacobian() const {
-    throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented");
     return std::make_pair(Matrix(), Vector());
   }
   virtual Matrix augmentedInformation() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::augmentedInformation non implemented");
+        "RegularImplicitSchurFactor::augmentedInformation non implemented");
     return Matrix();
   }
   virtual Matrix information() const {
     throw std::runtime_error(
-        "ImplicitSchurFactor::information non implemented");
+        "RegularImplicitSchurFactor::information non implemented");
     return Matrix();
   }
 
@@ -210,18 +210,18 @@ public:
   }
 
   virtual GaussianFactor::shared_ptr clone() const {
-    return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
         PointCovariance_, E_, b_);
-    throw std::runtime_error("ImplicitSchurFactor::clone non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
   }
   virtual bool empty() const {
     return false;
   }
 
   virtual GaussianFactor::shared_ptr negate() const {
-    return boost::make_shared<ImplicitSchurFactor<D> >(Fblocks_,
+    return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
         PointCovariance_, E_, b_);
-    throw std::runtime_error("ImplicitSchurFactor::negate non implemented");
+    throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented");
   }
 
   // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@@ -454,7 +454,7 @@ public:
   }
 
 };
-// ImplicitSchurFactor
+// RegularImplicitSchurFactor
 
 }
 

gtsam/slam/SmartFactorBase.h

@@ -21,7 +21,7 @@
 
 #include "JacobianFactorQ.h"
 #include "JacobianFactorSVD.h"
-#include "ImplicitSchurFactor.h"
+#include "RegularImplicitSchurFactor.h"
 #include "RegularHessianFactor.h"
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
@@ -73,7 +73,7 @@ public:
 
   /**
    * Constructor
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
       body_P_sensor_(body_P_sensor) {
@@ -271,8 +271,13 @@ public:
 
       Vector bi;
       try {
-        bi =
+        bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
-            -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
+        if(body_P_sensor_){
+          Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
+          Matrix J(6, 6);
+          Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+          Fi = Fi * J;
+        }
       } catch (CheiralityException&) {
         std::cout << "Cheirality exception " << std::endl;
         exit(EXIT_FAILURE);
@@ -624,11 +629,11 @@ public:
   }
 
   // ****************************************************************************************************
-  boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
-    typename boost::shared_ptr<ImplicitSchurFactor<D> > f(
+    typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
-        new ImplicitSchurFactor<D>());
+        new RegularImplicitSchurFactor<D>());
     computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
         cameras, point, lambda, diagonalDamping);
     f->initKeys();

gtsam/slam/SmartProjectionFactor.h

@@ -120,7 +120,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
@@ -298,7 +298,7 @@ public:
         || (!this->manageDegeneracy_
             && (this->cheiralityException_ || this->degenerate_))) {
       if (isDebug) {
-        std::cout << "createImplicitSchurFactor: degenerate configuration"
+        std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
             << std::endl;
       }
       return false;
@@ -409,12 +409,12 @@ public:
   }
 
   // create factor
-  boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
+  boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
-      return Base::createImplicitSchurFactor(cameras, point_, lambda);
+      return Base::createRegularImplicitSchurFactor(cameras, point_, lambda);
     else
-      return boost::shared_ptr<ImplicitSchurFactor<D> >();
+      return boost::shared_ptr<RegularImplicitSchurFactor<D> >();
   }
 
   /// create factor
@@ -685,7 +685,7 @@ public:
   inline bool isPointBehindCamera() const {
     return cheiralityException_;
   }
-  /** return chirality verbosity */
+  /** return cheirality verbosity */
   inline bool verboseCheirality() const {
     return verboseCheirality_;
   }

gtsam/slam/SmartProjectionPoseFactor.h

@@ -63,7 +63,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
@@ -157,6 +157,9 @@ public:
     size_t i=0;
     BOOST_FOREACH(const Key& k, this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
+      if(Base::body_P_sensor_)
+        pose = pose.compose(*(Base::body_P_sensor_));
+
       typename Base::Camera camera(pose, *K_all_[i++]);
       cameras.push_back(camera);
     }

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -6,7 +6,7 @@
  */
 
 //#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
-#include <gtsam/slam/ImplicitSchurFactor.h>
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
 //#include <gtsam_unstable/slam/JacobianFactorQ.h>
 #include <gtsam/slam/JacobianFactorQ.h>
 //#include "gtsam_unstable/slam/JacobianFactorQR.h"
@@ -38,19 +38,19 @@ const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
 
 //*************************************************************************************
-TEST( implicitSchurFactor, creation ) {
+TEST( regularImplicitSchurFactor, creation ) {
   // Matrix E = Matrix::Ones(6,3);
   Matrix E = zeros(6, 3);
   E.block<2,2>(0, 0) = eye(2);
   E.block<2,3>(2, 0) = 2 * ones(2, 3);
   Matrix3 P = (E.transpose() * E).inverse();
-  ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
   Matrix expectedP = expected.getPointCovariance();
   EXPECT(assert_equal(expectedP, P));
 }
 
 /* ************************************************************************* */
-TEST( implicitSchurFactor, addHessianMultiply ) {
+TEST( regularImplicitSchurFactor, addHessianMultiply ) {
 
   Matrix E = zeros(6, 3);
   E.block<2,2>(0, 0) = eye(2);
@@ -81,11 +81,11 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
   keys += 0,1,2,3;
   Vector x = xvalues.vector(keys);
   Vector expected = zero(24);
-  ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
+  RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
   EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
 
   // Create ImplicitSchurFactor
-  ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
 
   VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
   { // First Version
@@ -190,7 +190,7 @@ TEST( implicitSchurFactor, addHessianMultiply ) {
 }
 
 /* ************************************************************************* */
-TEST(implicitSchurFactor, hessianDiagonal)
+TEST(regularImplicitSchurFactor, hessianDiagonal)
 {
   /* TESTED AGAINST MATLAB
    *  F = [ones(2,6) zeros(2,6) zeros(2,6)
@@ -207,7 +207,7 @@ TEST(implicitSchurFactor, hessianDiagonal)
   E.block<2,3>(2, 0) << 1,2,3,4,5,6;
   E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
   Matrix3 P = (E.transpose() * E).inverse();
-  ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
+  RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
 
   // hessianDiagonal
   VectorValues expected;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
   if(isDebugTest) tictoc_print_();
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
+  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
+
+  SimpleCamera cam1(cameraPose1, *K); // with camera poses
+  SimpleCamera cam2(cameraPose2, *K);
+  SimpleCamera cam3(cameraPose3, *K);
+
+  // create arbitrary body_Pose_sensor (transforms from sensor to body)
+  Pose3 sensor_to_body =  Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
+
+  // These are the poses we want to estimate, from camera measurements
+  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
+  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
+  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(5, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  // Create smart factors
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  double rankTol = 1;
+  double linThreshold = -1;
+  bool manageDegeneracy = false;
+  bool enableEPI = false;
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor2->add(measurements_cam2, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor3->add(measurements_cam3, views, model, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  // Put all factors in factor graph, adding priors
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
+
+  // Check errors at ground truth poses
+  Values gtValues;
+  gtValues.insert(x1, bodyPose1);
+  gtValues.insert(x2, bodyPose2);
+  gtValues.insert(x3, bodyPose3);
+  double actualError = graph.error(gtValues);
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
+  Values values;
+  values.insert(x1, bodyPose1);
+  values.insert(x2, bodyPose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(x3, bodyPose3*noise_pose);
+
+  LevenbergMarquardtParams params;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
+}
+
+
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
   // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;

gtsam_unstable/gtsam_unstable.h

@@ -4,14 +4,15 @@
 
 // specify the classes from gtsam we are using
 virtual class gtsam::Value;
-virtual class gtsam::LieScalar;
+class gtsam::Vector6;
-virtual class gtsam::LieVector;
+class gtsam::LieScalar;
-virtual class gtsam::Point2;
+class gtsam::LieVector;
-virtual class gtsam::Rot2;
+class gtsam::Point2;
-virtual class gtsam::Pose2;
+class gtsam::Rot2;
-virtual class gtsam::Point3;
+class gtsam::Pose2;
-virtual class gtsam::Rot3;
+class gtsam::Point3;
-virtual class gtsam::Pose3;
+class gtsam::Rot3;
+class gtsam::Pose3;
 virtual class gtsam::noiseModel::Base;
 virtual class gtsam::noiseModel::Gaussian;
 virtual class gtsam::imuBias::ConstantBias;
@@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4;
 virtual class gtsam::GaussianFactor;
 virtual class gtsam::HessianFactor;
 virtual class gtsam::JacobianFactor;
-virtual class gtsam::Cal3_S2;
+class gtsam::Cal3_S2;
-virtual class gtsam::Cal3DS2;
+class gtsam::Cal3DS2;
 class gtsam::GaussianFactorGraph;
 class gtsam::NonlinearFactorGraph;
 class gtsam::Ordering;
@@ -48,7 +49,7 @@ class Dummy {
 };
 
 #include <gtsam_unstable/dynamics/PoseRTV.h>
-virtual class PoseRTV : gtsam::Value {
+class PoseRTV {
   PoseRTV();
   PoseRTV(Vector rtv);
   PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
@@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value {
 };
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
-virtual class Pose3Upright : gtsam::Value {
+class Pose3Upright {
   Pose3Upright();
   Pose3Upright(const gtsam::Pose3Upright& x);
   Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
@@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value {
 }; // \class Pose3Upright
 
 #include <gtsam_unstable/geometry/BearingS2.h>
-virtual class BearingS2 : gtsam::Value {
+class BearingS2 {
   BearingS2();
   BearingS2(double azimuth, double elevation);
   BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
@@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
 virtual class Reconstruction : gtsam::NonlinearFactor {
   Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h);
 
-  Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const;
+  Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const;
 };
 
 virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor {
   DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
       double h, Matrix Inertia, Vector Fu, double m);
 
-  Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const;
+  Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const;
 };
 
 //*************************************************************************

gtsam_unstable/nonlinear/Expression-inl.h

@@ -64,7 +64,8 @@ public:
   }
   /// Access via key
   VerticalBlockMatrix::Block operator()(Key key) {
-    FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
+    FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
+        key);
     DenseIndex block = it - keys_.begin();
     return Ab_(block);
   }
@@ -347,8 +348,8 @@ public:
   }
 
   /// Construct an execution trace for reverse AD
-  virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace,
+  virtual const value_type& traceExecution(const Values& values,
-      char* raw) const {
+      ExecutionTrace<value_type>& trace, char* raw) const {
     trace.setLeaf(key_);
     return dynamic_cast<const value_type&>(values.at(key_));
   }
@@ -405,6 +406,7 @@ public:
 //   C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
 //   and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
 // to recursively generate a class, that will be the base for function nodes.
+//
 // The class generated, for three arguments A1, A2, and A3 will be
 //
 // struct Base1 : Argument<T,A1,1>, FunctionalBase<T> {
@@ -429,6 +431,30 @@ public:
 //
 // All this magic happens when  we generate the Base3 base class of FunctionalNode
 // by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode
+//
+// Similarly, the inner Record struct will be
+//
+// struct Record1 : JacobianTrace<T,A1,1>, CallRecord<traits::dimension<T>::value> {
+//   ... storage related to A1 ...
+//   ... methods that work on A1 ...
+// };
+//
+// struct Record2 : JacobianTrace<T,A2,2>, Record1 {
+//   ... storage related to A2 ...
+//   ... methods that work on A2 and (recursively) on A1 ...
+// };
+//
+// struct Record3 : JacobianTrace<T,A3,3>, Record2 {
+//   ... storage related to A3 ...
+//   ... methods that work on A3 and (recursively) on A2 and A1 ...
+// };
+//
+// struct Record : Record3 {
+//   Provides convenience access to storage in hierarchy by using
+//   static_cast<JacobianTrace<T, A, N> &>(*this)
+// }
+//
+
 //-----------------------------------------------------------------------------
 
 /// meta-function to generate fixed-size JacobianTA type
@@ -457,6 +483,7 @@ struct FunctionalBase: ExpressionNode<T> {
 
   /// Construct an execution trace for reverse AD
   void trace(const Values& values, Record* record, char*& raw) const {
+    // base case: does not do anything
   }
 };
 
@@ -562,15 +589,23 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
 template<class T, class TYPES>
 struct FunctionalNode {
 
+  /// The following typedef generates the recursively defined Base class
   typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
       GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base;
 
+  /**
+   *  The type generated by this meta-function derives from Base
+   *  and adds access functions as well as the crucial [trace] function
+   */
   struct type: public Base {
 
     // Argument types and derived, note these are base 0 !
+    // These are currently not used - useful for Phoenix in future
+#ifdef EXPRESSIONS_PHOENIX
     typedef TYPES Arguments;
     typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
     typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
+#endif
 
     /// Reset expression shared pointer
     template<class A, size_t N>
@@ -725,7 +760,8 @@ public:
 
   typedef boost::function<
       T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
-          typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
+          typename OptionalJacobian<T, A2>::type,
+          typename OptionalJacobian<T, A3>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
   typedef typename Base::Record Record;
 

gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/base/Testable.h>
 
 #include <CppUnitLite/TestHarness.h>
+#include <algorithm>
 
 using namespace std;
 using namespace gtsam;
@@ -143,7 +144,7 @@ TEST(ExpressionFactor, Triple) {
 
 // Test out invoke
 TEST(ExpressionFactor, Invoke) {
-  assert(invoke(add,boost::fusion::make_vector(1,1)) == 2);
+  EXPECT_LONGS_EQUAL(2, invoke(plus<int>(),boost::fusion::make_vector(1,1)));
 
   // Creating a Pose3 (is there another way?)
   boost::fusion::vector<Rot3, Point3> pair;

gtsam_unstable/partition/FindSeparator-inl.h

@@ -17,12 +17,14 @@
 #include <boost/shared_array.hpp>
 #include <boost/timer.hpp>
 
+#include "FindSeparator.h"
+
 extern "C" {
 #include <metis.h>
 #include "metislib.h"
 }
 
-#include "FindSeparator.h"
+
 
 namespace gtsam { namespace partition {
 

gtsam_unstable/slam/BetweenFactorEM.h

@@ -56,8 +56,7 @@ namespace gtsam {
   bool flag_bump_up_near_zero_probs_;
 
   /** concept check by type */
-    GTSAM_CONCEPT_LIE_TYPE(T)
+  GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
-    GTSAM_CONCEPT_TESTABLE_TYPE(T)
 
 public:
 
@@ -65,32 +64,34 @@ namespace gtsam {
   typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
 
   /** default constructor - only use for serialization */
-    BetweenFactorEM() {}
+  BetweenFactorEM() {
+  }
 
   /** Constructor */
   BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
       const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
-        const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) :
+      const double prior_inlier, const double prior_outlier,
-          Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
+      const bool flag_bump_up_near_zero_probs = false) :
-          model_inlier_(model_inlier), model_outlier_(model_outlier),
+      Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
-          prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){
+          measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(
+          prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(
+          flag_bump_up_near_zero_probs) {
   }
 
-    virtual ~BetweenFactorEM() {}
+  virtual ~BetweenFactorEM() {
-
+  }
 
   /** implement functions needed for Testable */
 
   /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      std::cout << s << "BetweenFactorEM("
+      DefaultKeyFormatter) const {
-          << keyFormatter(key1_) << ","
+    std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
         << keyFormatter(key2_) << ")\n";
     measured_.print("  measured: ");
     model_inlier_->print("  noise model inlier: ");
     model_outlier_->print("  noise model outlier: ");
-      std::cout << "(prior_inlier, prior_outlier_) = ("
+    std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
-                << prior_inlier_ << ","
         << prior_outlier_ << ")\n";
     //      Base::print(s, keyFormatter);
   }
@@ -100,10 +101,12 @@ namespace gtsam {
     const This *t = dynamic_cast<const This*>(&f);
 
     if (t && Base::equals(f))
-        return key1_ == t->key1_ && key2_ == t->key2_ &&
+      return key1_ == t->key1_ && key2_ == t->key2_
+          &&
           //            model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
           //            model_outlier_->equals(t->model_outlier_ ) &&
-            prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
+          prior_outlier_ == t->prior_outlier_
+          && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
     else
       return false;
   }
@@ -122,7 +125,8 @@ namespace gtsam {
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
   /* This version of linearize recalculates the noise model each time */
-    virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
+  virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
+      const gtsam::Values& x) const {
     // Only linearize if the factor is active
     if (!this->active(x))
       return boost::shared_ptr<gtsam::JacobianFactor>();
@@ -135,10 +139,10 @@ namespace gtsam {
     A2 = A[1];
 
     return gtsam::GaussianFactor::shared_ptr(
-          new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size())));
+        new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
+            gtsam::noiseModel::Unit::Create(b.size())));
   }
 
-
   /* ************************************************************************* */
   gtsam::Vector whitenedError(const gtsam::Values& x,
       boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
@@ -164,11 +168,13 @@ namespace gtsam {
     Vector err_wh_outlier = model_outlier_->whiten(err);
 
     Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
-      Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
+    Matrix invCov_outlier = model_outlier_->R().transpose()
+        * model_outlier_->R();
 
     Vector err_wh_eq;
     err_wh_eq.resize(err_wh_inlier.rows() * 2);
-      err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
+    err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
+        * err_wh_outlier.array();
 
     if (H) {
       // stack Jacobians for the two indicators for each of the key
@@ -219,7 +225,6 @@ namespace gtsam {
       //        std::cout<<"===="<<std::endl;
     }
 
-
     return err_wh_eq;
   }
 
@@ -235,18 +240,27 @@ namespace gtsam {
     Vector err_wh_outlier = model_outlier_->whiten(err);
 
     Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
-      Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
+    Matrix invCov_outlier = model_outlier_->R().transpose()
+        * model_outlier_->R();
 
-      double p_inlier  = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
+    double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
-      double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
+        * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
+    double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
+        * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
 
     if (debug) {
-        std::cout<<"in calcIndicatorProb. err_unwh: "<<err[0]<<", "<<err[1]<<", "<<err[2]<<std::endl;
+      std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
-        std::cout<<"in calcIndicatorProb. err_wh_inlier: "<<err_wh_inlier[0]<<", "<<err_wh_inlier[1]<<", "<<err_wh_inlier[2]<<std::endl;
+          << err[1] << ", " << err[2] << std::endl;
-        std::cout<<"in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "<<err_wh_inlier.dot(err_wh_inlier)<<std::endl;
+      std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
-        std::cout<<"in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "<<err_wh_outlier.dot(err_wh_outlier)<<std::endl;
+          << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
+      std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
+          << err_wh_inlier.dot(err_wh_inlier) << std::endl;
+      std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
+          << err_wh_outlier.dot(err_wh_outlier) << std::endl;
 
-        std::cout<<"in calcIndicatorProb. p_inlier, p_outlier before normalization: "<<p_inlier<<", "<<p_outlier<<std::endl;
+      std::cout
+          << "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
+          << p_inlier << ", " << p_outlier << std::endl;
     }
 
     double sumP = p_inlier + p_outlier;
@@ -314,7 +328,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+  void updateNoiseModels(const gtsam::Values& values,
+      const gtsam::NonlinearFactorGraph& graph) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
      *
@@ -338,7 +353,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-        void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
+  void updateNoiseModels_givenCovs(const gtsam::Values& values,
+      const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
      *
@@ -352,7 +368,7 @@ namespace gtsam {
     const T& p2 = values.at<T>(key2_);
 
     Matrix H1, H2;
-           T hx = p1.between(p2, H1, H2); // h(x)
+    p1.between(p2, H1, H2); // h(x)
 
     Matrix H;
     H.resize(H1.rows(), H1.rows() + H2.rows());
@@ -360,19 +376,22 @@ namespace gtsam {
 
     Matrix joint_cov;
     joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
-           joint_cov << cov1, cov12,
+    joint_cov << cov1, cov12, cov12.transpose(), cov2;
-               cov12.transpose(), cov2;
 
     Matrix cov_state = H * joint_cov * H.transpose();
 
     //       model_inlier_->print("before:");
 
     // update inlier and outlier noise models
-           Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    Matrix covRinlier =
-           model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+        (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
+    model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
+        covRinlier + cov_state);
 
-           Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    Matrix covRoutlier =
-           model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+        (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
+    model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
+        covRoutlier + cov_state);
 
     //       model_inlier_->print("after:");
     //       std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
@@ -399,10 +418,12 @@ namespace gtsam {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NonlinearFactor",
+    ar
+        & boost::serialization::make_nvp("NonlinearFactor",
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measured_);
   }
-  }; // \class BetweenFactorEM
+};
+// \class BetweenFactorEM
 
 }/// namespace gtsam

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -372,7 +372,7 @@ namespace gtsam {
       const T& p2 = values.at<T>(keyB_);
 
       Matrix H1, H2;
-      T hx = p1.between(p2, H1, H2); // h(x)
+      p1.between(p2, H1, H2); // h(x)
 
       Matrix H;
       H.resize(H1.rows(), H1.rows()+H2.rows());

gtsam_unstable/timing/timeDSFvariants.cpp

@@ -57,7 +57,8 @@ int main(int argc, char* argv[]) {
     volatile double fpm = 0.5; // fraction of points matched
     volatile size_t nm = fpm * n * np; // number of matches
 
-    cout << format("\nTesting with %1% images, %2% points, %3% matches\n") % m % N % nm;
+    cout << format("\nTesting with %1% images, %2% points, %3% matches\n")
+            % (int)m % (int)N % (int)nm;
     cout << "Generating " << nm << " matches" << endl;
     boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
         boost::mt19937(), boost::uniform_int<size_t>(0, N - 1));
@@ -67,7 +68,7 @@ int main(int argc, char* argv[]) {
     for (size_t k = 0; k < nm; k++)
       matches.push_back(Match(rn(), rn()));
 
-    os << format("%1%,%2%,%3%,") % m % N % nm;
+    os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm;
 
     {
       // DSFBase version

gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp

@@ -35,12 +35,12 @@ gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
 gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
 /* ************************************************************************* */
-gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
+gtsam::Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
   return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
 }
 
-gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
+gtsam::Vector3 predictionErrorVel(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
-  return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3));
+  return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
 }
 
 #include <gtsam/linear/GaussianFactorGraph.h>
@@ -64,21 +64,21 @@ int main() {
   Vector measurement_acc((Vector(3) << 6.501390843381716,  -6.763926150509185,  -2.300389940090343));
   Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
 
   Rot3 R1(0.487316618,   0.125253866,   0.86419557,
        0.580273724,  0.693095498, -0.427669306,
       -0.652537293,  0.709880342,  0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4));
+  Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4));
   Rot3 R2(0.473618898,   0.119523052,  0.872582019,
        0.609241153,   0.67099888, -0.422594037,
       -0.636011287,  0.731761397,  0.244979388);
   Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
   Pose3 Pose2(R2, t2);
   Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
-  LieVector Vel2 = Vel1.compose( dv );
+  Vector3 Vel2 = Vel1 + dv;
   imuBias::ConstantBias Bias1;
 
   Values values;

gtsam_unstable/timing/timeSFMExpressions.cpp

@@ -47,15 +47,15 @@ int main() {
   // Create values
   Values values;
   values.insert(Symbol('K', 0), Cal3_S2());
-  for (int i = 0; i < M; i++)
+  for (size_t i = 0; i < M; i++)
     values.insert(Symbol('x', i), Pose3());
-  for (int j = 0; j < N; j++)
+  for (size_t j = 0; j < N; j++)
     values.insert(Symbol('p', j), Point3(0, 0, 1));
 
   long timeLog = clock();
   NonlinearFactorGraph graph;
-  for (int i = 0; i < M; i++) {
+  for (size_t i = 0; i < M; i++) {
-    for (int j = 0; j < N; j++) {
+    for (size_t j = 0; j < N; j++) {
       NonlinearFactor::shared_ptr f = boost::make_shared<
           ExpressionFactor<Point2> >
 #ifdef TERNARY

matlab/+gtsam/VisualISAMPlot.m

@@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals);
 M = 1;
 while result.exists(symbol('x',M))
     ii = symbol('x',M);
-    pose_i = result.at(ii);
+    pose_i = result.atPose3(ii);
     if options.hardConstraint && (M==1)
         gtsam.plotPose3(pose_i,[],10);
     else

matlab/+gtsam/VisualISAMStep.m

@@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex})
 end
 
 %% Initial estimates for the new pose.
-prevPose = result.at(symbol('x',nextPoseIndex-1));
+prevPose = result.atPose3(symbol('x',nextPoseIndex-1));
 initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
 
 %% Update ISAM

matlab/+gtsam/load3D.m

@@ -46,9 +46,9 @@ for i=1:n
                 graph.add(BetweenFactorPose3(i1, i2, dpose, model));
                 if successive
                     if i2>i1
-                        initial.insert(i2,initial.at(i1).compose(dpose));
+                        initial.insert(i2,initial.atPose3(i1).compose(dpose));
                     else
-                        initial.insert(i1,initial.at(i2).compose(dpose.inverse));
+                        initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse));
                     end
                 end
             end

matlab/+gtsam/plot2DPoints.m

@@ -18,15 +18,18 @@ hold on
 % Plot points and covariance matrices
 for i = 0:keys.size-1
     key = keys.at(i);
-    p = values.at(key);
+    try
-    if isa(p, 'gtsam.Point2')
+        p = values.atPoint2(key);
         if haveMarginals
             P = marginals.marginalCovariance(key);
             gtsam.plotPoint2(p, linespec, P);
         else
             gtsam.plotPoint2(p, linespec);
         end
+    catch err
+        % I guess it's not a Point2
     end
+    
 end
 
 if ~holdstate

matlab/+gtsam/plot2DTrajectory.m

@@ -33,10 +33,12 @@ else
     keys = KeyVector(values.keys);
     for i = 0:keys.size-1
         key = keys.at(i);
-    x = values.at(key);
+        try
-    if isa(x, 'gtsam.Pose2')
+            x = values.atPose2(key);
             P = marginals.marginalCovariance(key);
             gtsam.plotPose2(x,linespec(1), P);
+        catch err
+            % I guess it's not a Pose2
         end
     end
 end

matlab/+gtsam/plot3DPoints.m

@@ -18,14 +18,16 @@ hold on
 % Plot points and covariance matrices
 for i = 0:keys.size-1
     key = keys.at(i);
-    p = values.at(key);
+    try
-    if isa(p, 'gtsam.Point3')
+        p = values.atPoint3(key);
         if haveMarginals
             P = marginals.marginalCovariance(key);
             gtsam.plotPoint3(p, linespec, P);
         else
             gtsam.plotPoint3(p, linespec);
         end
+    catch
+        % I guess it's not a Point3
     end
 end
 

matlab/+gtsam/plot3DTrajectory.m

@@ -17,13 +17,14 @@ hold on
 lastIndex = [];
 for i = 0:keys.size-1
     key = keys.at(i);
-    x = values.at(key);
+    try
-    if isa(x, 'gtsam.Pose3')
+        x = values.atPose3(key);
         if ~isempty(lastIndex)
             % Draw line from last pose then covariance ellipse on top of
             % last pose.
             lastKey = keys.at(lastIndex);
-            lastPose = values.at(lastKey);
+            try
+                lastPose = values.atPose3(lastKey);
                 plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
                 if haveMarginals
                     P = marginals.marginalCovariance(lastKey);
@@ -31,21 +32,29 @@ for i = 0:keys.size-1
                     P = [];
                 end
                 gtsam.plotPose3(lastPose, P, scale);
+            catch err
+                warning(['no Pose3 at ' lastKey]);
             end
             lastIndex = i;
         end
+    catch
+        warning(['no Pose3 at ' key]);
     end
     
     % Draw final pose
     if ~isempty(lastIndex)
         lastKey = keys.at(lastIndex);
-    lastPose = values.at(lastKey);
+        try
+            lastPose = values.atPose3(lastKey);
             if haveMarginals
                 P = marginals.marginalCovariance(lastKey);
             else
                 P = [];
             end
             gtsam.plotPose3(lastPose, P, scale);
+        catch
+            warning(['no Pose3 at ' lastIndex]);
+        end
     end
     
     if ~holdstate

matlab/gtsam_examples/PlanarSLAMExample.m

@@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
 plot2DTrajectory(result, [], marginals);
 plot2DPoints(result, 'b', marginals);
 
-plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
+plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
-plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
+plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
-plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
+plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
 axis([-0.6 4.8 -1 1])
 axis equal
 view(2)

matlab/gtsam_examples/PlanarSLAMExample_graph.m

@@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
 [graph,initial] = load2D(datafile, model);
 
 %% Add a Gaussian prior on a pose in the middle
-priorMean = initial.at(40);
+priorMean = initial.atPose2(40);
 priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
 graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph
 

matlab/gtsam_examples/PlanarSLAMExample_sampling.m

@@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals);
 
 for j=1:2
     key = symbol('l',j);
-    point{j} = sample.at(key);
+    point{j} = sample.atPoint2(key);
     Q{j}=marginals.marginalCovariance(key);
     S{j}=chol(Q{j}); % for sampling
 end
 
-plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
+plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
+plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
+plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
 view(2); axis auto; axis equal
 
 %% Do Sampling on point 2

matlab/gtsam_examples/Pose2SLAMExample.m

@@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n'));
 %% Plot Covariance Ellipses
 cla;
 hold on
-plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
+plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-');
 marginals = Marginals(graph, result);
 
 plot2DTrajectory(result, [], marginals);

matlab/gtsam_examples/Pose2SLAMExample_circle.m

@@ -14,8 +14,8 @@ import gtsam.*
 
 %% Create a hexagon of poses
 hexagon = circlePose2(6,1.0);
-p0 = hexagon.at(0);
+p0 = hexagon.atPose2(0);
-p1 = hexagon.at(1);
+p1 = hexagon.atPose2(1);
 
 %% create a Pose graph with one equality constraint and one measurement
 fg = NonlinearFactorGraph;
@@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance));
 %% Create initial config
 initial = Values;
 initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
+initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]'));
-initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]'));
+initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]'));
-initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
+initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]'));
 
 %% Plot Initial Estimate
 cla

matlab/gtsam_examples/Pose2SLAMExample_graph.m

@@ -41,7 +41,7 @@ marginals = Marginals(graph, result);
 toc
 P={};
 for i=1:result.size()-1
-    pose_i = result.at(i);
+    pose_i = result.atPose2(i);
     P{i}=marginals.marginalCovariance(i);
     plotPose2(pose_i,'b',P{i})
 end

matlab/gtsam_examples/Pose3SLAMExample.m

@@ -14,8 +14,8 @@ import gtsam.*
 
 %% Create a hexagon of poses
 hexagon = circlePose3(6,1.0);
-p0 = hexagon.at(0);
+p0 = hexagon.atPose3(0);
-p1 = hexagon.at(1);
+p1 = hexagon.atPose3(1);
 
 %% create a Pose graph with one equality constraint and one measurement
 fg = NonlinearFactorGraph;
@@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
 initial = Values;
 s = 0.10;
 initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
+initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
-initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
+initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
-initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
+initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
-initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
+initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
-initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
+initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
 
 %% Plot Initial Estimate
 cla

matlab/gtsam_examples/Pose3SLAMExample_graph.m

@@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.
 
 %% Plot Initial Estimate
 cla
-first = initial.at(0);
+first = initial.atPose3(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
 plot3DTrajectory(initial,'g-',false);
 drawnow;

matlab/gtsam_examples/StereoVOExample_large.m

@@ -45,7 +45,7 @@ for i=1:size(measurements,1)
     if ~initial.exists(symbol('l',sf(2)))
         % 3D landmarks are stored in camera coordinates: transform
         % to world coordinates using the respective initial pose
-        pose = initial.at(symbol('x', sf(1)));
+        pose = initial.atPose3(symbol('x', sf(1)));
         world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
         initial.insert(symbol('l',sf(2)), world_point);
     end
@@ -54,7 +54,7 @@ toc
 
 %% add a constraint on the starting pose
 key = symbol('x',1);
-first_pose = initial.at(key);
+first_pose = initial.atPose3(key);
 graph.add(NonlinearEqualityPose3(key, first_pose));
 
 %% optimize

matlab/gtsam_tests/testLocalizationExample.m

@@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
 groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
 model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
 for i=1:3
-    graph.add(PriorFactorPose2(i, groundTruth.at(i), model));
+    graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
 end
 
 %% Initialize to noisy points
@@ -46,7 +46,7 @@ result = optimizer.optimizeSafely();
 marginals = Marginals(graph, result);
 P={};
 for i=1:result.size()
-    pose_i = result.at(i);
+    pose_i = result.atPose2(i);
-    CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4));
+    CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
     P{i}=marginals.marginalCovariance(i);
 end

matlab/gtsam_tests/testOdometryExample.m

@@ -39,5 +39,5 @@ marginals = Marginals(graph, result);
 marginals.marginalCovariance(1);
 
 %% Check first pose equality
-pose_1 = result.at(1);
+pose_1 = result.atPose2(1);
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -60,10 +60,10 @@ result = optimizer.optimizeSafely();
 marginals = Marginals(graph, result);
 
 %% Check first pose and point equality
-pose_1 = result.at(symbol('x',1));
+pose_1 = result.atPose2(symbol('x',1));
 marginals.marginalCovariance(symbol('x',1));
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
 
-point_1 = result.at(symbol('l',1));
+point_1 = result.atPoint2(symbol('l',1));
 marginals.marginalCovariance(symbol('l',1));
 CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -57,6 +57,6 @@ result = optimizer.optimizeSafely();
 marginals = Marginals(graph, result);
 P = marginals.marginalCovariance(1);
 
-pose_1 = result.at(1);
+pose_1 = result.atPose2(1);
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
 

matlab/gtsam_tests/testPose3SLAMExample.m

@@ -14,8 +14,8 @@ import gtsam.*
 
 %% Create a hexagon of poses
 hexagon = circlePose3(6,1.0);
-p0 = hexagon.at(0);
+p0 = hexagon.atPose3(0);
-p1 = hexagon.at(1);
+p1 = hexagon.atPose3(1);
 
 %% create a Pose graph with one equality constraint and one measurement
 fg = NonlinearFactorGraph;
@@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
 initial = Values;
 s = 0.10;
 initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
+initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
-initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
+initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
-initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
+initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
-initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
+initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
-initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
+initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
 
 %% optimize
 optimizer = LevenbergMarquardtOptimizer(fg, initial);
 result = optimizer.optimizeSafely;
 
-pose_1 = result.at(1);
+pose_1 = result.atPose3(1);
 CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4));
 
 

matlab/gtsam_tests/testSFMExample.m

@@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1));
 
 %% Check optimized results, should be equal to ground truth
 for i=1:size(truth.cameras,2)
-    pose_i = result.at(symbol('x',i));
+    pose_i = result.atPose3(symbol('x',i));
     CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
 end
 
 for j=1:size(truth.points,2)
-    point_j = result.at(symbol('p',j));
+    point_j = result.atPoint3(symbol('p',j));
     CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
 end

matlab/gtsam_tests/testStereoVOExample.m

@@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimize();
 
 %% check equality for the first pose and point
-pose_x1 = result.at(x1);
+pose_x1 = result.atPose3(x1);
 CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
 
-point_l1 = result.at(l1);
+point_l1 = result.atPoint3(l1);
 CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));

matlab/gtsam_tests/testVisualISAMExample.m

@@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras
 end
 
 for i=1:size(truth.cameras,2)
-    pose_i = result.at(symbol('x',i));
+    pose_i = result.atPose3(symbol('x',i));
     CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
 end
 
 for j=1:size(truth.points,2)
-    point_j = result.at(symbol('l',j));
+    point_j = result.atPoint3(symbol('l',j));
     CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
 end

matlab/gtsam_tests/test_gtsam.m

@@ -30,11 +30,11 @@ testStereoVOExample
 display 'Starting: testVisualISAMExample'
 testVisualISAMExample
 
-display 'Starting: testSerialization'
-testSerialization
-
 display 'Starting: testUtilities'
 testUtilities
 
+display 'Starting: testSerialization'
+testSerialization
+
 % end of tests
 display 'Tests complete!'

package_scripts/toolbox_package_unix.sh

@@ -61,4 +61,4 @@ if [ $? -ne 0 ]; then
 fi
 
 # Create package
-tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox
+tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox

tests/testPCGSolver.cpp

@@ -13,6 +13,7 @@
  * @file    testPCGSolver.cpp
  * @brief   Unit tests for PCGSolver class
  * @author  Yong-Dian Jian
+ * @date    Aug 06, 2014
  */
 
 #include <tests/smallExample.h>
@@ -51,6 +52,7 @@ using symbol_shorthand::X;
 using symbol_shorthand::L;
 
 /* ************************************************************************* */
+// Test cholesky decomposition
 TEST( PCGSolver, llt ) {
   Matrix R = (Matrix(3,3) <<
                 1., -1., -1.,
@@ -90,6 +92,7 @@ TEST( PCGSolver, llt ) {
 }
 
 /* ************************************************************************* */
+// Test Dummy Preconditioner
 TEST( PCGSolver, dummy )
 {
   LevenbergMarquardtParams paramsPCG;
@@ -110,6 +113,7 @@ TEST( PCGSolver, dummy )
 }
 
 /* ************************************************************************* */
+// Test Block-Jacobi Precondioner
 TEST( PCGSolver, blockjacobi )
 {
   LevenbergMarquardtParams paramsPCG;
@@ -130,6 +134,7 @@ TEST( PCGSolver, blockjacobi )
 }
 
 /* ************************************************************************* */
+// Test Incremental Subgraph PCG Solver
 TEST( PCGSolver, subgraph )
 {
   LevenbergMarquardtParams paramsPCG;

tests/testPreconditioner.cpp

@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   testPreconditioner.cpp
+ *  @brief  Unit tests for Preconditioners
+ *  @author Sungtae An
+ *  @date   Nov 6, 2014
+ **/
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/linear/PCGSolver.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+static GaussianFactorGraph createSimpleGaussianFactorGraph() {
+  GaussianFactorGraph fg;
+  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
+  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
+  fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
+  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
+  fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
+  // measurement between x1 and l1: l1-x1=[0.0;0.2]
+  fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
+  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
+  fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
+  return fg;
+}
+
+/* ************************************************************************* */
+// Copy of BlockJacobiPreconditioner::build
+std::vector<Matrix> buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo)
+{
+  const size_t n = keyInfo.size();
+  std::vector<size_t> dims_ = keyInfo.colSpec();
+
+  /* prepare the buffer of block diagonals */
+  std::vector<Matrix> blocks; blocks.reserve(n);
+
+  /* allocate memory for the factorization of block diagonals */
+  size_t nnz = 0;
+  for ( size_t i = 0 ; i < n ; ++i ) {
+    const size_t dim = dims_[i];
+    blocks.push_back(Matrix::Zero(dim, dim));
+    // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
+    nnz += dim*dim;
+  }
+
+  /* compute the block diagonal by scanning over the factors */
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
+      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Ai = jf->getA(it);
+        blocks[entry.index()] += (Ai.transpose() * Ai);
+      }
+    }
+    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
+      for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
+        const KeyInfoEntry &entry =  keyInfo.find(*it)->second;
+        const Matrix &Hii = hf->info(it, it).selfadjointView();
+        blocks[entry.index()] += Hii;
+      }
+    }
+    else {
+      throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+  }
+
+  return blocks;
+}
+
+/* ************************************************************************* */
+TEST( Preconditioner, buildBlocks ) {
+  // Create simple Gaussian factor graph and initial values
+  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
+  Values initial;
+  initial.insert(0,Point2(4, 5));
+  initial.insert(1,Point2(0,  1));
+  initial.insert(2,Point2(-5, 7));
+
+  // Expected Hessian block diagonal matrices
+  std::map<Key, Matrix> expectedHessian =gfg.hessianBlockDiagonal();
+
+  // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build
+  std::vector<Matrix> actualHessian = buildBlocks(gfg, KeyInfo(gfg));
+
+  // Compare the number of block diagonal matrices
+  EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size());
+
+  // Compare the values of matrices
+  std::map<Key, Matrix>::const_iterator it1 = expectedHessian.begin();
+  std::vector<Matrix>::const_iterator it2 = actualHessian.begin();
+  for(; it1!=expectedHessian.end(); it1++, it2++)
+    EXPECT(assert_equal(it1->second, *it2));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

timing/timeMatrixOps.cpp

@@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {
     gtsam::SubMatrix top = mat.block(0, 0, n, n);
     gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2);
 
-    cout << format("  Basic: %1%x%2%\n") % m % n;
+    cout << format("  Basic: %1%x%2%\n") % (int)m % (int)n;
-    cout << format("  Full:  mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n;
+    cout << format("  Full:  mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)m % 0 % (int)n;
-    cout << format("  Top:   mat(%1%:%2%, %3%:%4%)\n") % 0 % n % 0 % n;
+    cout << format("  Top:   mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)n % 0 % (int)n;
     cout << format("  Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4);
     cout << endl;
 

timing/timePinholeCamera.cpp

@@ -89,7 +89,7 @@ int main()
     Matrix Dpose, Dpoint;
     long timeLog = clock();
     for(int i = 0; i < n; i++)
-      camera.project(point1, Dpose, Dpoint);
+      camera.project(point1, Dpose, Dpoint, boost::none);
     long timeLog2 = clock();
     double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
     cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;

wrap/Argument.cpp

@@ -28,34 +28,53 @@
 using namespace std;
 using namespace wrap;
 
+/* ************************************************************************* */
+Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
+  Argument instArg = *this;
+  instArg.type = ts(type);
+  return instArg;
+}
+
+/* ************************************************************************* */
+ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const {
+  ArgumentList instArgList;
+  BOOST_FOREACH(const Argument& arg, *this) {
+    Argument instArg = arg.expandTemplate(ts);
+    instArgList.push_back(instArg);
+  }
+  return instArgList;
+}
+
 /* ************************************************************************* */
 string Argument::matlabClass(const string& delim) const {
   string result;
-  BOOST_FOREACH(const string& ns, namespaces)
+  BOOST_FOREACH(const string& ns, type.namespaces)
     result += ns + delim;
-  if (type == "string" || type == "unsigned char" || type == "char")
+  if (type.name == "string" || type.name == "unsigned char"
+      || type.name == "char")
     return result + "char";
-  if (type == "Vector" || type == "Matrix")
+  if (type.name == "Vector" || type.name == "Matrix")
     return result + "double";
-  if (type == "int" || type == "size_t")
+  if (type.name == "int" || type.name == "size_t")
     return result + "numeric";
-  if (type == "bool")
+  if (type.name == "bool")
     return result + "logical";
-  return result + type;
+  return result + type.name;
 }
 
 /* ************************************************************************* */
 bool Argument::isScalar() const {
-  return (type == "bool" || type == "char" || type == "unsigned char"
+  return (type.name == "bool" || type.name == "char"
-      || type == "int" || type == "size_t" || type == "double");
+      || type.name == "unsigned char" || type.name == "int"
+      || type.name == "size_t" || type.name == "double");
 }
 
 /* ************************************************************************* */
 void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
   file.oss << "  ";
 
-  string cppType = qualifiedType("::");
+  string cppType = type.qualifiedName("::");
-  string matlabUniqueType = qualifiedType();
+  string matlabUniqueType = type.qualifiedName();
 
   if (is_ptr)
     // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
@@ -78,14 +97,6 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
   file.oss << ");" << endl;
 }
 
-/* ************************************************************************* */
-string Argument::qualifiedType(const string& delim) const {
-  string result;
-  BOOST_FOREACH(const string& ns, namespaces)
-    result += ns + delim;
-  return result + type;
-}
-
 /* ************************************************************************* */
 string ArgumentList::types() const {
   string str;
@@ -93,7 +104,7 @@ string ArgumentList::types() const {
   BOOST_FOREACH(Argument arg, *this) {
     if (!first)
       str += ",";
-    str += arg.type;
+    str += arg.type.name;
     first = false;
   }
   return str;
@@ -105,14 +116,14 @@ string ArgumentList::signature() const {
   bool cap = false;
 
   BOOST_FOREACH(Argument arg, *this) {
-    BOOST_FOREACH(char ch, arg.type)
+    BOOST_FOREACH(char ch, arg.type.name)
       if (isupper(ch)) {
         sig += ch;
         //If there is a capital letter, we don't want to read it below
         cap = true;
       }
     if (!cap)
-      sig += arg.type[0];
+      sig += arg.type.name[0];
     //Reset to default
     cap = false;
   }
@@ -136,7 +147,8 @@ string ArgumentList::names() const {
 /* ************************************************************************* */
 bool ArgumentList::allScalar() const {
   BOOST_FOREACH(Argument arg, *this)
-      if (!arg.isScalar()) return false;
+    if (!arg.isScalar())
+      return false;
   return true;
 }
 
@@ -158,42 +170,43 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
   BOOST_FOREACH(Argument arg, *this) {
     if (!first)
       file.oss << ", ";
-    file.oss << arg.type << " " << arg.name;
+    file.oss << arg.type.name << " " << arg.name;
     first = false;
   }
   file.oss << ")";
 }
 /* ************************************************************************* */
-void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal,
+void ArgumentList::emit_call(FileWriter& proxyFile,
-    const string& wrapperName, int id, bool staticMethod) const {
+    const ReturnValue& returnVal, const string& wrapperName, int id,
-  returnVal.emit_matlab(file);
+    bool staticMethod) const {
-  file.oss << wrapperName << "(" << id;
+  returnVal.emit_matlab(proxyFile);
+  proxyFile.oss << wrapperName << "(" << id;
   if (!staticMethod)
-    file.oss << ", this";
+    proxyFile.oss << ", this";
-  file.oss << ", varargin{:});\n";
+  proxyFile.oss << ", varargin{:});\n";
 }
 /* ************************************************************************* */
-void ArgumentList::emit_conditional_call(FileWriter& file,
+void ArgumentList::emit_conditional_call(FileWriter& proxyFile,
     const ReturnValue& returnVal, const string& wrapperName, int id,
     bool staticMethod) const {
   // Check nr of arguments
-  file.oss << "if length(varargin) == " << size();
+  proxyFile.oss << "if length(varargin) == " << size();
   if (size() > 0)
-    file.oss << " && ";
+    proxyFile.oss << " && ";
-  // ...and their types
+  // ...and their type.names
   bool first = true;
   for (size_t i = 0; i < size(); i++) {
     if (!first)
-      file.oss << " && ";
+      proxyFile.oss << " && ";
-    file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".")
+    proxyFile.oss << "isa(varargin{" << i + 1 << "},'"
-        << "')";
+        << (*this)[i].matlabClass(".") << "')";
     first = false;
   }
-  file.oss << "\n";
+  proxyFile.oss << "\n";
 
   // output call to C++ wrapper
-  file.oss << "        ";
+  proxyFile.oss << "        ";
-  emit_call(file, returnVal, wrapperName, id, staticMethod);
+  emit_call(proxyFile, returnVal, wrapperName, id, staticMethod);
 }
 /* ************************************************************************* */
 

wrap/Argument.h

@@ -19,36 +19,39 @@
 
 #pragma once
 
+#include "TemplateSubstitution.h"
 #include "FileWriter.h"
 #include "ReturnValue.h"
 
-#include <string>
-#include <vector>
-
 namespace wrap {
 
 /// Argument class
 struct Argument {
+  Qualified type;
   bool is_const, is_ref, is_ptr;
-  std::string type;
   std::string name;
-  std::vector<std::string> namespaces;
 
   Argument() :
       is_const(false), is_ref(false), is_ptr(false) {
   }
 
+  Argument expandTemplate(const TemplateSubstitution& ts) const;
+
   /// return MATLAB class for use in isa(x,class)
   std::string matlabClass(const std::string& delim = "") const;
 
   /// Check if will be unwrapped using scalar login in wrap/matlab.h
   bool isScalar() const;
 
-  /// adds namespaces to type
-  std::string qualifiedType(const std::string& delim = "") const;
-
   /// MATLAB code generation, MATLAB to C++
   void matlab_unwrap(FileWriter& file, const std::string& matlabName) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const Argument& arg) {
+    os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "")
+        << (arg.is_ref ? "&" : "");
+    return os;
+  }
+
 };
 
 /// Argument list is just a container with Arguments
@@ -66,6 +69,8 @@ struct ArgumentList: public std::vector<Argument> {
   /// Check if all arguments scalar
   bool allScalar() const;
 
+  ArgumentList expandTemplate(const TemplateSubstitution& ts) const;
+
   // MATLAB code generation:
 
   /**
@@ -83,24 +88,38 @@ struct ArgumentList: public std::vector<Argument> {
   void emit_prototype(FileWriter& file, const std::string& name) const;
 
   /**
-   * emit emit MATLAB call to wrapper
+   * emit emit MATLAB call to proxy
-   * @param file output stream
+   * @param proxyFile output stream
    * @param returnVal the return value
    * @param wrapperName of method or function
    * @param staticMethod flag to emit "this" in call
    */
-  void emit_call(FileWriter& file, const ReturnValue& returnVal,
+  void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
       const std::string& wrapperName, int id, bool staticMethod = false) const;
 
   /**
-   * emit conditional MATLAB call to wrapper (checking arguments first)
+   * emit conditional MATLAB call to proxy (checking arguments first)
-   * @param file output stream
+   * @param proxyFile output stream
    * @param returnVal the return value
    * @param wrapperName of method or function
    * @param staticMethod flag to emit "this" in call
    */
-  void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal,
+  void emit_conditional_call(FileWriter& proxyFile,
-      const std::string& wrapperName, int id, bool staticMethod = false) const;
+      const ReturnValue& returnVal, const std::string& wrapperName, int id,
+      bool staticMethod = false) const;
+
+  friend std::ostream& operator<<(std::ostream& os,
+      const ArgumentList& argList) {
+    os << "(";
+    if (argList.size() > 0)
+      os << argList.front();
+    if (argList.size() > 1)
+      for (size_t i = 1; i < argList.size(); i++)
+        os << ", " << argList[i];
+    os << ")";
+    return os;
+  }
+
 };
 
 } // \namespace wrap

wrap/Class.cpp

@@ -33,7 +33,7 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
+void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
     const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
     vector<string>& functionNames) const {
 
@@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
   createNamespaceStructure(namespaces, toolboxPath);
 
   // open destination classFile 
-  string classFile = toolboxPath;
+  string classFile = matlabName(toolboxPath);
-  if (!namespaces.empty())
-    classFile += "/+" + wrap::qualifiedName("/+", namespaces);
-  classFile += "/" + name + ".m";
   FileWriter proxyFile(classFile, verbose_, "%");
 
   // get the name of actual matlab object 
-  const string matlabQualName = qualifiedName("."), matlabUniqueName =
+  const string matlabQualName = qualifiedName(".");
-      qualifiedName(), cppName = qualifiedName("::");
+  const string matlabUniqueName = qualifiedName();
-  const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
+  const string cppName = qualifiedName("::");
-  const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
+  const string matlabBaseName = qualifiedParent.qualifiedName(".");
+  const string cppBaseName = qualifiedParent.qualifiedName("::");
 
   // emit class proxy code 
   // we want our class to inherit the handle class for memory purposes 
@@ -75,13 +73,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
   pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
       functionNames);
   wrapperFile.oss << "\n";
+
   // Regular constructors 
-  BOOST_FOREACH(ArgumentList a, constructor.args_list) {
+  for (size_t i = 0; i < constructor.nrOverloads(); i++) {
+    ArgumentList args = constructor.argumentList(i);
     const int id = (int) functionNames.size();
     constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
-        id, a);
+        id, args);
     const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
-        cppName, matlabUniqueName, cppBaseName, id, a);
+        cppName, matlabUniqueName, cppBaseName, id, args);
     wrapperFile.oss << "\n";
     functionNames.push_back(wrapFunctionName);
   }
@@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
   proxyFile.emit(true);
 }
 
-/* ************************************************************************* */
-string Class::qualifiedName(const string& delim) const {
-  return ::wrap::qualifiedName(delim, namespaces, name);
-}
-
 /* ************************************************************************* */
 void Class::pointer_constructor_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, const string& wrapperName,
+    FileWriter& wrapperFile, Str wrapperName,
     vector<string>& functionNames) const {
 
-  const string matlabUniqueName = qualifiedName(), cppName = qualifiedName(
+  const string matlabUniqueName = qualifiedName();
-      "::");
+  const string cppName = qualifiedName("::");
-  const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
+  const string baseCppName = qualifiedParent.qualifiedName("::");
 
   const int collectorInsertId = (int) functionNames.size();
   const string collectorInsertFunctionName = matlabUniqueName
@@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
 }
 
 /* ************************************************************************* */
-vector<ArgumentList> expandArgumentListsTemplate(
+Class Class::expandTemplate(const TemplateSubstitution& ts) const {
-    const vector<ArgumentList>& argLists, const string& templateArg,
+  Class inst = *this;
-    const vector<string>& instName,
+  inst.methods = expandMethodTemplate(methods, ts);
-    const std::vector<string>& expandedClassNamespace,
+  inst.static_methods = expandMethodTemplate(static_methods, ts);
-    const string& expandedClassName) {
+  inst.constructor = constructor.expandTemplate(ts);
-  vector<ArgumentList> result;
-  BOOST_FOREACH(const ArgumentList& argList, argLists) {
-    ArgumentList instArgList;
-    BOOST_FOREACH(const Argument& arg, argList) {
-      Argument instArg = arg;
-      if (arg.type == templateArg) {
-        instArg.namespaces.assign(instName.begin(), instName.end() - 1);
-        instArg.type = instName.back();
-      } else if (arg.type == "This") {
-        instArg.namespaces.assign(expandedClassNamespace.begin(),
-            expandedClassNamespace.end());
-        instArg.type = expandedClassName;
-      }
-      instArgList.push_back(instArg);
-    }
-    result.push_back(instArgList);
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-template<class METHOD>
-map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods,
-    const string& templateArg, const vector<string>& instName,
-    const std::vector<string>& expandedClassNamespace,
-    const string& expandedClassName) {
-  map<string, METHOD> result;
-  typedef pair<const string, METHOD> Name_Method;
-  BOOST_FOREACH(const Name_Method& name_method, methods) {
-    const METHOD& method = name_method.second;
-    METHOD instMethod = method;
-    instMethod.argLists = expandArgumentListsTemplate(method.argLists,
-        templateArg, instName, expandedClassNamespace, expandedClassName);
-    instMethod.returnVals.clear();
-    BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
-      ReturnValue instRetVal = retVal;
-      if (retVal.type1 == templateArg) {
-        instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1);
-        instRetVal.type1 = instName.back();
-      } else if (retVal.type1 == "This") {
-        instRetVal.namespaces1.assign(expandedClassNamespace.begin(),
-            expandedClassNamespace.end());
-        instRetVal.type1 = expandedClassName;
-      }
-      if (retVal.type2 == templateArg) {
-        instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1);
-        instRetVal.type2 = instName.back();
-      } else if (retVal.type1 == "This") {
-        instRetVal.namespaces2.assign(expandedClassNamespace.begin(),
-            expandedClassNamespace.end());
-        instRetVal.type2 = expandedClassName;
-      }
-      instMethod.returnVals.push_back(instRetVal);
-    }
-    result.insert(make_pair(name_method.first, instMethod));
-  }
-  return result;
-}
-
-/* ************************************************************************* */
-Class expandClassTemplate(const Class& cls, const string& templateArg,
-    const vector<string>& instName,
-    const std::vector<string>& expandedClassNamespace,
-    const string& expandedClassName) {
-  Class inst;
-  inst.name = cls.name;
-  inst.templateArgs = cls.templateArgs;
-  inst.typedefName = cls.typedefName;
-  inst.isVirtual = cls.isVirtual;
-  inst.isSerializable = cls.isSerializable;
-  inst.qualifiedParent = cls.qualifiedParent;
-  inst.methods = expandMethodTemplate(cls.methods, templateArg, instName,
-      expandedClassNamespace, expandedClassName);
-  inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg,
-      instName, expandedClassNamespace, expandedClassName);
-  inst.namespaces = cls.namespaces;
-  inst.constructor = cls.constructor;
-  inst.constructor.args_list = expandArgumentListsTemplate(
-      cls.constructor.args_list, templateArg, instName, expandedClassNamespace,
-      expandedClassName);
-  inst.constructor.name = inst.name;
-  inst.deconstructor = cls.deconstructor;
   inst.deconstructor.name = inst.name;
-  inst.verbose_ = cls.verbose_;
   return inst;
 }
 
 /* ************************************************************************* */
-vector<Class> Class::expandTemplate(const string& templateArg,
+vector<Class> Class::expandTemplate(Str templateArg,
-    const vector<vector<string> >& instantiations) const {
+    const vector<Qualified>& instantiations) const {
   vector<Class> result;
-  BOOST_FOREACH(const vector<string>& instName, instantiations) {
+  BOOST_FOREACH(const Qualified& instName, instantiations) {
-    const string expandedName = name + instName.back();
+    Qualified expandedClass = (Qualified) (*this);
-    Class inst = expandClassTemplate(*this, templateArg, instName,
+    expandedClass.name += instName.name;
-        this->namespaces, expandedName);
+    const TemplateSubstitution ts(templateArg, instName, expandedClass);
-    inst.name = expandedName;
+    Class inst = expandTemplate(ts);
+    inst.name = expandedClass.name;
     inst.templateArgs.clear();
-    inst.typedefName = qualifiedName("::") + "<"
+    inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
-        + wrap::qualifiedName("::", instName) + ">";
+        + ">";
     result.push_back(inst);
   }
   return result;
 }
 
 /* ************************************************************************* */
-Class Class::expandTemplate(const string& templateArg,
+void Class::addMethod(bool verbose, bool is_const, Str methodName,
-    const vector<string>& instantiation,
+    const ArgumentList& argumentList, const ReturnValue& returnValue,
-    const std::vector<string>& expandedClassNamespace,
+    Str templateArgName, const vector<Qualified>& templateArgValues) {
-    const string& expandedClassName) const {
+  // Check if templated
-  return expandClassTemplate(*this, templateArg, instantiation,
+  if (!templateArgName.empty() && templateArgValues.size() > 0) {
-      expandedClassNamespace, expandedClassName);
+    // Create method to expand
+    // For all values of the template argument, create a new method
+    BOOST_FOREACH(const Qualified& instName, templateArgValues) {
+      const TemplateSubstitution ts(templateArgName, instName, this->name);
+      // substitute template in arguments
+      ArgumentList expandedArgs = argumentList.expandTemplate(ts);
+      // do the same for return type
+      ReturnValue expandedRetVal = returnValue.expandTemplate(ts);
+      // Now stick in new overload stack with expandedMethodName key
+      // but note we use the same, unexpanded methodName in overload
+      string expandedMethodName = methodName + instName.name;
+      methods[expandedMethodName].addOverload(methodName, expandedArgs,
+          expandedRetVal, is_const, instName, verbose);
+    }
+  } else
+    // just add overload
+    methods[methodName].addOverload(methodName, argumentList, returnValue,
+        is_const, Qualified(), verbose);
 }
 
 /* ************************************************************************* */
-std::string Class::getTypedef() const {
+void Class::erase_serialization() {
+  Methods::iterator it = methods.find("serializable");
+  if (it != methods.end()) {
+#ifndef WRAP_DISABLE_SERIALIZE
+    isSerializable = true;
+#else
+    // cout << "Ignoring serializable() flag in class " << name << endl;
+#endif
+    methods.erase(it);
+  }
+
+  it = methods.find("serialize");
+  if (it != methods.end()) {
+#ifndef WRAP_DISABLE_SERIALIZE
+    isSerializable = true;
+    hasSerialization = true;
+#else
+    // cout << "Ignoring serialize() flag in class " << name << endl;
+#endif
+    methods.erase(it);
+  }
+}
+
+/* ************************************************************************* */
+void Class::verifyAll(vector<string>& validTypes, bool& hasSerialiable) const {
+
+  hasSerialiable |= isSerializable;
+
+  // verify all of the function arguments
+  //TODO:verifyArguments<ArgumentList>(validTypes, constructor.args_list);
+  verifyArguments<StaticMethod>(validTypes, static_methods);
+  verifyArguments<Method>(validTypes, methods);
+
+  // verify function return types
+  verifyReturnTypes<StaticMethod>(validTypes, static_methods);
+  verifyReturnTypes<Method>(validTypes, methods);
+
+  // verify parents
+  if (!qualifiedParent.empty()
+      && find(validTypes.begin(), validTypes.end(),
+          qualifiedParent.qualifiedName("::")) == validTypes.end())
+    throw DependencyMissing(qualifiedParent.qualifiedName("::"),
+        qualifiedName("::"));
+}
+
+/* ************************************************************************* */
+void Class::appendInheritedMethods(const Class& cls,
+    const vector<Class>& classes) {
+
+  if (!cls.qualifiedParent.empty()) {
+
+    // Find parent
+    BOOST_FOREACH(const Class& parent, classes) {
+      // We found a parent class for our parent, TODO improve !
+      if (parent.name == cls.qualifiedParent.name) {
+        methods.insert(parent.methods.begin(), parent.methods.end());
+        appendInheritedMethods(parent, classes);
+      }
+    }
+  }
+}
+
+/* ************************************************************************* */
+string Class::getTypedef() const {
   string result;
-  BOOST_FOREACH(const string& namesp, namespaces) {
+  BOOST_FOREACH(Str namesp, namespaces) {
     result += ("namespace " + namesp + " { ");
   }
   result += ("typedef " + typedefName + " " + name + ";");
@@ -379,43 +372,22 @@ std::string Class::getTypedef() const {
 }
 
 /* ************************************************************************* */
-
 void Class::comment_fragment(FileWriter& proxyFile) const {
   proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
   proxyFile.oss
       << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
 
-  if (!constructor.args_list.empty())
+  constructor.comment_fragment(proxyFile);
-    proxyFile.oss << "%\n%-------Constructors-------\n";
-  BOOST_FOREACH(ArgumentList argList, constructor.args_list) {
-    proxyFile.oss << "%";
-    argList.emit_prototype(proxyFile, name);
-    proxyFile.oss << "\n";
-  }
 
   if (!methods.empty())
     proxyFile.oss << "%\n%-------Methods-------\n";
-  BOOST_FOREACH(const Methods::value_type& name_m, methods) {
+  BOOST_FOREACH(const Methods::value_type& name_m, methods)
-    const Method& m = name_m.second;
+    name_m.second.comment_fragment(proxyFile);
-    BOOST_FOREACH(ArgumentList argList, m.argLists) {
-      proxyFile.oss << "%";
-      argList.emit_prototype(proxyFile, m.name);
-      proxyFile.oss << " : returns "
-          << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
-    }
-  }
 
   if (!static_methods.empty())
     proxyFile.oss << "%\n%-------Static Methods-------\n";
-  BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
+  BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods)
-    const StaticMethod& m = name_m.second;
+    name_m.second.comment_fragment(proxyFile);
-    BOOST_FOREACH(ArgumentList argList, m.argLists) {
-      proxyFile.oss << "%";
-      argList.emit_prototype(proxyFile, m.name);
-      proxyFile.oss << " : returns "
-          << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
-    }
-  }
 
   if (hasSerialization) {
     proxyFile.oss << "%\n%-------Serialization Interface-------\n";
@@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
 
 /* ************************************************************************* */
 void Class::serialization_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, const std::string& wrapperName,
+    FileWriter& wrapperFile, Str wrapperName,
-    std::vector<std::string>& functionNames) const {
+    vector<string>& functionNames) const {
 
 //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 //{
 //  typedef boost::shared_ptr<Point3> Shared;
 //  checkArguments("string_serialize",nargout,nargin-1,0);
 //  Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
-//  std::ostringstream out_archive_stream;
+//  ostringstream out_archive_stream;
 //  boost::archive::text_oarchive out_archive(out_archive_stream);
 //  out_archive << *obj;
 //  out[0] = wrap< string >(out_archive_stream.str());
 //}
 
   int serialize_id = functionNames.size();
-  const string matlabQualName = qualifiedName("."), matlabUniqueName =
+  const string matlabQualName = qualifiedName(".");
-      qualifiedName(), cppClassName = qualifiedName("::");
+  const string matlabUniqueName = qualifiedName();
+  const string cppClassName = qualifiedName("::");
   const string wrapFunctionNameSerialize = matlabUniqueName
       + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
   functionNames.push_back(wrapFunctionNameSerialize);
@@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile,
       << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
 
   // Serialization boilerplate
-  wrapperFile.oss << "  std::ostringstream out_archive_stream;\n";
+  wrapperFile.oss << "  ostringstream out_archive_stream;\n";
   wrapperFile.oss
       << "  boost::archive::text_oarchive out_archive(out_archive_stream);\n";
   wrapperFile.oss << "  out_archive << *obj;\n";
@@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile,
 
 /* ************************************************************************* */
 void Class::deserialization_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, const std::string& wrapperName,
+    FileWriter& wrapperFile, Str wrapperName,
-    std::vector<std::string>& functionNames) const {
+    vector<string>& functionNames) const {
   //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   //{
   //  typedef boost::shared_ptr<Point3> Shared;
   //  checkArguments("Point3.string_deserialize",nargout,nargin,1);
   //  string serialized = unwrap< string >(in[0]);
-  //  std::istringstream in_archive_stream(serialized);
+  //  istringstream in_archive_stream(serialized);
   //  boost::archive::text_iarchive in_archive(in_archive_stream);
   //  Shared output(new Point3());
   //  in_archive >> *output;
   //  out[0] = wrap_shared_ptr(output,"Point3", false);
   //}
   int deserialize_id = functionNames.size();
-  const string matlabQualName = qualifiedName("."), matlabUniqueName =
+  const string matlabQualName = qualifiedName(".");
-      qualifiedName(), cppClassName = qualifiedName("::");
+  const string matlabUniqueName = qualifiedName();
+  const string cppClassName = qualifiedName("::");
   const string wrapFunctionNameDeserialize = matlabUniqueName
       + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
   functionNames.push_back(wrapFunctionNameDeserialize);
@@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
 
   // string argument with deserialization boilerplate
   wrapperFile.oss << "  string serialized = unwrap< string >(in[0]);\n";
-  wrapperFile.oss << "  std::istringstream in_archive_stream(serialized);\n";
+  wrapperFile.oss << "  istringstream in_archive_stream(serialized);\n";
   wrapperFile.oss
       << "  boost::archive::text_iarchive in_archive(in_archive_stream);\n";
   wrapperFile.oss << "  Shared output(new " << cppClassName << "());\n";
@@ -604,9 +578,21 @@ void Class::deserialization_fragments(FileWriter& proxyFile,
 }
 
 /* ************************************************************************* */
-std::string Class::getSerializationExport() const {
+string Class::getSerializationExport() const {
   //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
   return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
       + qualifiedName() + "\");";
 }
+
+/* ************************************************************************* */
+void Class::python_wrapper(FileWriter& wrapperFile) const {
+  wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n";
+  constructor.python_wrapper(wrapperFile, name);
+  BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values)
+    m.python_wrapper(wrapperFile, name);
+  BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values)
+    m.python_wrapper(wrapperFile, name);
+  wrapperFile.oss << ";\n\n";
+}
+
 /* ************************************************************************* */

wrap/Class.h

@@ -19,66 +19,118 @@
 
 #pragma once
 
-#include <string>
-#include <map>
-
 #include "Constructor.h"
 #include "Deconstructor.h"
 #include "Method.h"
 #include "StaticMethod.h"
 #include "TypeAttributesTable.h"
 
+#include <boost/foreach.hpp>
+#include <boost/range/adaptor/map.hpp>
+
+#include <string>
+#include <map>
+
 namespace wrap {
 
 /// Class has name, constructors, methods
-struct Class {
+class Class: public Qualified {
+
   typedef std::map<std::string, Method> Methods;
+  Methods methods; ///< Class methods
+
+public:
+
+  typedef const std::string& Str;
   typedef std::map<std::string, StaticMethod> StaticMethods;
 
-  /// Constructor creates an empty class
-  Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {}
-
   // Then the instance variables are set directly by the Module constructor
-  std::string name;                         ///< Class name
   std::vector<std::string> templateArgs; ///< Template arguments
   std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name]
   bool isVirtual; ///< Whether the class is part of a virtual inheritance chain
   bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
   bool hasSerialization; ///< Whether we should create the serialization functions
-  std::vector<std::string> qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack
+  Qualified qualifiedParent; ///< The *single* parent
-  Methods methods;                          ///< Class methods
   StaticMethods static_methods; ///< Static methods
-  std::vector<std::string> namespaces;      ///< Stack of namespaces
   Constructor constructor; ///< Class constructors
   Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
   bool verbose_; ///< verbose flag
 
+  /// Constructor creates an empty class
+  Class(bool verbose = true) :
+      isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor(
+          verbose), verbose_(verbose) {
+  }
+
+  size_t nrMethods() const {
+    return methods.size();
+  }
+  Method& method(Str name) {
+    return methods.at(name);
+  }
+  bool exists(Str name) const {
+    return methods.find(name) != methods.end();
+  }
+
   // And finally MATLAB code is emitted, methods below called by Module::matlab_code
-  void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
+  void matlab_proxy(Str toolboxPath, Str wrapperName,
-    FileWriter& wrapperFile, std::vector<std::string>& functionNames) const;          ///< emit proxy class
+      const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
+      std::vector<std::string>& functionNames) const; ///< emit proxy class
 
-  std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
+  Class expandTemplate(const TemplateSubstitution& ts) const;
 
-  std::vector<Class> expandTemplate(const std::string& templateArg, const std::vector<std::vector<std::string> >& instantiations) const;
+  std::vector<Class> expandTemplate(Str templateArg,
+      const std::vector<Qualified>& instantiations) const;
 
-  Class expandTemplate(const std::string& templateArg, const std::vector<std::string>& instantiation, const std::vector<std::string>& expandedClassNamespace, const std::string& expandedClassName) const;
+  /// Add potentially overloaded, potentially templated method
+  void addMethod(bool verbose, bool is_const, Str methodName,
+      const ArgumentList& argumentList, const ReturnValue& returnValue,
+      Str templateArgName, const std::vector<Qualified>& templateArgValues);
 
-  // The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
+  /// Post-process classes for serialization markers
+  void erase_serialization(); // non-const !
+
+  /// verify all of the function arguments
+  void verifyAll(std::vector<std::string>& functionNames,
+      bool& hasSerialiable) const;
+
+  void appendInheritedMethods(const Class& cls,
+      const std::vector<Class>& classes);
+
+  /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string.
   std::string getTypedef() const;
 
-  // Returns the string for an export flag
+  /// Returns the string for an export flag
   std::string getSerializationExport() const;
 
-  // Creates a member function that performs serialization
+  /// Creates a member function that performs serialization
   void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      const std::string& wrapperName, std::vector<std::string>& functionNames) const;
+      Str wrapperName, std::vector<std::string>& functionNames) const;
 
-  // Creates a static member function that performs deserialization
+  /// Creates a static member function that performs deserialization
   void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      const std::string& wrapperName, std::vector<std::string>& functionNames) const;
+      Str wrapperName, std::vector<std::string>& functionNames) const;
+
+  // emit python wrapper
+  void python_wrapper(FileWriter& wrapperFile) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const Class& cls) {
+    os << "class " << cls.name << "{\n";
+    os << cls.constructor << ";\n";
+    BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values)
+      os << m << ";\n";
+    BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values)
+      os << m << ";\n";
+    os << "};" << std::endl;
+    return os;
+  }
 
 private:
-  void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector<std::string>& functionNames) const;
+
+  void pointer_constructor_fragments(FileWriter& proxyFile,
+      FileWriter& wrapperFile, Str wrapperName,
+      std::vector<std::string>& functionNames) const;
+
   void comment_fragment(FileWriter& proxyFile) const;
 };
 

wrap/Constructor.cpp

@@ -29,25 +29,28 @@
 using namespace std;
 using namespace wrap;
 
-
 /* ************************************************************************* */
-string Constructor::matlab_wrapper_name(const string& className) const {
+string Constructor::matlab_wrapper_name(Str className) const {
   string str = "new_" + className;
   return str;
 }
 
 /* ************************************************************************* */
-void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName,
+void Constructor::proxy_fragment(FileWriter& file,
-        bool hasParent, const int id, const ArgumentList args) const {
+    const std::string& wrapperName, bool hasParent, const int id,
+    const ArgumentList args) const {
   size_t nrArgs = args.size();
   // check for number of arguments...
   file.oss << "      elseif nargin == " << nrArgs;
-  if (nrArgs>0) file.oss << " && ";
+  if (nrArgs > 0)
+    file.oss << " && ";
   // ...and their types
   bool first = true;
   for (size_t i = 0; i < nrArgs; i++) {
-    if (!first) file.oss << " && ";
+    if (!first)
-    file.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
+      file.oss << " && ";
+    file.oss << "isa(varargin{" << i + 1 << "},'" << args[i].matlabClass(".")
+        << "')";
     first = false;
   }
   // emit code for calling constructor
@@ -65,16 +68,16 @@ void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperNam
 }
 
 /* ************************************************************************* */
-string Constructor::wrapper_fragment(FileWriter& file,
+string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
-         const string& cppClassName,
+    Str matlabUniqueName, Str cppBaseClassName, int id,
-         const string& matlabUniqueName,
-         const string& cppBaseClassName,
-         int id,
     const ArgumentList& al) const {
 
-  const string wrapFunctionName = matlabUniqueName + "_constructor_" + boost::lexical_cast<string>(id);
+  const string wrapFunctionName = matlabUniqueName + "_constructor_"
+      + boost::lexical_cast<string>(id);
 
-  file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
+  file.oss << "void " << wrapFunctionName
+      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])"
+      << endl;
   file.oss << "{\n";
   file.oss << "  mexAtExit(&_deleteAllObjects);\n";
   //Typedef boost::shared_ptr
@@ -84,20 +87,27 @@ string Constructor::wrapper_fragment(FileWriter& file,
   //Check to see if there will be any arguments and remove {} for consiseness
   if (al.size() > 0)
     al.matlab_unwrap(file); // unwrap arguments
-  file.oss << "  Shared *self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
+  file.oss << "  Shared *self = new Shared(new " << cppClassName << "("
+      << al.names() << "));" << endl;
   file.oss << "  collector_" << matlabUniqueName << ".insert(self);\n";
 
   if (verbose_)
     file.oss << "  std::cout << \"constructed \" << self << std::endl;" << endl;
-  file.oss << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl;
+  file.oss
-  file.oss << "  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;" << endl;
+      << "  out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);"
+      << endl;
+  file.oss << "  *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;"
+      << endl;
 
   // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
   if (!cppBaseClassName.empty()) {
     file.oss << "\n";
-    file.oss << "  typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n";
+    file.oss << "  typedef boost::shared_ptr<" << cppBaseClassName
-    file.oss << "  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
+        << "> SharedBase;\n";
-    file.oss << "  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
+    file.oss
+        << "  out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
+    file.oss
+        << "  *reinterpret_cast<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
   }
 
   file.oss << "}" << endl;
@@ -106,3 +116,9 @@ string Constructor::wrapper_fragment(FileWriter& file,
 }
 
 /* ************************************************************************* */
+void Constructor::python_wrapper(FileWriter& wrapperFile, Str className) const {
+  wrapperFile.oss << "  .def(\"" << name_ << "\", &" << className << "::" << name_
+      << ");\n";
+}
+
+/* ************************************************************************* */

wrap/Constructor.h

@@ -18,7 +18,7 @@
 
 #pragma once
 
-#include "Argument.h"
+#include "OverloadedFunction.h"
 
 #include <string>
 #include <vector>
@@ -26,42 +26,64 @@
 namespace wrap {
 
 // Constructor class
-struct Constructor {
+struct Constructor: public OverloadedFunction {
+
+  typedef const std::string& Str;
 
   /// Constructor creates an empty class
-  Constructor(bool verbose = false) :
+  Constructor(bool verbose = false) {
-      verbose_(verbose) {
+    verbose_ = verbose;
   }
 
-  // Then the instance variables are set directly by the Module constructor
+  Constructor expandTemplate(const TemplateSubstitution& ts) const {
-  std::vector<ArgumentList> args_list;
+    Constructor inst = *this;
-  std::string name;
+    inst.argLists_ = expandArgumentListsTemplate(ts);
-  bool verbose_;
+    inst.name_ = ts.expandedClassName();
+    return inst;
+  }
 
   // MATLAB code generation
   // toolboxPath is main toolbox directory, e.g., ../matlab
   // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m
 
   /// wrapper name
-  std::string matlab_wrapper_name(const std::string& className) const;
+  std::string matlab_wrapper_name(Str className) const;
+
+  void comment_fragment(FileWriter& proxyFile) const {
+    if (nrOverloads() > 0)
+      proxyFile.oss << "%\n%-------Constructors-------\n";
+    for (size_t i = 0; i < nrOverloads(); i++) {
+      proxyFile.oss << "%";
+      argumentList(i).emit_prototype(proxyFile, name_);
+      proxyFile.oss << "\n";
+    }
+  }
 
   /**
    * Create fragment to select constructor in proxy class, e.g.,
    * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
    */
-  void proxy_fragment(FileWriter& file, const std::string& wrapperName,
+  void proxy_fragment(FileWriter& file, Str wrapperName, bool hasParent,
-      bool hasParent, const int id, const ArgumentList args) const;
+      const int id, const ArgumentList args) const;
 
   /// cpp wrapper
-  std::string wrapper_fragment(FileWriter& file,
+  std::string wrapper_fragment(FileWriter& file, Str cppClassName,
-      const std::string& cppClassName, const std::string& matlabUniqueName,
+      Str matlabUniqueName, Str cppBaseClassName, int id,
-      const std::string& cppBaseClassName, int id,
       const ArgumentList& al) const;
 
   /// constructor function
-  void generate_construct(FileWriter& file, const std::string& cppClassName,
+  void generate_construct(FileWriter& file, Str cppClassName,
       std::vector<ArgumentList>& args_list) const;
 
+  // emit python wrapper
+  void python_wrapper(FileWriter& wrapperFile, Str className) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const Constructor& m) {
+    for (size_t i = 0; i < m.nrOverloads(); i++)
+      os << m.name_ << m.argLists_[i];
+    return os;
+  }
+
 };
 
 } // \namespace wrap

wrap/FileWriter.cpp

@@ -15,14 +15,15 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-FileWriter::FileWriter(const string& filename, bool verbose, const string& comment_str)
+FileWriter::FileWriter(const string& filename, bool verbose,
-: verbose_(verbose),filename_(filename), comment_str_(comment_str)
+    const string& comment_str) :
-{
+    verbose_(verbose), filename_(filename), comment_str_(comment_str) {
 }
 
 /* ************************************************************************* */
 void FileWriter::emit(bool add_header, bool force_overwrite) const {
-  if (verbose_) cerr << "generating " << filename_ << " ";
+  if (verbose_)
+    cerr << "generating " << filename_ << " ";
   // read in file if it exists
   string existing_contents;
   bool file_exists = true;
@@ -35,23 +36,17 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
   // Only write a file if it is new, an update, or overwrite is forced
   string new_contents = oss.str();
   if (force_overwrite || !file_exists || existing_contents != new_contents) {
-    ofstream ofs(filename_.c_str(), ios::binary); // Binary to use LF line endings instead of CRLF
+    // Binary to use LF line endings instead of CRLF
-    if (!ofs) throw CantOpenFile(filename_);
+    ofstream ofs(filename_.c_str(), ios::binary);
+    if (!ofs)
+      throw CantOpenFile(filename_);
 
     // dump in stringstream
     ofs << new_contents;
     ofs.close();
-    if (verbose_) cerr << " ...complete" << endl;
-
-    // Add small message whenever writing a new file and not running in full verbose mode
-    if (!verbose_)
-      cout << "wrap: generating " << filename_ << endl;
-  } else {
-    if (verbose_) cerr << " ...no update" << endl;
   }
+  if (verbose_)
+    cerr << " ...no update" << endl;
 }
 /* ************************************************************************* */
 
-
-
-

wrap/FullyOverloadedFunction.h

@@ -0,0 +1,133 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file FullyOverloadedFunction.h
+ * @brief Function that can be fully overloaded: arguments and return values
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#pragma once
+
+#include "OverloadedFunction.h"
+
+namespace wrap {
+
+/**
+ * Signature Overload (including return value)
+ */
+class SignatureOverloads: public ArgumentOverloads {
+
+protected:
+
+  std::vector<ReturnValue> returnVals_;
+
+public:
+
+  const ReturnValue& returnValue(size_t i) const {
+    return returnVals_.at(i);
+  }
+
+  void push_back(const ArgumentList& args, const ReturnValue& retVal) {
+    argLists_.push_back(args);
+    returnVals_.push_back(retVal);
+  }
+
+  void verifyReturnTypes(const std::vector<std::string>& validtypes,
+      const std::string& s) const {
+    BOOST_FOREACH(const ReturnValue& retval, returnVals_) {
+      retval.type1.verify(validtypes, s);
+      if (retval.isPair)
+        retval.type2.verify(validtypes, s);
+    }
+  }
+
+  // TODO use transform ?
+  std::vector<ReturnValue> expandReturnValuesTemplate(
+      const TemplateSubstitution& ts) const {
+    std::vector<ReturnValue> result;
+    BOOST_FOREACH(const ReturnValue& retVal, returnVals_) {
+      ReturnValue instRetVal = retVal.expandTemplate(ts);
+      result.push_back(instRetVal);
+    }
+    return result;
+  }
+
+  /// Expand templates, imperative !
+  void expandTemplate(const TemplateSubstitution& ts) {
+    // substitute template in arguments
+    argLists_ = expandArgumentListsTemplate(ts);
+    // do the same for return types
+    returnVals_ = expandReturnValuesTemplate(ts);
+  }
+
+  // emit a list of comments, one for each overload
+  void usage_fragment(FileWriter& proxyFile, const std::string& name) const {
+    unsigned int argLCount = 0;
+    BOOST_FOREACH(ArgumentList argList, argLists_) {
+      argList.emit_prototype(proxyFile, name);
+      if (argLCount != nrOverloads() - 1)
+        proxyFile.oss << ", ";
+      else
+        proxyFile.oss << " : returns " << returnValue(0).return_type(false)
+            << std::endl;
+      argLCount++;
+    }
+  }
+
+  // emit a list of comments, one for each overload
+  void comment_fragment(FileWriter& proxyFile, const std::string& name) const {
+    size_t i = 0;
+    BOOST_FOREACH(ArgumentList argList, argLists_) {
+      proxyFile.oss << "%";
+      argList.emit_prototype(proxyFile, name);
+      proxyFile.oss << " : returns " << returnVals_[i++].return_type(false)
+          << std::endl;
+    }
+  }
+
+  friend std::ostream& operator<<(std::ostream& os,
+      const SignatureOverloads& overloads) {
+    for (size_t i = 0; i < overloads.nrOverloads(); i++)
+      os << overloads.returnVals_[i] << overloads.argLists_[i] << std::endl;
+    return os;
+  }
+
+};
+
+class FullyOverloadedFunction: public Function, public SignatureOverloads {
+
+public:
+
+  bool addOverload(const std::string& name, const ArgumentList& args,
+      const ReturnValue& retVal, const Qualified& instName = Qualified(),
+      bool verbose = false) {
+    bool first = initializeOrCheck(name, instName, verbose);
+    SignatureOverloads::push_back(args, retVal);
+    return first;
+  }
+
+};
+
+// Templated checking functions
+// TODO: do this via polymorphism, use transform ?
+
+template<class F>
+inline void verifyReturnTypes(const std::vector<std::string>& validTypes,
+    const std::map<std::string, F>& vt) {
+  typedef typename std::map<std::string, F>::value_type NamedMethod;
+  BOOST_FOREACH(const NamedMethod& namedMethod, vt)
+    namedMethod.second.verifyReturnTypes(validTypes);
+}
+
+} // \namespace wrap
+

wrap/Function.cpp

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Function.ccp
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#include "Function.h"
+#include "utilities.h"
+
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace wrap;
+
+/* ************************************************************************* */
+bool Function::initializeOrCheck(const std::string& name,
+    const Qualified& instName, bool verbose) {
+
+  if (name.empty())
+    throw std::runtime_error(
+        "Function::initializeOrCheck called with empty name");
+
+  // Check if this overload is give to the correct method
+  if (name_.empty()) {
+    name_ = name;
+    templateArgValue_ = instName;
+    verbose_ = verbose;
+    return true;
+  } else {
+    if (name_ != name || templateArgValue_ != instName || verbose_ != verbose)
+      throw std::runtime_error(
+          "Function::initializeOrCheck called with different arguments:  with name "
+              + name + " instead of expected " + name_
+              + ", or with template argument " + instName.qualifiedName(":")
+              + " instead of expected " + templateArgValue_.qualifiedName(":"));
+
+    return false;
+  }
+}
+
+/* ************************************************************************* */

wrap/Function.h

@@ -0,0 +1,56 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Function.h
+ * @brief Base class for global functions and methods
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#pragma once
+
+#include "Argument.h"
+
+namespace wrap {
+
+/// Function class
+class Function {
+
+protected:
+
+  std::string name_; ///< name of method
+  Qualified templateArgValue_; ///< value of template argument if applicable
+  bool verbose_;
+
+public:
+
+  /**
+   * @brief first time, fill in instance variables, otherwise check if same
+   * @return true if first time, false thereafter
+   */
+  bool initializeOrCheck(const std::string& name, const Qualified& instName =
+      Qualified(), bool verbose = false);
+
+  std::string name() const {
+    return name_;
+  }
+
+  std::string matlabName() const {
+    if (templateArgValue_.empty())
+      return name_;
+    else
+      return name_ + templateArgValue_.name;
+  }
+};
+
+} // \namespace wrap
+

wrap/GlobalFunction.cpp

@@ -16,38 +16,31 @@ namespace wrap {
 using namespace std;
 
 /* ************************************************************************* */
-void GlobalFunction::addOverload(bool verbose, const std::string& name,
+void GlobalFunction::addOverload(const Qualified& overload,
     const ArgumentList& args, const ReturnValue& retVal,
-    const StrVec& ns_stack) {
+    const Qualified& instName, bool verbose) {
-  this->verbose_ = verbose;
+  string name(overload.name);
-  this->name = name;
+  FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose);
-  this->argLists.push_back(args);
+  overloads.push_back(overload);
-  this->returnVals.push_back(retVal);
-  this->namespaces.push_back(ns_stack);
 }
 
 /* ************************************************************************* */
-void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
+void GlobalFunction::matlab_proxy(const string& toolboxPath,
-    const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
+    const string& wrapperName, const TypeAttributesTable& typeAttributes,
-    FileWriter& file, std::vector<std::string>& functionNames) const {
+    FileWriter& file, vector<string>& functionNames) const {
 
   // cluster overloads with same namespace
   // create new GlobalFunction structures around namespaces - same namespaces and names are overloads
   // map of namespace to global function
   typedef map<string, GlobalFunction> GlobalFunctionMap;
   GlobalFunctionMap grouped_functions;
-  for (size_t i = 0; i < namespaces.size(); ++i) {
+  for (size_t i = 0; i < overloads.size(); ++i) {
-    StrVec ns = namespaces.at(i);
+    Qualified overload = overloads.at(i);
-    string str_ns = qualifiedName("", ns, "");
+    // use concatenated namespaces as key
-    ReturnValue ret = returnVals.at(i);
+    string str_ns = qualifiedName("", overload.namespaces);
-    ArgumentList args = argLists.at(i);
+    const ReturnValue& ret = returnValue(i);
-
+    const ArgumentList& args = argumentList(i);
-    if (!grouped_functions.count(str_ns))
+    grouped_functions[str_ns].addOverload(overload, args, ret);
-      grouped_functions[str_ns] = GlobalFunction(name, verbose_);
-
-    grouped_functions[str_ns].argLists.push_back(args);
-    grouped_functions[str_ns].returnVals.push_back(ret);
-    grouped_functions[str_ns].namespaces.push_back(ns);
   }
 
   size_t lastcheck = grouped_functions.size();
@@ -60,37 +53,34 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
 }
 
 /* ************************************************************************* */
-void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
+void GlobalFunction::generateSingleFunction(const string& toolboxPath,
-    const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
+    const string& wrapperName, const TypeAttributesTable& typeAttributes,
-    FileWriter& file, std::vector<std::string>& functionNames) const {
+    FileWriter& file, vector<string>& functionNames) const {
 
   // create the folder for the namespace
-  const StrVec& ns = namespaces.front();
+  const Qualified& overload1 = overloads.front();
-  createNamespaceStructure(ns, toolboxPath);
+  createNamespaceStructure(overload1.namespaces, toolboxPath);
 
   // open destination mfunctionFileName
-  string mfunctionFileName = toolboxPath;
+  string mfunctionFileName = overload1.matlabName(toolboxPath);
-  if (!ns.empty())
-    mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns);
-  mfunctionFileName += "/" + name + ".m";
   FileWriter mfunctionFile(mfunctionFileName, verbose_, "%");
 
   // get the name of actual matlab object
-  const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName =
+  const string matlabQualName = overload1.qualifiedName(".");
-      qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name);
+  const string matlabUniqueName = overload1.qualifiedName("");
+  const string cppName = overload1.qualifiedName("::");
 
-  mfunctionFile.oss << "function varargout = " << name << "(varargin)\n";
+  mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n";
 
-  for (size_t overload = 0; overload < argLists.size(); ++overload) {
+  for (size_t i = 0; i < nrOverloads(); ++i) {
-    const ArgumentList& args = argLists[overload];
+    const ArgumentList& args = argumentList(i);
-    const ReturnValue& returnVal = returnVals[overload];
+    const ReturnValue& returnVal = returnValue(i);
 
     const int id = functionNames.size();
 
     // Output proxy matlab code
-    mfunctionFile.oss << "      " << (overload == 0 ? "" : "else");
+    mfunctionFile.oss << "      " << (i == 0 ? "" : "else");
-    argLists[overload].emit_conditional_call(mfunctionFile,
+    args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this"
-        returnVals[overload], wrapperName, id, true); // true omits "this"
 
     // Output C++ wrapper code
 
@@ -114,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
     args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
 
     // call method with default type and wrap result
-    if (returnVal.type1 != "void")
+    if (returnVal.type1.name != "void")
       returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
           typeAttributes);
     else
@@ -137,6 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
   mfunctionFile.emit(true);
 }
 
+/* ************************************************************************* */
+void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const {
+  wrapperFile.oss << "def(\"" << name_ << "\", " << name_ << ");\n";
+}
+
 /* ************************************************************************* */
 
 } // \namespace wrap

wrap/GlobalFunction.h

@@ -9,43 +9,35 @@
 
 #pragma once
 
-#include "Argument.h"
+#include "FullyOverloadedFunction.h"
-#include "ReturnValue.h"
 
 namespace wrap {
 
-struct GlobalFunction {
+struct GlobalFunction: public FullyOverloadedFunction {
 
-  typedef std::vector<std::string> StrVec;
+  std::vector<Qualified> overloads; ///< Stack of qualified names
 
-  bool verbose_;
+  // adds an overloaded version of this function,
-  std::string name;
+  void addOverload(const Qualified& overload, const ArgumentList& args,
+      const ReturnValue& retVal, const Qualified& instName = Qualified(),
+      bool verbose = false);
 
-  // each overload, regardless of namespace
+  void verifyArguments(const std::vector<std::string>& validArgs) const {
-  std::vector<ArgumentList> argLists; ///< arugments for each overload
+    SignatureOverloads::verifyArguments(validArgs, name_);
-  std::vector<ReturnValue> returnVals; ///< returnVals for each overload
-  std::vector<StrVec> namespaces; ///< Stack of namespaces
-
-  // Constructor only used in Module
-  GlobalFunction(bool verbose = true) :
-      verbose_(verbose) {
   }
 
-  // Used to reconstruct
+  void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
-  GlobalFunction(const std::string& name_, bool verbose = true) :
+    SignatureOverloads::verifyReturnTypes(validtypes, name_);
-      verbose_(verbose), name(name_) {
   }
 
-  // adds an overloaded version of this function
-  void addOverload(bool verbose, const std::string& name,
-      const ArgumentList& args, const ReturnValue& retVal,
-      const StrVec& ns_stack);
-
   // codegen function called from Module to build the cpp and matlab versions of the function
   void matlab_proxy(const std::string& toolboxPath,
       const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
       FileWriter& file, std::vector<std::string>& functionNames) const;
 
+  // emit python wrapper
+  void python_wrapper(FileWriter& wrapperFile) const;
+
 private:
 
   // Creates a single global function - all in same namespace

wrap/Method.cpp

@@ -29,155 +29,53 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void Method::addOverload(bool verbose, bool is_const, const std::string& name,
+bool Method::addOverload(Str name, const ArgumentList& args,
-    const ArgumentList& args, const ReturnValue& retVal) {
+    const ReturnValue& retVal, bool is_const, const Qualified& instName,
-  this->verbose_ = verbose;
+    bool verbose) {
-  this->is_const_ = is_const;
+  bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose);
-  this->name = name;
+  if (first)
-  this->argLists.push_back(args);
+    is_const_ = is_const;
-  this->returnVals.push_back(retVal);
+  else if (is_const && !is_const_)
+    throw std::runtime_error(
+        "Method::addOverload now designated as const whereas before it was not");
+  else if (!is_const && is_const_)
+    throw std::runtime_error(
+        "Method::addOverload now designated as non-const whereas before it was");
+  return first;
 }
 
 /* ************************************************************************* */
-void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile,
+void Method::proxy_header(FileWriter& proxyFile) const {
-    const string& cppClassName, const std::string& matlabQualName,
+  proxyFile.oss << "    function varargout = " << matlabName()
-    const std::string& matlabUniqueName, const string& wrapperName,
+      << "(this, varargin)\n";
-    const TypeAttributesTable& typeAttributes,
-    vector<string>& functionNames) const {
-
-  // Create function header
-  file.oss << "    function varargout = " << name << "(this, varargin)\n";
-
-  // Emit comments for documentation
-  string up_name = boost::to_upper_copy(name);
-  file.oss << "      % " << up_name << " usage: ";
-  unsigned int argLCount = 0;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    argList.emit_prototype(file, name);
-    if (argLCount != argLists.size() - 1)
-      file.oss << ", ";
-    else
-      file.oss << " : returns "
-          << returnVals[0].return_type(false, returnVals[0].pair) << endl;
-    argLCount++;
-  }
-
-  // Emit URL to Doxygen page
-  file.oss << "      % "
-      << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
-      << endl;
-
-  // Document all overloads, if any
-  if (argLists.size() > 1) {
-    file.oss << "      % " << "" << endl;
-    file.oss << "      % " << "Method Overloads" << endl;
-    BOOST_FOREACH(ArgumentList argList, argLists) {
-      file.oss << "      % ";
-      argList.emit_prototype(file, name);
-      file.oss << endl;
-    }
-  }
-
-  // Handle special case of single overload with all numeric arguments
-  if (argLists.size() == 1 && argLists[0].allScalar()) {
-    // Output proxy matlab code
-    file.oss << "      ";
-    const int id = (int) functionNames.size();
-    argLists[0].emit_call(file, returnVals[0], wrapperName, id);
-
-    // Output C++ wrapper code
-    const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
-        matlabUniqueName, 0, id, typeAttributes);
-
-    // Add to function list
-    functionNames.push_back(wrapFunctionName);
-  } else {
-    // Check arguments for all overloads
-    for (size_t overload = 0; overload < argLists.size(); ++overload) {
-
-      // Output proxy matlab code
-      file.oss << "      " << (overload == 0 ? "" : "else");
-      const int id = (int) functionNames.size();
-      argLists[overload].emit_conditional_call(file, returnVals[overload],
-          wrapperName, id);
-
-      // Output C++ wrapper code
-      const string wrapFunctionName = wrapper_fragment(wrapperFile,
-          cppClassName, matlabUniqueName, overload, id, typeAttributes);
-
-      // Add to function list
-      functionNames.push_back(wrapFunctionName);
-    }
-    file.oss << "      else\n";
-    file.oss
-        << "        error('Arguments do not match any overload of function "
-        << matlabQualName << "." << name << "');" << endl;
-    file.oss << "      end\n";
-  }
-
-  file.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-string Method::wrapper_fragment(FileWriter& file, const string& cppClassName,
+string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-    const string& matlabUniqueName, int overload, int id,
+    Str matlabUniqueName, const ArgumentList& args,
-    const TypeAttributesTable& typeAttributes) const {
+    const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
-
+    const Qualified& instName) const {
-  // generate code
-
-  const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
-      + boost::lexical_cast<string>(id);
-
-  const ArgumentList& args = argLists[overload];
-  const ReturnValue& returnVal = returnVals[overload];
-
-  // call
-  file.oss << "void " << wrapFunctionName
-      << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
-  // start
-  file.oss << "{\n";
-
-  if (returnVal.isPair) {
-    if (returnVal.category1 == ReturnValue::CLASS)
-      file.oss << "  typedef boost::shared_ptr<"
-          << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1
-          << ";" << endl;
-    if (returnVal.category2 == ReturnValue::CLASS)
-      file.oss << "  typedef boost::shared_ptr<"
-          << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2
-          << ";" << endl;
-  } else if (returnVal.category1 == ReturnValue::CLASS)
-    file.oss << "  typedef boost::shared_ptr<" << returnVal.qualifiedType1("::")
-        << "> Shared" << returnVal.type1 << ";" << endl;
-
-  file.oss << "  typedef boost::shared_ptr<" << cppClassName << "> Shared;"
-      << endl;
-
   // check arguments
   // extra argument obj -> nargin-1 is passed !
   // example: checkArguments("equals",nargout,nargin-1,2);
-  file.oss << "  checkArguments(\"" << name << "\",nargout,nargin-1,"
+  wrapperFile.oss << "  checkArguments(\"" << name_ << "\",nargout,nargin-1,"
       << args.size() << ");\n";
 
   // get class pointer
   // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
-  file.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName
+  wrapperFile.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName
       << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
-  // unwrap arguments, see Argument.cpp
+
-  args.matlab_unwrap(file, 1);
+  // unwrap arguments, see Argument.cpp, we start at 1 as first is obj
+  args.matlab_unwrap(wrapperFile, 1);
 
   // call method and wrap result
-  // example: out[0]=wrap<bool>(self->return_field(t));
+  // example: out[0]=wrap<bool>(obj->return_field(t));
-  if (returnVal.type1 != "void")
+  string expanded = "obj->" + name_;
-    returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file,
+  if (!instName.empty())
-        typeAttributes);
+    expanded += ("<" + instName.qualifiedName("::") + ">");
-  else
-    file.oss << "  obj->" + name + "(" + args.names() + ");\n";
 
-  // finish
+  return expanded;
-  file.oss << "}\n";
-
-  return wrapFunctionName;
 }
 
 /* ************************************************************************* */

wrap/Method.h

@@ -18,49 +18,46 @@
 
 #pragma once
 
-#include "Argument.h"
+#include "StaticMethod.h"
-#include "ReturnValue.h"
-#include "TypeAttributesTable.h"
-
-#include <string>
-#include <list>
 
 namespace wrap {
 
 /// Method class
-struct Method {
+class Method: public StaticMethod {
 
-  /// Constructor creates empty object
-  Method(bool verbose = true) :
-      verbose_(verbose), is_const_(false) {}
-
-  // Then the instance variables are set directly by the Module constructor
-  bool verbose_;
   bool is_const_;
-  std::string name;
-  std::vector<ArgumentList> argLists;
-  std::vector<ReturnValue> returnVals;
 
-  // The first time this function is called, it initializes the class members
+public:
-  // with those in rhs, but in subsequent calls it adds additional argument
-  // lists as function overloads.
-  void addOverload(bool verbose, bool is_const, const std::string& name,
-    const ArgumentList& args, const ReturnValue& retVal);
 
-  // MATLAB code generation
+  typedef const std::string& Str;
-  // classPath is class directory, e.g., ../matlab/@Point2
+
-  void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
+  bool addOverload(Str name, const ArgumentList& args,
-    const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName,
+      const ReturnValue& retVal, bool is_const, const Qualified& instName =
-    const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
+          Qualified(), bool verbose = false);
-    std::vector<std::string>& functionNames) const;
+
+  virtual bool isStatic() const {
+    return false;
+  }
+
+  virtual bool isConst() const {
+    return is_const_;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const Method& m) {
+    for (size_t i = 0; i < m.nrOverloads(); i++)
+      os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
+    return os;
+  }
 
 private:
-  std::string wrapper_fragment(FileWriter& file,
+
-      const std::string& cppClassName,
+  // Emit method header
-      const std::string& matlabUniqueName,
+  void proxy_header(FileWriter& proxyFile) const;
-      int overload,
+
-      int id,
+  virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
-      const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
+      Str matlabUniqueName, const ArgumentList& args,
+      const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
+      const Qualified& instName) const;
 };
 
 } // \namespace wrap

wrap/Module.cpp

@@ -62,16 +62,22 @@ typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
 /* ************************************************************************* */ 
  
 /* ************************************************************************* */ 
-void handle_possible_template(vector<Class>& classes, const Class& cls, const string& templateArgument, const vector<vector<string> >& instantiations) { 
+// If a number of template arguments were given, generate a number of expanded
-  if(instantiations.empty()) { 
+// class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes
+static void handle_possible_template(vector<Class>& classes, const Class& cls,
+    const vector<Qualified>& instantiations) {
+  if (cls.templateArgs.empty() || instantiations.empty()) {
     classes.push_back(cls);
   } else {
-    vector<Class> classInstantiations = cls.expandTemplate(templateArgument, instantiations); 
+    if (cls.templateArgs.size() != 1)
-    BOOST_FOREACH(const Class& c, classInstantiations) { 
+      throw std::runtime_error(
+          "In-line template instantiations only handle a single template argument");
+    vector<Class> classInstantiations = //
+        cls.expandTemplate(cls.templateArgs.front(), instantiations);
+    BOOST_FOREACH(const Class& c, classInstantiations)
       classes.push_back(c);
   }
 }
-} 
 
 /* ************************************************************************* */
 Module::Module(const std::string& moduleName, bool enable_verbose)
@@ -94,28 +100,9 @@ Module::Module(const string& interfacePath,
 
 /* ************************************************************************* */
 void Module::parseMarkup(const std::string& data) {
-  // these variables will be imperatively updated to gradually build [cls] 
+  // The parse imperatively :-( updates variables gradually during parse
   // The one with postfix 0 are used to reset the variables after parse. 
-  string methodName, methodName0; 
+  vector<string> namespaces; // current namespace tag
-  bool isConst, isConst0 = false; 
-  ReturnValue retVal0, retVal;
-  Argument arg0, arg; 
-  ArgumentList args0, args; 
-  vector<string> arg_dup; ///keep track of duplicates 
-  Constructor constructor0(verbose), constructor(verbose);
-  Deconstructor deconstructor0(verbose), deconstructor(verbose);
-  StaticMethod static_method0(verbose), static_method(verbose);
-  Class cls0(verbose),cls(verbose);
-  GlobalFunction globalFunc0(verbose), globalFunc(verbose);
-  ForwardDeclaration fwDec0, fwDec; 
-  vector<string> namespaces, /// current namespace tag 
-                 namespaces_return; /// namespace for current return type 
-  string templateArgument; 
-  vector<string> templateInstantiationNamespace; 
-  vector<vector<string> > templateInstantiations; 
-  TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; 
-  string include_path = ""; 
-  const string null_str = ""; 
  
   //---------------------------------------------------------------------------- 
   // Grammar with actions that build the Class object. Actions are 
@@ -144,51 +131,63 @@ void Module::parseMarkup(const std::string& data) {
  
   Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; 
  
-  Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::"); 
+  Argument arg0, arg;
+  Rule namespace_arg_p = namespace_name_p
+      [push_back_a(arg.type.namespaces)] >> str_p("::");
  
   Rule argEigenType_p = 
-    eigenType_p[assign_a(arg.type)];
+    eigenType_p[assign_a(arg.type.name)];
  
   Rule eigenRef_p = 
       !str_p("const") [assign_a(arg.is_const,true)] >> 
-      eigenType_p     [assign_a(arg.type)] >> 
+      eigenType_p     [assign_a(arg.type.name)] >>
       ch_p('&')       [assign_a(arg.is_ref,true)]; 
  
   Rule classArg_p = 
     !str_p("const") [assign_a(arg.is_const,true)] >> 
     *namespace_arg_p >> 
-    className_p[assign_a(arg.type)] >> 
+    className_p[assign_a(arg.type.name)] >>
     !ch_p('&')[assign_a(arg.is_ref,true)];
  
   Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; 
 
+  // TODO, do we really need cls here? Non-local
+  Class cls0(verbose),cls(verbose);
   Rule classParent_p = 
-    *(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >> 
+    *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >>
-    className_p[push_back_a(cls.qualifiedParent)]; 
+    className_p[assign_a(cls.qualifiedParent.name)];
  
-  Rule templateInstantiation_p = 
+  // parse "gtsam::Pose2" and add to templateArgValues
-    (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> 
+  Qualified templateArgValue;
-    className_p[push_back_a(templateInstantiationNamespace)]) 
+  vector<Qualified> templateArgValues;
-    [push_back_a(templateInstantiations, templateInstantiationNamespace)] 
+  Rule templateArgValue_p =
-    [clear_a(templateInstantiationNamespace)]; 
+    (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
+    className_p[assign_a(templateArgValue.name)])
+    [push_back_a(templateArgValues, templateArgValue)]
+    [clear_a(templateArgValue)];
  
-  Rule templateInstantiations_p = 
+  // template<CALIBRATION = {gtsam::Cal3DS2}>
+  string templateArgName;
+  Rule templateArgValues_p =
     (str_p("template") >> 
-    '<' >> name_p[assign_a(templateArgument)] >> '=' >> '{' >> 
+    '<' >> name_p[assign_a(templateArgName)] >> '=' >>
-    !(templateInstantiation_p >> *(',' >> templateInstantiation_p)) >> 
+    '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >>
-    '}' >> '>') 
+    '>');
-    [push_back_a(cls.templateArgs, templateArgument)]; 
  
+  // parse "gtsam::Pose2" and add to singleInstantiation.typeList
+  TemplateInstantiationTypedef singleInstantiation;
   Rule templateSingleInstantiationArg_p = 
-    (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> 
+    (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
-    className_p[push_back_a(templateInstantiationNamespace)]) 
+    className_p[assign_a(templateArgValue.name)])
-    [push_back_a(singleInstantiation.typeList, templateInstantiationNamespace)] 
+    [push_back_a(singleInstantiation.typeList, templateArgValue)]
-    [clear_a(templateInstantiationNamespace)]; 
+    [clear_a(templateArgValue)];
  
+  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+  TemplateInstantiationTypedef singleInstantiation0;
   Rule templateSingleInstantiation_p = 
     (str_p("typedef") >> 
-    *(namespace_name_p[push_back_a(singleInstantiation.classNamespaces)] >> str_p("::")) >> 
+    *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >>
-    className_p[assign_a(singleInstantiation.className)] >> 
+    className_p[assign_a(singleInstantiation.class_.name)] >>
     '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> 
     '>' >> 
     className_p[assign_a(singleInstantiation.name)] >> 
@@ -197,14 +196,16 @@ void Module::parseMarkup(const std::string& data) {
     [push_back_a(templateInstantiationTypedefs, singleInstantiation)] 
     [assign_a(singleInstantiation, singleInstantiation0)]; 
  
+  // template<POSE, POINT>
   Rule templateList_p = 
     (str_p("template") >> 
     '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> 
     '>'); 
 
   // NOTE: allows for pointers to all types
+  ArgumentList args;
   Rule argument_p =  
-    ((basisType_p[assign_a(arg.type)] | argEigenType_p | eigenRef_p | classArg_p) 
+    ((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p)
         >> !ch_p('*')[assign_a(arg.is_ptr,true)]
         >> name_p[assign_a(arg.name)])
     [push_back_a(args, arg)] 
@@ -212,114 +213,111 @@ void Module::parseMarkup(const std::string& data) {
  
   Rule argumentList_p = !argument_p >> * (',' >> argument_p); 
 
+  // parse class constructor
+  Constructor constructor0(verbose), constructor(verbose);
   Rule constructor_p =  
     (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) 
-    [push_back_a(constructor.args_list, args)] 
+    [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))]
-    [assign_a(args,args0)]; 
+    [clear_a(args)];
-    //[assign_a(constructor.args,args)] 
-    //[assign_a(constructor.name,cls.name)] 
-    //[push_back_a(cls.constructors, constructor)] 
-    //[assign_a(constructor,constructor0)]; 
  
+  vector<string> namespaces_return; /// namespace for current return type
   Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); 
  
   // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish
-  static const ReturnValue::return_category RETURN_EIGEN = ReturnValue::EIGEN;
+  static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN;
-  static const ReturnValue::return_category RETURN_BASIS = ReturnValue::BASIS;
+  static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS;
-  static const ReturnValue::return_category RETURN_CLASS = ReturnValue::CLASS;
+  static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS;
-  static const ReturnValue::return_category RETURN_VOID = ReturnValue::VOID;
+  static const ReturnType::return_category RETURN_VOID = ReturnType::VOID;
 
-  Rule returnType1_p =
+  ReturnType retType0, retType;
-    (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_BASIS)]) |
+  Rule returnType_p =
-    ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)]
+    (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) |
-        >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_CLASS)]) >>
+    ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)]
-        !ch_p('*')[assign_a(retVal.isPtr1,true)]) |
+        >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >>
-    (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_EIGEN)]);
+        !ch_p('*')[assign_a(retType.isPtr,true)]) |
+    (eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]);
 
-  Rule returnType2_p = 
+  ReturnValue retVal0, retVal;
-    (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_BASIS)]) |
+  Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)];
-    ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] 
+  Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)];
-        >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_CLASS)]) >>
-        !ch_p('*')  [assign_a(retVal.isPtr2,true)]) | 
-    (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_EIGEN)]);
 
   Rule pair_p =  
     (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') 
     [assign_a(retVal.isPair,true)]; 
  
-  Rule void_p = str_p("void")[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_VOID)];
+  Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)];
  
-  Rule returnType_p = void_p | pair_p | returnType1_p;
+  Rule returnValue_p = void_p | pair_p | returnType1_p;
  
   Rule methodName_p = lexeme_d[(upper_p | lower_p)  >> *(alnum_p | '_')];
  
+  // gtsam::Values retract(const gtsam::VectorValues& delta) const;
+  string methodName;
+  bool isConst, isConst0 = false;
   Rule method_p =  
-    (returnType_p >> methodName_p[assign_a(methodName)] >> 
+    !templateArgValues_p >>
+    (returnValue_p >> methodName_p[assign_a(methodName)] >>
      '(' >> argumentList_p >> ')' >>  
      !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) 
-    [bl::bind(&Method::addOverload, 
+    [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst),
-      bl::var(cls.methods)[bl::var(methodName)], 
+        bl::var(methodName), bl::var(args), bl::var(retVal),
-      verbose, 
+        bl::var(templateArgName), bl::var(templateArgValues))]
-      bl::var(isConst), 
+    [assign_a(retVal,retVal0)]
-      bl::var(methodName), 
+    [clear_a(args)]
-      bl::var(args), 
+    [clear_a(templateArgValues)]
-      bl::var(retVal))] 
+    [assign_a(isConst,isConst0)];
-    [assign_a(isConst,isConst0)] 
-    [assign_a(methodName,methodName0)] 
-    [assign_a(args,args0)] 
-    [assign_a(retVal,retVal0)]; 
  
   Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; 
  
   Rule static_method_p = 
-    (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(methodName)] >> 
+    (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >>
      '(' >> argumentList_p >> ')' >> ';' >> *comments_p) 
     [bl::bind(&StaticMethod::addOverload, 
       bl::var(cls.static_methods)[bl::var(methodName)], 
-      verbose, 
+      bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)]
-      bl::var(methodName), 
+    [assign_a(retVal,retVal0)]
-      bl::var(args), 
+    [clear_a(args)];
-      bl::var(retVal))] 
-    [assign_a(methodName,methodName0)] 
-    [assign_a(args,args0)] 
-    [assign_a(retVal,retVal0)]; 
  
   Rule functions_p = constructor_p | method_p | static_method_p; 
  
+  // parse a full class
+  vector<Qualified> templateInstantiations;
   Rule class_p = 
-      (str_p("")[assign_a(cls,cls0)]) 
+      eps_p[assign_a(cls,cls0)]
-      >> (!(templateInstantiations_p | templateList_p) 
+      >> (!(templateArgValues_p
+          [push_back_a(cls.templateArgs, templateArgName)]
+          [assign_a(templateInstantiations,templateArgValues)]
+          [clear_a(templateArgValues)]
+          | templateList_p)
       >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) 
       >> str_p("class") 
       >> className_p[assign_a(cls.name)] 
       >> ((':' >> classParent_p >> '{') | '{') 
       >> *(functions_p | comments_p) 
       >> str_p("};")) 
-      [assign_a(constructor.name, cls.name)] 
+      [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor),
+          bl::var(cls.name), Qualified(), verbose)]
       [assign_a(cls.constructor, constructor)] 
       [assign_a(cls.namespaces, namespaces)] 
-      [assign_a(deconstructor.name,cls.name)] 
+      [assign_a(cls.deconstructor.name,cls.name)]
-      [assign_a(cls.deconstructor, deconstructor)] 
+      [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls),
-      [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), bl::var(templateArgument), bl::var(templateInstantiations))] 
+          bl::var(templateInstantiations))]
-      [assign_a(deconstructor,deconstructor0)] 
+      [clear_a(templateInstantiations)]
       [assign_a(constructor, constructor0)] 
-      [assign_a(cls,cls0)] 
+      [assign_a(cls,cls0)];
-      [clear_a(templateArgument)] 
-      [clear_a(templateInstantiations)]; 
  
+  // parse a global function
+  Qualified globalFunction;
   Rule global_function_p = 
-      (returnType_p >> staticMethodName_p[assign_a(methodName)] >> 
+      (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >>
        '(' >> argumentList_p >> ')' >> ';' >> *comments_p) 
+      [assign_a(globalFunction.namespaces,namespaces)]
       [bl::bind(&GlobalFunction::addOverload, 
-        bl::var(global_functions)[bl::var(methodName)], 
+        bl::var(global_functions)[bl::var(globalFunction.name)],
-        verbose, 
+        bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)]
-        bl::var(methodName), 
+      [assign_a(retVal,retVal0)]
-        bl::var(args), 
+      [clear_a(globalFunction)]
-        bl::var(retVal), 
+      [clear_a(args)];
-        bl::var(namespaces))] 
-      [assign_a(methodName,methodName0)] 
-      [assign_a(args,args0)] 
-      [assign_a(retVal,retVal0)]; 
  
   Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>');
 
@@ -340,6 +338,8 @@ void Module::parseMarkup(const std::string& data) {
 #pragma clang diagnostic pop
 #endif
 
+  // parse forward declaration
+  ForwardDeclaration fwDec0, fwDec;
   Rule forward_declaration_p =
       !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) 
       >> str_p("class") 
@@ -367,7 +367,7 @@ void Module::parseMarkup(const std::string& data) {
   BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
   BOOST_SPIRIT_DEBUG_NODE(pair_p);
   BOOST_SPIRIT_DEBUG_NODE(void_p);
-  BOOST_SPIRIT_DEBUG_NODE(returnType_p);
+  BOOST_SPIRIT_DEBUG_NODE(returnValue_p);
   BOOST_SPIRIT_DEBUG_NODE(methodName_p);
   BOOST_SPIRIT_DEBUG_NODE(method_p);
   BOOST_SPIRIT_DEBUG_NODE(class_p);
@@ -388,125 +388,41 @@ void Module::parseMarkup(const std::string& data) {
   }
 
   // Post-process classes for serialization markers
-  BOOST_FOREACH(Class& cls, classes) {
+  BOOST_FOREACH(Class& cls, classes)
-    Class::Methods::iterator serializable_it = cls.methods.find("serializable");
+    cls.erase_serialization();
-    if (serializable_it != cls.methods.end()) {
-#ifndef WRAP_DISABLE_SERIALIZE
-      cls.isSerializable = true;
-#else
-      cout << "Ignoring serializable() flag in class " << cls.name << endl;
-#endif
-      cls.methods.erase(serializable_it);
-    }
-
-    Class::Methods::iterator serialize_it = cls.methods.find("serialize");
-    if (serialize_it != cls.methods.end()) {
-#ifndef WRAP_DISABLE_SERIALIZE
-      cls.isSerializable = true;
-      cls.hasSerialization= true;
-#else
-      cout << "Ignoring serialize() flag in class " << cls.name << endl;
-#endif
-      cls.methods.erase(serialize_it);
-    }
-  }
 
   // Explicitly add methods to the classes from parents so it shows in documentation
   BOOST_FOREACH(Class& cls, classes)
-  {
+    cls.appendInheritedMethods(cls, classes);
-    map<string, Method> inhereted = appendInheretedMethods(cls, classes);
-    cls.methods.insert(inhereted.begin(), inhereted.end());
-  }
-} 
- 
-/* ************************************************************************* */ 
-template<class T> 
-void verifyArguments(const vector<string>& validArgs, const map<string,T>& vt) { 
-  typedef typename map<string,T>::value_type Name_Method; 
-  BOOST_FOREACH(const Name_Method& name_method, vt) { 
-    const T& t = name_method.second; 
-    BOOST_FOREACH(const ArgumentList& argList, t.argLists) { 
-      BOOST_FOREACH(Argument arg, argList) { 
-        string fullType = arg.qualifiedType("::"); 
-        if(find(validArgs.begin(), validArgs.end(), fullType) 
-          == validArgs.end()) 
-          throw DependencyMissing(fullType, t.name); 
-      } 
-    } 
-  } 
-} 
- 
-/* ************************************************************************* */ 
-template<class T> 
-void verifyReturnTypes(const vector<string>& validtypes, const map<string,T>& vt) { 
-  typedef typename map<string,T>::value_type Name_Method; 
-  BOOST_FOREACH(const Name_Method& name_method, vt) { 
-    const T& t = name_method.second; 
-    BOOST_FOREACH(const ReturnValue& retval, t.returnVals) { 
-      if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end()) 
-        throw DependencyMissing(retval.qualifiedType1("::"), t.name); 
-      if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end()) 
-        throw DependencyMissing(retval.qualifiedType2("::"), t.name); 
-    } 
-  } 
-} 
- 
-/* ************************************************************************* */ 
-void Module::generateIncludes(FileWriter& file) const { 
- 
-  // collect includes 
-  vector<string> all_includes(includes); 
- 
-  // sort and remove duplicates 
-  sort(all_includes.begin(), all_includes.end()); 
-  vector<string>::const_iterator last_include = unique(all_includes.begin(), all_includes.end()); 
-  vector<string>::const_iterator it = all_includes.begin(); 
-  // add includes to file 
-  for (; it != last_include; ++it) 
-    file.oss << "#include <" << *it << ">" << endl; 
-  file.oss << "\n"; 
-} 
-
- 
-/* ************************************************************************* */ 
-void Module::matlab_code(const string& toolboxPath, const string& headerPath) const { 
-
-  fs::create_directories(toolboxPath);
 
   // Expand templates - This is done first so that template instantiations are
   // counted in the list of valid types, have their attributes and dependencies
   // checked, etc.
-  vector<Class> expandedClasses = ExpandTypedefInstantiations(classes, templateInstantiationTypedefs);
+  expandedClasses = ExpandTypedefInstantiations(classes,
+      templateInstantiationTypedefs);
 
   // Dependency check list
-  vector<string> validTypes = GenerateValidTypes(expandedClasses, forward_declarations);
+  vector<string> validTypes = GenerateValidTypes(expandedClasses,
+      forward_declarations);
 
   // Check that all classes have been defined somewhere
   verifyArguments<GlobalFunction>(validTypes, global_functions);
   verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
 
-  bool hasSerialiable = false;
+  hasSerialiable = false;
-  BOOST_FOREACH(const Class& cls, expandedClasses) {
+  BOOST_FOREACH(const Class& cls, expandedClasses)
-    hasSerialiable |= cls.isSerializable;
+    cls.verifyAll(validTypes,hasSerialiable);
-    // verify all of the function arguments
-    //TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
-    verifyArguments<StaticMethod>(validTypes, cls.static_methods);
-    verifyArguments<Method>(validTypes, cls.methods);
-
-    // verify function return types
-    verifyReturnTypes<StaticMethod>(validTypes, cls.static_methods);
-    verifyReturnTypes<Method>(validTypes, cls.methods);
-
-    // verify parents
-    if(!cls.qualifiedParent.empty() && std::find(validTypes.begin(), validTypes.end(), wrap::qualifiedName("::", cls.qualifiedParent)) == validTypes.end())
-      throw DependencyMissing(wrap::qualifiedName("::", cls.qualifiedParent), cls.qualifiedName("::"));
-  }
 
   // Create type attributes table and check validity
-  TypeAttributesTable typeAttributes;
   typeAttributes.addClasses(expandedClasses);
   typeAttributes.addForwardDeclarations(forward_declarations);
   typeAttributes.checkValidity(expandedClasses);
+} 
+ 
+/* ************************************************************************* */ 
+void Module::matlab_code(const string& toolboxPath) const {
+
+  fs::create_directories(toolboxPath);
 
   // create the unified .cpp switch file
   const string wrapperName = name + "_wrapper";
@@ -527,19 +443,18 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
   // Generate includes while avoiding redundant includes
   generateIncludes(wrapperFile);
 
-  // create typedef classes - we put this at the top of the wrap file so that collectors and method arguments can use these typedefs
+  // create typedef classes - we put this at the top of the wrap file so that
-  BOOST_FOREACH(const Class& cls, expandedClasses) {
+  // collectors and method arguments can use these typedefs
+  BOOST_FOREACH(const Class& cls, expandedClasses)
     if(!cls.typedefName.empty())
       wrapperFile.oss << cls.getTypedef() << "\n";
-  }
   wrapperFile.oss << "\n";
 
   // Generate boost.serialization export flags (needs typedefs from above)
   if (hasSerialiable) {
-    BOOST_FOREACH(const Class& cls, expandedClasses) {
+    BOOST_FOREACH(const Class& cls, expandedClasses)
       if(cls.isSerializable)
         wrapperFile.oss << cls.getSerializationExport() << "\n";
-    }
     wrapperFile.oss << "\n";
   }
 
@@ -552,14 +467,12 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
   vector<string> functionNames; // Function names stored by index for switch
 
   // create proxy class and wrapper code
-  BOOST_FOREACH(const Class& cls, expandedClasses) {
+  BOOST_FOREACH(const Class& cls, expandedClasses)
     cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
-  }
 
   // create matlab files and wrapper code for global functions
-  BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions) {
+  BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
     p.second.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
-  } 
 
   // finish wrapper file
   wrapperFile.oss << "\n";
@@ -567,27 +480,23 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 
   wrapperFile.emit(true);
 }
+
 /* ************************************************************************* */ 
-map<string, Method> Module::appendInheretedMethods(const Class& cls, const vector<Class>& classes)
+void Module::generateIncludes(FileWriter& file) const {
-{
+
-  map<string, Method> methods;
+  // collect includes
-  if(!cls.qualifiedParent.empty())
+  vector<string> all_includes(includes);
-  {
+
-    //Find Class
+  // sort and remove duplicates
-    BOOST_FOREACH(const Class& parent, classes) {
+  sort(all_includes.begin(), all_includes.end());
-      //We found the class for our parent
+  vector<string>::const_iterator last_include = unique(all_includes.begin(), all_includes.end());
-      if(parent.name == cls.qualifiedParent.back())
+  vector<string>::const_iterator it = all_includes.begin();
-      {
+  // add includes to file
-        Methods inhereted = appendInheretedMethods(parent, classes);
+  for (; it != last_include; ++it)
-        methods.insert(inhereted.begin(), inhereted.end());
+    file.oss << "#include <" << *it << ">" << endl;
-      }
+  file.oss << "\n";
-    }
-  } else {
-    methods.insert(cls.methods.begin(), cls.methods.end());
 }
 
-  return methods;
-}
 
 /* ************************************************************************* */
   void Module::finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const { 
@@ -741,3 +650,31 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul
 } 
  
 /* ************************************************************************* */ 
+void Module::python_wrapper(const string& toolboxPath) const {
+
+  fs::create_directories(toolboxPath);
+
+  // create the unified .cpp switch file
+  const string wrapperName = name + "_python";
+  string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
+  FileWriter wrapperFile(wrapperFileName, verbose, "//");
+  wrapperFile.oss << "#include <boost/python.hpp>\n\n";
+  wrapperFile.oss << "using namespace boost::python;\n";
+  wrapperFile.oss << "BOOST_PYTHON_MODULE(" + name + ")\n";
+  wrapperFile.oss << "{\n";
+
+  // write out classes
+  BOOST_FOREACH(const Class& cls, expandedClasses)
+    cls.python_wrapper(wrapperFile);
+
+  // write out global functions
+  BOOST_FOREACH(const GlobalFunctions::value_type& p, global_functions)
+    p.second.python_wrapper(wrapperFile);
+
+  // finish wrapper file
+  wrapperFile.oss << "}\n";
+
+  wrapperFile.emit(true);
+}
+
+/* ************************************************************************* */

wrap/Module.h

@@ -37,6 +37,7 @@ struct Module {
   typedef std::map<std::string, GlobalFunction> GlobalFunctions;
   typedef std::map<std::string, Method> Methods;
 
+  // Filled during parsing:
   std::string name; ///< module name
   bool verbose; ///< verbose flag
   std::vector<Class> classes; ///< list of classes
@@ -45,35 +46,44 @@ struct Module {
   std::vector<std::string> includes; ///< Include statements
   GlobalFunctions global_functions;
 
+  // After parsing:
+  std::vector<Class> expandedClasses;
+  bool hasSerialiable;
+  TypeAttributesTable typeAttributes;
+
   /// constructor that parses interface file
-  Module(const std::string& interfacePath,
+  Module(const std::string& interfacePath, const std::string& moduleName,
-   const std::string& moduleName,
       bool enable_verbose = true);
 
   /// Dummy constructor that does no parsing - use only for testing
   Module(const std::string& moduleName, bool enable_verbose = true);
 
-  //Recursive method to append all methods inhereted from parent classes
-  std::map<std::string, Method> appendInheretedMethods(const Class& cls, const std::vector<Class>& classes);
-
-  /// MATLAB code generation:
-  void matlab_code(
-       const std::string& path,
-       const std::string& headerPath) const; // FIXME: headerPath not actually used?
-
-  void finish_wrapper(FileWriter& file, const std::vector<std::string>& functionNames) const;
-
-  void generateIncludes(FileWriter& file) const;
-
   /// non-const function that performs parsing - typically called by constructor
   /// Throws exception on failure
   void parseMarkup(const std::string& data);
 
+  /// MATLAB code generation:
+  void matlab_code(const std::string& path) const;
+
+  void generateIncludes(FileWriter& file) const;
+
+  void finish_wrapper(FileWriter& file,
+      const std::vector<std::string>& functionNames) const;
+
+  /// Python code generation:
+  void python_wrapper(const std::string& path) const;
+
 private:
-  static std::vector<Class> ExpandTypedefInstantiations(const std::vector<Class>& classes, const std::vector<TemplateInstantiationTypedef> instantiations);
+  static std::vector<Class> ExpandTypedefInstantiations(
-  static std::vector<std::string> GenerateValidTypes(const std::vector<Class>& classes, const std::vector<ForwardDeclaration> forwardDeclarations);
+      const std::vector<Class>& classes,
-  static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes);
+      const std::vector<TemplateInstantiationTypedef> instantiations);
-  static void WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes);
+  static std::vector<std::string> GenerateValidTypes(
+      const std::vector<Class>& classes,
+      const std::vector<ForwardDeclaration> forwardDeclarations);
+  static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile,
+      const std::string& moduleName, const std::vector<Class>& classes);
+  static void WriteRTTIRegistry(FileWriter& wrapperFile,
+      const std::string& moduleName, const std::vector<Class>& classes);
 };
 
 } // \namespace wrap

wrap/OverloadedFunction.h

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file OverloadedFunction.h
+ * @brief Function that can overload its arguments only
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#pragma once
+
+#include "Function.h"
+#include "Argument.h"
+
+namespace wrap {
+
+/**
+ * ArgumentList Overloads
+ */
+class ArgumentOverloads {
+
+protected:
+
+  std::vector<ArgumentList> argLists_;
+
+public:
+
+  size_t nrOverloads() const {
+    return argLists_.size();
+  }
+
+  const ArgumentList& argumentList(size_t i) const {
+    return argLists_.at(i);
+  }
+
+  void push_back(const ArgumentList& args) {
+    argLists_.push_back(args);
+  }
+
+  std::vector<ArgumentList> expandArgumentListsTemplate(
+      const TemplateSubstitution& ts) const {
+    std::vector<ArgumentList> result;
+    BOOST_FOREACH(const ArgumentList& argList, argLists_) {
+      ArgumentList instArgList = argList.expandTemplate(ts);
+      result.push_back(instArgList);
+    }
+    return result;
+  }
+
+  /// Expand templates, imperative !
+  virtual void ExpandTemplate(const TemplateSubstitution& ts) {
+    argLists_ = expandArgumentListsTemplate(ts);
+  }
+
+  void verifyArguments(const std::vector<std::string>& validArgs,
+      const std::string s) const {
+    BOOST_FOREACH(const ArgumentList& argList, argLists_) {
+      BOOST_FOREACH(Argument arg, argList) {
+        std::string fullType = arg.type.qualifiedName("::");
+        if (find(validArgs.begin(), validArgs.end(), fullType)
+            == validArgs.end())
+          throw DependencyMissing(fullType, "checking argument of " + s);
+      }
+    }
+  }
+
+  friend std::ostream& operator<<(std::ostream& os,
+      const ArgumentOverloads& overloads) {
+    BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_)
+      os << argList << std::endl;
+    return os;
+  }
+
+};
+
+class OverloadedFunction: public Function, public ArgumentOverloads {
+
+public:
+
+  bool addOverload(const std::string& name, const ArgumentList& args,
+      const Qualified& instName = Qualified(), bool verbose = false) {
+    bool first = initializeOrCheck(name, instName, verbose);
+    ArgumentOverloads::push_back(args);
+    return first;
+  }
+
+private:
+
+};
+
+// Templated checking functions
+// TODO: do this via polymorphism, use transform ?
+
+template<class F>
+static std::map<std::string, F> expandMethodTemplate(
+    const std::map<std::string, F>& methods, const TemplateSubstitution& ts) {
+  std::map<std::string, F> result;
+  typedef std::pair<const std::string, F> NamedMethod;
+  BOOST_FOREACH(NamedMethod namedMethod, methods) {
+    F instMethod = namedMethod.second;
+    instMethod.expandTemplate(ts);
+    namedMethod.second = instMethod;
+    result.insert(namedMethod);
+  }
+  return result;
+}
+
+template<class F>
+inline void verifyArguments(const std::vector<std::string>& validArgs,
+    const std::map<std::string, F>& vt) {
+  typedef typename std::map<std::string, F>::value_type NamedMethod;
+  BOOST_FOREACH(const NamedMethod& namedMethod, vt)
+    namedMethod.second.verifyArguments(validArgs);
+}
+
+} // \namespace wrap
+

wrap/Qualified.h

@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Qualified.h
+ * @brief Qualified name
+ * @author Frank Dellaert
+ * @date Nov 11, 2014
+ **/
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace wrap {
+
+/**
+ * Class to encapuslate a qualified name, i.e., with (nested) namespaces
+ */
+struct Qualified {
+
+  std::vector<std::string> namespaces; ///< Stack of namespaces
+  std::string name; ///< type name
+
+  Qualified(const std::string& name_ = "") :
+      name(name_) {
+  }
+
+  bool empty() const {
+    return namespaces.empty() && name.empty();
+  }
+
+  void clear() {
+    namespaces.clear();
+    name.clear();
+  }
+
+  bool operator!=(const Qualified& other) const {
+    return other.name != name || other.namespaces != namespaces;
+  }
+
+  /// Return a qualified string using given delimiter
+  std::string qualifiedName(const std::string& delimiter = "") const {
+    std::string result;
+    for (std::size_t i = 0; i < namespaces.size(); ++i)
+      result += (namespaces[i] + delimiter);
+    result += name;
+    return result;
+  }
+
+  /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m"
+  std::string matlabName(const std::string& toolboxPath) const {
+    std::string result = toolboxPath;
+    for (std::size_t i = 0; i < namespaces.size(); ++i)
+      result += ("/+" + namespaces[i]);
+    result += "/" + name + ".m";
+    return result;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const Qualified& q) {
+    os << q.qualifiedName("::");
+    return os;
+  }
+
+};
+
+} // \namespace wrap
+

wrap/ReturnType.cpp

@@ -0,0 +1,61 @@
+/**
+ * @file ReturnType.cpp
+ * @date Nov 13, 2014
+ * @author Frank Dellaert
+ */
+
+#include "ReturnType.h"
+#include "utilities.h"
+#include <boost/foreach.hpp>
+#include <iostream>
+
+using namespace std;
+using namespace wrap;
+
+/* ************************************************************************* */
+string ReturnType::str(bool add_ptr) const {
+  return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name);
+}
+
+/* ************************************************************************* */
+void ReturnType::wrap_result(const string& out, const string& result,
+    FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const {
+
+  string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
+
+  if (category == CLASS) {
+    string objCopy, ptrType;
+    ptrType = "Shared" + name;
+    const bool isVirtual = typeAttributes.at(cppType).isVirtual;
+    if (isVirtual) {
+      if (isPtr)
+        objCopy = result;
+      else
+        objCopy = result + ".clone()";
+    } else {
+      if (isPtr)
+        objCopy = result;
+      else
+        objCopy = ptrType + "(new " + cppType + "(" + result + "))";
+    }
+    wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\""
+        << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n";
+  } else if (isPtr) {
+    wrapperFile.oss << "  Shared" << name << "* ret = new Shared" << name << "("
+        << result << ");" << endl;
+    wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType
+        << "\");\n";
+  } else if (matlabType != "void")
+    wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result
+        << ");\n";
+}
+
+/* ************************************************************************* */
+void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
+  if (category == CLASS)
+    wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
+        << "> Shared" << name << ";" << endl;
+}
+
+/* ************************************************************************* */
+

wrap/ReturnType.h

@@ -0,0 +1,67 @@
+/**
+ * @file ReturnValue.h
+ * @brief Encapsulates a return type of a method
+ * @date Nov 13, 2014
+ * @author Frank Dellaert
+ */
+
+#include "Qualified.h"
+#include "FileWriter.h"
+#include "TypeAttributesTable.h"
+#include "utilities.h"
+
+#pragma once
+
+namespace wrap {
+
+/**
+ * Encapsulates return value of a method or function
+ */
+struct ReturnType: Qualified {
+
+  /// the different supported return value categories
+  typedef enum {
+    CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
+  } return_category;
+
+  bool isPtr;
+  return_category category;
+
+  ReturnType() :
+      isPtr(false), category(CLASS) {
+  }
+
+  ReturnType(const std::string& name) :
+      isPtr(false), category(CLASS) {
+    Qualified::name = name;
+  }
+
+  void rename(const Qualified& q) {
+    name = q.name;
+    namespaces = q.namespaces;
+  }
+
+  /// Check if this type is in a set of valid types
+  template<class TYPES>
+  void verify(TYPES validtypes, const std::string& s) const {
+    std::string key = qualifiedName("::");
+    if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end())
+      throw DependencyMissing(key, "checking return type of " + s);
+  }
+
+private:
+
+  friend struct ReturnValue;
+
+  std::string str(bool add_ptr) const;
+
+  /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
+  void wrap_result(const std::string& out, const std::string& result,
+      FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const;
+
+  /// Creates typedef
+  void wrapTypeUnwrap(FileWriter& wrapperFile) const;
+
+};
+
+} // \namespace wrap

wrap/ReturnValue.cpp

@@ -1,29 +1,34 @@
 /**
  * @file ReturnValue.cpp
- *
  * @date Dec 1, 2011
  * @author Alex Cunningham
  * @author Andrew Melim
  * @author Richard Roberts
  */
 
-#include <boost/foreach.hpp>
-
 #include "ReturnValue.h"
 #include "utilities.h"
+#include <boost/foreach.hpp>
+#include <iostream>
 
 using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-string ReturnValue::return_type(bool add_ptr, pairing p) const {
+ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
-  if (p==pair && isPair) {
+  ReturnValue instRetVal = *this;
-    string str = "pair< " +
+  instRetVal.type1 = ts(type1);
-        maybe_shared_ptr(add_ptr || isPtr1, qualifiedType1("::"), type1) + ", " +
+  if (isPair)
-      maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >";
+    instRetVal.type2 = ts(type2);
-    return str;
+  return instRetVal;
-  } else
+}
-    return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1);
+
+/* ************************************************************************* */
+string ReturnValue::return_type(bool add_ptr) const {
+  if (isPair)
+    return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >";
+  else
+    return type1.str(add_ptr);
 }
 
 /* ************************************************************************* */
@@ -32,122 +37,36 @@ string ReturnValue::matlab_returnType() const {
 }
 
 /* ************************************************************************* */
-string ReturnValue::qualifiedType1(const string& delim) const {
+void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
-  string result;
+    const TypeAttributesTable& typeAttributes) const {
-  BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim;
-  return result + type1;
-}
-
-/* ************************************************************************* */
-string ReturnValue::qualifiedType2(const string& delim) const {
-  string result;
-  BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim;
-  return result + type2;
-}
-
-/* ************************************************************************* */
-//TODO:Fix this
-void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const {
-  string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(".");
-  string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(".");
-
   if (isPair) {
     // For a pair, store the returned pair so we do not evaluate the function twice
-    file.oss << "  " << return_type(false, pair) << " pairResult = " << result << ";\n";
+    wrapperFile.oss << "  " << return_type(true) << " pairResult = " << result
-    
+        << ";\n";
-    // first return value in pair
+    type1.wrap_result("  out[0]", "pairResult.first", wrapperFile,
-    if (category1 == ReturnValue::CLASS) { // if we are going to make one
+        typeAttributes);
-      string objCopy, ptrType;
+    type2.wrap_result("  out[1]", "pairResult.second", wrapperFile,
-      ptrType = "Shared" + type1;
+        typeAttributes);
-      const bool isVirtual = typeAttributes.at(cppType1).isVirtual;
-      if(isVirtual) {
-        if(isPtr1)
-          objCopy = "pairResult.first";
-        else
-          objCopy = "pairResult.first.clone()";
-      } else {
-        if(isPtr1)
-          objCopy = "pairResult.first";
-        else
-          objCopy = ptrType + "(new " + cppType1 + "(pairResult.first))";
-      }
-      file.oss << "  out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n";
-    } else if(isPtr1) {
-      file.oss << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(pairResult.first);" << endl;
-      file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\", false);\n";
-    } else // if basis type
-      file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(pairResult.first);\n";
-
-    // second return value in pair
-    if (category2 == ReturnValue::CLASS) { // if we are going to make one
-      string objCopy, ptrType;
-      ptrType = "Shared" + type2;
-      const bool isVirtual = typeAttributes.at(cppType2).isVirtual;
-      if(isVirtual) {
-        if(isPtr2)
-          objCopy = "pairResult.second";
-        else
-          objCopy = "pairResult.second.clone()";
-      } else {
-        if(isPtr2)
-          objCopy = "pairResult.second";
-        else
-          objCopy = ptrType + "(new " + cppType2 + "(pairResult.second))";
-      }
-      file.oss << "  out[1] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n";
-    } else if(isPtr2) {
-      file.oss << "  Shared" << type2 <<"* ret = new Shared" << type2 << "(pairResult.second);" << endl;
-      file.oss << "  out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n";
-    } else
-      file.oss << "  out[1] = wrap< " << return_type(true,arg2) << " >(pairResult.second);\n";
   } else { // Not a pair
-
+    type1.wrap_result("  out[0]", result, wrapperFile, typeAttributes);
-    if (category1 == ReturnValue::CLASS) {
-      string objCopy, ptrType;
-      ptrType = "Shared" + type1;
-      const bool isVirtual = typeAttributes.at(cppType1).isVirtual;
-      if(isVirtual) {
-        if(isPtr1)
-          objCopy = result;
-        else
-          objCopy = result + ".clone()";
-      } else {
-        if(isPtr1)
-          objCopy = result;
-        else
-          objCopy = ptrType + "(new " + cppType1 + "(" + result + "))";
-      }
-      file.oss << "  out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n";
-    } else if(isPtr1) {
-      file.oss << "  Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ");" << endl;
-      file.oss << "  out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n";
-    } else if (matlabType1!="void")
-      file.oss << "  out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ");\n";
   }
 }
 
 /* ************************************************************************* */
 void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const {
+  type1.wrapTypeUnwrap(wrapperFile);
   if (isPair)
-  {
+    type2.wrapTypeUnwrap(wrapperFile);
-    if(category1 == ReturnValue::CLASS)
-      wrapperFile.oss << "  typedef boost::shared_ptr<"  << qualifiedType1("::")  << "> Shared" <<  type1 << ";"<< endl;
-    if(category2 == ReturnValue::CLASS)
-      wrapperFile.oss << "  typedef boost::shared_ptr<"  << qualifiedType2("::")  << "> Shared" <<  type2 << ";"<< endl;
-  }
-  else {
-    if (category1 == ReturnValue::CLASS)
-      wrapperFile.oss << "  typedef boost::shared_ptr<"  << qualifiedType1("::")  << "> Shared" <<  type1 << ";"<< endl;
-  }
 }
+
 /* ************************************************************************* */
-void ReturnValue::emit_matlab(FileWriter& file) const {
+void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
   string output;
   if (isPair)
-    file.oss << "[ varargout{1} varargout{2} ] = ";
+    proxyFile.oss << "[ varargout{1} varargout{2} ] = ";
-  else if (category1 != ReturnValue::VOID)
+  else if (type1.category != ReturnType::VOID)
-    file.oss << "varargout{1} = ";
+    proxyFile.oss << "varargout{1} = ";
 }
+
 /* ************************************************************************* */
 
-

wrap/ReturnValue.h

@@ -2,59 +2,61 @@
  * @file ReturnValue.h
  *
  * @brief Encapsulates a return value from a method
- *
  * @date Dec 1, 2011
  * @author Alex Cunningham
  * @author Richard Roberts
  */
 
+#include "ReturnType.h"
+#include "TemplateSubstitution.h"
 #include "FileWriter.h"
 #include "TypeAttributesTable.h"
-
+#include "utilities.h"
-#include <vector>
 
 #pragma once
 
 namespace wrap {
 
 /**
- * Encapsulates return value of a method or function
+ * Encapsulates return type of a method or function, possibly a pair
  */
 struct ReturnValue {
 
-  /// the different supported return value categories
+  bool isPair;
-  typedef enum {
+  ReturnType type1, type2;
-    CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
-  } return_category;
-
-  bool isPtr1, isPtr2, isPair;
-  return_category category1, category2;
-  std::string type1, type2;
-  std::vector<std::string> namespaces1, namespaces2;
 
   /// Constructor
   ReturnValue() :
-      isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2(
+      isPair(false) {
-          CLASS) {
   }
 
-  typedef enum {
+  /// Constructor
-    arg1, arg2, pair
+  ReturnValue(const ReturnType& type) :
-  } pairing;
+      isPair(false), type1(type) {
+  }
 
-  std::string return_type(bool add_ptr, pairing p) const;
+  /// Substitute template argument
+  ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
 
-  std::string qualifiedType1(const std::string& delim = "") const;
+  std::string return_type(bool add_ptr) const;
-  std::string qualifiedType2(const std::string& delim = "") const;
 
   std::string matlab_returnType() const;
 
-  void wrap_result(const std::string& result, FileWriter& file,
+  void wrap_result(const std::string& result, FileWriter& wrapperFile,
       const TypeAttributesTable& typeAttributes) const;
 
   void wrapTypeUnwrap(FileWriter& wrapperFile) const;
 
-  void emit_matlab(FileWriter& file) const;
+  void emit_matlab(FileWriter& proxyFile) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) {
+    if (!r.isPair && r.type1.category == ReturnType::VOID)
+      os << "void";
+    else
+      os << r.return_type(true);
+    return os;
+  }
+
 };
 
 } // \namespace wrap

wrap/StaticMethod.cpp

@@ -16,7 +16,7 @@
  * @author Richard Roberts
  **/
 
-#include "StaticMethod.h"
+#include "Method.h"
 #include "utilities.h"
 
 #include <boost/foreach.hpp>
@@ -30,117 +30,145 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void StaticMethod::addOverload(bool verbose, const std::string& name,
+void StaticMethod::proxy_header(FileWriter& proxyFile) const {
-    const ArgumentList& args, const ReturnValue& retVal) {
+  string upperName = matlabName();
-  this->verbose = verbose;
+  upperName[0] = toupper(upperName[0], locale());
-  this->name = name;
+  proxyFile.oss << "    function varargout = " << upperName << "(varargin)\n";
-  this->argLists.push_back(args);
-  this->returnVals.push_back(retVal);
 }
 
 /* ************************************************************************* */
-void StaticMethod::proxy_wrapper_fragments(FileWriter& file,
+void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile,
-    FileWriter& wrapperFile, const string& cppClassName,
+    FileWriter& wrapperFile, Str cppClassName, Str matlabQualName,
-    const std::string& matlabQualName, const std::string& matlabUniqueName,
+    Str matlabUniqueName, Str wrapperName,
-    const string& wrapperName, const TypeAttributesTable& typeAttributes,
+    const TypeAttributesTable& typeAttributes,
     vector<string>& functionNames) const {
 
-  string upperName = name;
+  // emit header, e.g., function varargout = templatedMethod(this, varargin)
-  upperName[0] = std::toupper(upperName[0], std::locale());
+  proxy_header(proxyFile);
 
-  file.oss << "    function varargout = " << upperName << "(varargin)\n";
+  // Emit comments for documentation
-  //Comments for documentation
+  string up_name = boost::to_upper_copy(matlabName());
-  string up_name = boost::to_upper_copy(name);
+  proxyFile.oss << "      % " << up_name << " usage: ";
-  file.oss << "      % " << up_name << " usage: ";
+  usage_fragment(proxyFile, matlabName());
-  unsigned int argLCount = 0;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    argList.emit_prototype(file, name);
-    if (argLCount != argLists.size() - 1)
-      file.oss << ", ";
-    else
-      file.oss << " : returns "
-          << returnVals[0].return_type(false, returnVals[0].pair) << endl;
-    argLCount++;
-  }
 
-  file.oss << "      % "
+  // Emit URL to Doxygen page
+  proxyFile.oss << "      % "
       << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
       << endl;
-  file.oss << "      % " << "" << endl;
-  file.oss << "      % " << "Usage" << endl;
-  BOOST_FOREACH(ArgumentList argList, argLists) {
-    file.oss << "      % ";
-    argList.emit_prototype(file, up_name);
-    file.oss << endl;
-  }
-
-  // Check arguments for all overloads
-  for (size_t overload = 0; overload < argLists.size(); ++overload) {
 
+  // Handle special case of single overload with all numeric arguments
+  if (nrOverloads() == 1 && argumentList(0).allScalar()) {
     // Output proxy matlab code
-    file.oss << "      " << (overload == 0 ? "" : "else");
+    // TODO: document why is it OK to not check arguments in this case
+    proxyFile.oss << "      ";
     const int id = (int) functionNames.size();
-    argLists[overload].emit_conditional_call(file, returnVals[overload],
+    argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id,
-        wrapperName, id, true); // last bool is to indicate static !
+        isStatic());
 
     // Output C++ wrapper code
     const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
-        matlabUniqueName, (int) overload, id, typeAttributes);
+        matlabUniqueName, 0, id, typeAttributes, templateArgValue_);
+
+    // Add to function list
+    functionNames.push_back(wrapFunctionName);
+  } else {
+    // Check arguments for all overloads
+    for (size_t i = 0; i < nrOverloads(); ++i) {
+
+      // Output proxy matlab code
+      proxyFile.oss << "      " << (i == 0 ? "" : "else");
+      const int id = (int) functionNames.size();
+      argumentList(i).emit_conditional_call(proxyFile, returnValue(i),
+          wrapperName, id, isStatic());
+
+      // Output C++ wrapper code
+      const string wrapFunctionName = wrapper_fragment(wrapperFile,
+          cppClassName, matlabUniqueName, i, id, typeAttributes,
+          templateArgValue_);
 
       // Add to function list
       functionNames.push_back(wrapFunctionName);
     }
-  file.oss << "      else\n";
+    proxyFile.oss << "      else\n";
-  file.oss << "        error('Arguments do not match any overload of function "
+    proxyFile.oss
-      << matlabQualName << "." << upperName << "');" << endl;
+        << "        error('Arguments do not match any overload of function "
-  file.oss << "      end\n";
+        << matlabQualName << "." << name_ << "');" << endl;
+    proxyFile.oss << "      end\n";
+  }
 
-  file.oss << "    end\n";
+  proxyFile.oss << "    end\n";
 }
 
 /* ************************************************************************* */
-string StaticMethod::wrapper_fragment(FileWriter& file,
+string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
-    const string& cppClassName, const string& matlabUniqueName, int overload,
+    Str matlabUniqueName, int overload, int id,
-    int id, const TypeAttributesTable& typeAttributes) const {
+    const TypeAttributesTable& typeAttributes,
+    const Qualified& instName) const {
 
   // generate code
 
-  const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
+  const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
       + boost::lexical_cast<string>(id);
 
-  const ArgumentList& args = argLists[overload];
+  const ArgumentList& args = argumentList(overload);
-  const ReturnValue& returnVal = returnVals[overload];
+  const ReturnValue& returnVal = returnValue(overload);
 
   // call
-  file.oss << "void " << wrapFunctionName
+  wrapperFile.oss << "void " << wrapFunctionName
       << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
   // start
-  file.oss << "{\n";
+  wrapperFile.oss << "{\n";
 
-  returnVal.wrapTypeUnwrap(file);
+  returnVal.wrapTypeUnwrap(wrapperFile);
 
-  file.oss << "  typedef boost::shared_ptr<" << cppClassName << "> Shared;"
+  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
-      << endl;
+      << "> Shared;" << endl;
 
-  // check arguments
+  // get call
-  // NOTE: for static functions, there is no object passed
+  // for static methods: cppClassName::staticMethod<TemplateVal>
-  file.oss << "  checkArguments(\"" << matlabUniqueName << "." << name
+  // for instance methods: obj->instanceMethod<TemplateVal>
-      << "\",nargout,nargin," << args.size() << ");\n";
+  string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
+      args, returnVal, typeAttributes, instName);
 
-  // unwrap arguments, see Argument.cpp
+  expanded += ("(" + args.names() + ")");
-  args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
+  if (returnVal.type1.name != "void")
-
+    returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
-  // call method with default type and wrap result
-  if (returnVal.type1 != "void")
-    returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")",
-        file, typeAttributes);
   else
-    file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n";
+    wrapperFile.oss << "  " + expanded + ";\n";
 
   // finish
-  file.oss << "}\n";
+  wrapperFile.oss << "}\n";
 
   return wrapFunctionName;
 }
 
 /* ************************************************************************* */
+string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
+    Str matlabUniqueName, const ArgumentList& args,
+    const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
+    const Qualified& instName) const {
+  // check arguments
+  // NOTE: for static functions, there is no object passed
+  wrapperFile.oss << "  checkArguments(\"" << matlabUniqueName << "." << name_
+      << "\",nargout,nargin," << args.size() << ");\n";
+
+  // unwrap arguments, see Argument.cpp
+  args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object
+
+  // call method and wrap result
+  // example: out[0]=wrap<bool>(staticMethod(t));
+  string expanded = cppClassName + "::" + name_;
+  if (!instName.empty())
+    expanded += ("<" + instName.qualifiedName("::") + ">");
+
+  return expanded;
+}
+
+/* ************************************************************************* */
+void StaticMethod::python_wrapper(FileWriter& wrapperFile,
+    Str className) const {
+  wrapperFile.oss << "  .def(\"" << name_ << "\", &" << className << "::" << name_
+      << ");\n";
+}
+
+/* ************************************************************************* */

wrap/StaticMethod.h

@@ -19,44 +19,61 @@
 
 #pragma once
 
-#include "Argument.h"
+#include "FullyOverloadedFunction.h"
-#include "ReturnValue.h"
-#include "TypeAttributesTable.h"
 
 namespace wrap {
 
 /// StaticMethod class
-struct StaticMethod {
+struct StaticMethod: public FullyOverloadedFunction {
 
-  /// Constructor creates empty object
+  typedef const std::string& Str;
-  StaticMethod(bool verbosity = true) :
+
-      verbose(verbosity) {
+  virtual bool isStatic() const {
+    return true;
   }
 
-  // Then the instance variables are set directly by the Module constructor
+  // emit a list of comments, one for each overload
-  bool verbose;
+  void comment_fragment(FileWriter& proxyFile) const {
-  std::string name;
+    SignatureOverloads::comment_fragment(proxyFile, matlabName());
-  std::vector<ArgumentList> argLists;
+  }
-  std::vector<ReturnValue> returnVals;
 
-  // The first time this function is called, it initializes the class members
+  void verifyArguments(const std::vector<std::string>& validArgs) const {
-  // with those in rhs, but in subsequent calls it adds additional argument
+    SignatureOverloads::verifyArguments(validArgs, name_);
-  // lists as function overloads.
+  }
-  void addOverload(bool verbose, const std::string& name,
+
-      const ArgumentList& args, const ReturnValue& retVal);
+  void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
+    SignatureOverloads::verifyReturnTypes(validtypes, name_);
+  }
 
   // MATLAB code generation
   // classPath is class directory, e.g., ../matlab/@Point2
   void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-      const std::string& cppClassName, const std::string& matlabQualName,
+      Str cppClassName, Str matlabQualName, Str matlabUniqueName,
-      const std::string& matlabUniqueName, const std::string& wrapperName,
+      Str wrapperName, const TypeAttributesTable& typeAttributes,
-      const TypeAttributesTable& typeAttributes,
       std::vector<std::string>& functionNames) const;
 
-private:
+  // emit python wrapper
-  std::string wrapper_fragment(FileWriter& file,
+  void python_wrapper(FileWriter& wrapperFile, Str className) const;
-      const std::string& cppClassName, const std::string& matlabUniqueName,
+
-      int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
+  friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) {
+    for (size_t i = 0; i < m.nrOverloads(); i++)
+      os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
+    return os;
+  }
+
+protected:
+
+  virtual void proxy_header(FileWriter& proxyFile) const;
+
+  std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
+      Str matlabUniqueName, int overload, int id,
+      const TypeAttributesTable& typeAttributes, const Qualified& instName =
+          Qualified()) const; ///< cpp wrapper
+
+  virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
+      Str matlabUniqueName, const ArgumentList& args,
+      const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
+      const Qualified& instName) const;
 };
 
 } // \namespace wrap

wrap/TemplateInstantiationTypedef.cpp

@@ -19,39 +19,46 @@
 #include "TemplateInstantiationTypedef.h"
 
 #include "utilities.h"
+#include <iostream>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
 
 using namespace std;
 
 namespace wrap {
 
-  Class TemplateInstantiationTypedef::findAndExpand(const vector<Class>& classes) const {
+Class TemplateInstantiationTypedef::findAndExpand(
+    const vector<Class>& classes) const {
   // Find matching class
-    std::vector<Class>::const_iterator clsIt = classes.end();
+  boost::optional<Class const &> matchedClass;
-    for(std::vector<Class>::const_iterator it = classes.begin(); it != classes.end(); ++it) {
+  BOOST_FOREACH(const Class& cls, classes) {
-      if(it->name == className && it->namespaces == classNamespaces && it->templateArgs.size() == typeList.size()) {
+    if (cls.name == class_.name && cls.namespaces == class_.namespaces
-        clsIt = it;
+        && cls.templateArgs.size() == typeList.size()) {
+      matchedClass.reset(cls);
       break;
     }
   }
 
-    if(clsIt == classes.end())
+  if (!matchedClass)
-      throw DependencyMissing(wrap::qualifiedName("::", classNamespaces, className),
+    throw DependencyMissing(class_.qualifiedName("::"),
-      "instantiation into typedef name " + wrap::qualifiedName("::", namespaces, name) +
+        "instantiation into typedef name " + qualifiedName("::")
-      ".  Ensure that the typedef provides the correct number of template arguments.");
+            + ".  Ensure that the typedef provides the correct number of template arguments.");
 
   // Instantiate it
-    Class classInst = *clsIt;
+  Class classInst = *matchedClass;
-    for(size_t i = 0; i < typeList.size(); ++i)
+  for (size_t i = 0; i < typeList.size(); ++i) {
-      classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name);
+    TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this);
+    classInst = classInst.expandTemplate(ts);
+  }
 
   // Fix class properties
   classInst.name = name;
   classInst.templateArgs.clear();
-    classInst.typedefName = clsIt->qualifiedName("::") + "<";
+  classInst.typedefName = matchedClass->qualifiedName("::") + "<";
   if (typeList.size() > 0)
-      classInst.typedefName += wrap::qualifiedName("::", typeList[0]);
+    classInst.typedefName += typeList[0].qualifiedName("::");
   for (size_t i = 1; i < typeList.size(); ++i)
-      classInst.typedefName += (", " + wrap::qualifiedName("::", typeList[i]));
+    classInst.typedefName += (", " + typeList[i].qualifiedName("::"));
   classInst.typedefName += ">";
   classInst.namespaces = namespaces;
 

wrap/TemplateInstantiationTypedef.h

@@ -26,12 +26,9 @@
 
 namespace wrap {
 
-struct TemplateInstantiationTypedef {
+struct TemplateInstantiationTypedef : public Qualified {
-  std::vector<std::string> classNamespaces;
+  Qualified class_;
-  std::string className;
+  std::vector<Qualified> typeList;
-  std::vector<std::string> namespaces;
-  std::string name;
-  std::vector<std::vector<std::string> > typeList;
 
   Class findAndExpand(const std::vector<Class>& classes) const;
 };

wrap/TemplateSubstitution.h

@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file TemplateSubstitution.h
+ * @brief Auxiliary class for template substitutions
+ * @author Frank Dellaert
+ * @date Nov 13, 2014
+ **/
+
+#pragma once
+
+#include "ReturnType.h"
+#include <string>
+#include <iostream>
+
+namespace wrap {
+
+/**
+ * e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2)
+ */
+class TemplateSubstitution {
+
+  std::string templateArg_;
+  Qualified qualifiedType_, expandedClass_;
+
+public:
+
+  TemplateSubstitution(const std::string& a, const Qualified& t,
+      const Qualified& e) :
+      templateArg_(a), qualifiedType_(t), expandedClass_(e) {
+  }
+
+  std::string expandedClassName() const {
+    return expandedClass_.name;
+  }
+
+  // Substitute if needed
+  Qualified operator()(const Qualified& type) const {
+    if (type.name == templateArg_ && type.namespaces.empty())
+      return qualifiedType_;
+    else if (type.name == "This")
+      return expandedClass_;
+    else
+      return type;
+  }
+
+  // Substitute if needed
+  ReturnType operator()(const ReturnType& type) const {
+    ReturnType instType = type;
+    if (type.name == templateArg_ && type.namespaces.empty())
+      instType.rename(qualifiedType_);
+    else if (type.name == "This")
+      instType.rename(expandedClass_);
+    return instType;
+  }
+
+  friend std::ostream& operator<<(std::ostream& os,
+      const TemplateSubstitution& ts) {
+    os << ts.templateArg_ << '/' << ts.qualifiedType_.qualifiedName("::")
+        << " (" << ts.expandedClass_.qualifiedName("::") << ")";
+    return os;
+  }
+
+};
+
+} // \namespace wrap
+

wrap/TypeAttributesTable.cpp

@@ -47,11 +47,15 @@ namespace wrap {
     BOOST_FOREACH(const Class& cls, classes) {
       // Check that class is virtual if it has a parent
       if (!cls.qualifiedParent.empty() && !cls.isVirtual)
-        throw AttributeError(cls.qualifiedName("::"), "Has a base class so needs to be declared virtual, change to 'virtual class "+cls.name+" ...'");
+        throw AttributeError(cls.qualifiedName("::"),
+            "Has a base class so needs to be declared virtual, change to 'virtual class "
+                + cls.name + " ...'");
       // Check that parent is virtual as well
-      if(!cls.qualifiedParent.empty() && !at(wrap::qualifiedName("::", cls.qualifiedParent)).isVirtual)
+      Qualified parent = cls.qualifiedParent;
-        throw AttributeError(wrap::qualifiedName("::", cls.qualifiedParent),
+      if (!parent.empty() && !at(parent.qualifiedName("::")).isVirtual)
-        "Is the base class of " + cls.qualifiedName("::") + ", so needs to be declared virtual");
+        throw AttributeError(parent.qualifiedName("::"),
+            "Is the base class of " + cls.qualifiedName("::")
+                + ", so needs to be declared virtual");
     }
   }
 

wrap/TypeAttributesTable.h

@@ -28,7 +28,7 @@
 namespace wrap {
 
   // Forward declarations
-  struct Class;
+  class Class;
   
 /** Attributes about valid classes, both for classes defined in this module and
  * also those forward-declared from others.  At the moment this only contains

wrap/tests/expected-python/geometry_python.cpp

@@ -0,0 +1,83 @@
+#include <boost/python.hpp>
+
+using namespace boost::python;
+BOOST_PYTHON_MODULE(geometry)
+{
+class_<Point2>("Point2")
+  .def("Point2", &Point2::Point2);
+  .def("argChar", &Point2::argChar);
+  .def("argUChar", &Point2::argUChar);
+  .def("dim", &Point2::dim);
+  .def("returnChar", &Point2::returnChar);
+  .def("vectorConfusion", &Point2::vectorConfusion);
+  .def("x", &Point2::x);
+  .def("y", &Point2::y);
+;
+
+class_<Point3>("Point3")
+  .def("Point3", &Point3::Point3);
+  .def("StaticFunctionRet", &Point3::StaticFunctionRet);
+  .def("staticFunction", &Point3::staticFunction);
+  .def("norm", &Point3::norm);
+;
+
+class_<Test>("Test")
+  .def("Test", &Test::Test);
+  .def("arg_EigenConstRef", &Test::arg_EigenConstRef);
+  .def("create_MixedPtrs", &Test::create_MixedPtrs);
+  .def("create_ptrs", &Test::create_ptrs);
+  .def("print", &Test::print);
+  .def("return_Point2Ptr", &Test::return_Point2Ptr);
+  .def("return_Test", &Test::return_Test);
+  .def("return_TestPtr", &Test::return_TestPtr);
+  .def("return_bool", &Test::return_bool);
+  .def("return_double", &Test::return_double);
+  .def("return_field", &Test::return_field);
+  .def("return_int", &Test::return_int);
+  .def("return_matrix1", &Test::return_matrix1);
+  .def("return_matrix2", &Test::return_matrix2);
+  .def("return_pair", &Test::return_pair);
+  .def("return_ptrs", &Test::return_ptrs);
+  .def("return_size_t", &Test::return_size_t);
+  .def("return_string", &Test::return_string);
+  .def("return_vector1", &Test::return_vector1);
+  .def("return_vector2", &Test::return_vector2);
+;
+
+class_<MyBase>("MyBase")
+  .def("MyBase", &MyBase::MyBase);
+;
+
+class_<MyTemplatePoint2>("MyTemplatePoint2")
+  .def("MyTemplatePoint2", &MyTemplatePoint2::MyTemplatePoint2);
+  .def("accept_T", &MyTemplatePoint2::accept_T);
+  .def("accept_Tptr", &MyTemplatePoint2::accept_Tptr);
+  .def("create_MixedPtrs", &MyTemplatePoint2::create_MixedPtrs);
+  .def("create_ptrs", &MyTemplatePoint2::create_ptrs);
+  .def("return_T", &MyTemplatePoint2::return_T);
+  .def("return_Tptr", &MyTemplatePoint2::return_Tptr);
+  .def("return_ptrs", &MyTemplatePoint2::return_ptrs);
+  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint2::templatedMethod);
+;
+
+class_<MyTemplatePoint3>("MyTemplatePoint3")
+  .def("MyTemplatePoint3", &MyTemplatePoint3::MyTemplatePoint3);
+  .def("accept_T", &MyTemplatePoint3::accept_T);
+  .def("accept_Tptr", &MyTemplatePoint3::accept_Tptr);
+  .def("create_MixedPtrs", &MyTemplatePoint3::create_MixedPtrs);
+  .def("create_ptrs", &MyTemplatePoint3::create_ptrs);
+  .def("return_T", &MyTemplatePoint3::return_T);
+  .def("return_Tptr", &MyTemplatePoint3::return_Tptr);
+  .def("return_ptrs", &MyTemplatePoint3::return_ptrs);
+  .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
+  .def("templatedMethod", &MyTemplatePoint3::templatedMethod);
+;
+
+class_<MyFactorPosePoint2>("MyFactorPosePoint2")
+  .def("MyFactorPosePoint2", &MyFactorPosePoint2::MyFactorPosePoint2);
+;
+
+def("aGlobalFunction", aGlobalFunction);
+def("overloadedGlobalFunction", overloadedGlobalFunction);
+}

wrap/tests/expected2/+gtsam/Point2.m

@@ -0,0 +1,90 @@
+%class Point2, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%Point2()
+%Point2(double x, double y)
+%
+%-------Methods-------
+%argChar(char a) : returns void
+%argUChar(unsigned char a) : returns void
+%dim() : returns int
+%returnChar() : returns char
+%vectorConfusion() : returns VectorNotEigen
+%x() : returns double
+%y() : returns double
+%
+classdef Point2 < handle
+  properties
+    ptr_gtsamPoint2 = 0
+  end
+  methods
+    function obj = Point2(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(0, my_ptr);
+      elseif nargin == 0
+        my_ptr = geometry_wrapper(1);
+      elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
+        my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
+      else
+        error('Arguments do not match any overload of gtsam.Point2 constructor');
+      end
+      obj.ptr_gtsamPoint2 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(3, obj.ptr_gtsamPoint2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = argChar(this, varargin)
+      % ARGCHAR usage: argChar(char a) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      geometry_wrapper(4, this, varargin{:});
+    end
+
+    function varargout = argUChar(this, varargin)
+      % ARGUCHAR usage: argUChar(unsigned char a) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      geometry_wrapper(5, this, varargin{:});
+    end
+
+    function varargout = dim(this, varargin)
+      % DIM usage: dim() : returns int
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(6, this, varargin{:});
+    end
+
+    function varargout = returnChar(this, varargin)
+      % RETURNCHAR usage: returnChar() : returns char
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(7, this, varargin{:});
+    end
+
+    function varargout = vectorConfusion(this, varargin)
+      % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(8, this, varargin{:});
+    end
+
+    function varargout = x(this, varargin)
+      % X usage: x() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(9, this, varargin{:});
+    end
+
+    function varargout = y(this, varargin)
+      % Y usage: y() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(10, this, varargin{:});
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected2/+gtsam/Point3.m

@@ -0,0 +1,61 @@
+%class Point3, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%Point3(double x, double y, double z)
+%
+%-------Methods-------
+%norm() : returns double
+%
+%-------Static Methods-------
+%StaticFunctionRet(double z) : returns gtsam::Point3
+%staticFunction() : returns double
+%
+classdef Point3 < handle
+  properties
+    ptr_gtsamPoint3 = 0
+  end
+  methods
+    function obj = Point3(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(11, my_ptr);
+      elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
+        my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3});
+      else
+        error('Arguments do not match any overload of gtsam.Point3 constructor');
+      end
+      obj.ptr_gtsamPoint3 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(13, obj.ptr_gtsamPoint3);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = norm(this, varargin)
+      % NORM usage: norm() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(14, this, varargin{:});
+    end
+
+  end
+
+  methods(Static = true)
+    function varargout = StaticFunctionRet(varargin)
+      % STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(15, varargin{:});
+    end
+
+    function varargout = StaticFunction(varargin)
+      % STATICFUNCTION usage: staticFunction() : returns double
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      varargout{1} = geometry_wrapper(16, varargin{:});
+    end
+
+  end
+end

wrap/tests/expected2/MyBase.m

@@ -0,0 +1,35 @@
+%class MyBase, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+classdef MyBase < handle
+  properties
+    ptr_MyBase = 0
+  end
+  methods
+    function obj = MyBase(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = geometry_wrapper(41, varargin{2});
+        end
+        geometry_wrapper(40, my_ptr);
+      else
+        error('Arguments do not match any overload of MyBase constructor');
+      end
+      obj.ptr_MyBase = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(42, obj.ptr_MyBase);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected2/MyFactorPosePoint2.m

@@ -0,0 +1,36 @@
+%class MyFactorPosePoint2, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel)
+%
+classdef MyFactorPosePoint2 < handle
+  properties
+    ptr_MyFactorPosePoint2 = 0
+  end
+  methods
+    function obj = MyFactorPosePoint2(varargin)
+      if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        my_ptr = varargin{2};
+        geometry_wrapper(69, 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 = geometry_wrapper(70, varargin{1}, varargin{2}, varargin{3}, varargin{4});
+      else
+        error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
+      end
+      obj.ptr_MyFactorPosePoint2 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(71, obj.ptr_MyFactorPosePoint2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected2/MyTemplatePoint2.m

@@ -0,0 +1,134 @@
+%class MyTemplatePoint2, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%MyTemplatePoint2()
+%
+%-------Methods-------
+%accept_T(Point2 value) : returns void
+%accept_Tptr(Point2 value) : returns void
+%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+%return_T(Point2 value) : returns gtsam::Point2
+%return_Tptr(Point2 value) : returns gtsam::Point2
+%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
+%templatedMethodPoint2(Point2 t) : returns void
+%templatedMethodPoint3(Point3 t) : returns void
+%
+classdef MyTemplatePoint2 < MyBase
+  properties
+    ptr_MyTemplatePoint2 = 0
+  end
+  methods
+    function obj = MyTemplatePoint2(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = geometry_wrapper(44, varargin{2});
+        end
+        base_ptr = geometry_wrapper(43, my_ptr);
+      elseif nargin == 0
+        [ my_ptr, base_ptr ] = geometry_wrapper(45);
+      else
+        error('Arguments do not match any overload of MyTemplatePoint2 constructor');
+      end
+      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
+      obj.ptr_MyTemplatePoint2 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(46, obj.ptr_MyTemplatePoint2);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = accept_T(this, varargin)
+      % ACCEPT_T usage: accept_T(Point2 value) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        geometry_wrapper(47, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
+      end
+    end
+
+    function varargout = accept_Tptr(this, varargin)
+      % ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        geometry_wrapper(48, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
+      end
+    end
+
+    function varargout = create_MixedPtrs(this, varargin)
+      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      [ varargout{1} varargout{2} ] = geometry_wrapper(49, this, varargin{:});
+    end
+
+    function varargout = create_ptrs(this, varargin)
+      % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      [ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:});
+    end
+
+    function varargout = return_T(this, varargin)
+      % RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        varargout{1} = geometry_wrapper(51, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
+      end
+    end
+
+    function varargout = return_Tptr(this, varargin)
+      % RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        varargout{1} = geometry_wrapper(52, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
+      end
+    end
+
+    function varargout = return_ptrs(this, varargin)
+      % RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
+        [ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
+      end
+    end
+
+    function varargout = templatedMethodPoint2(this, varargin)
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        geometry_wrapper(54, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
+      end
+    end
+
+    function varargout = templatedMethodPoint3(this, varargin)
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        geometry_wrapper(55, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
+      end
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end

wrap/tests/expected2/MyTemplatePoint3.m

@@ -0,0 +1,134 @@
+%class MyTemplatePoint3, see Doxygen page for details
+%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+%
+%-------Constructors-------
+%MyTemplatePoint3()
+%
+%-------Methods-------
+%accept_T(Point3 value) : returns void
+%accept_Tptr(Point3 value) : returns void
+%create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
+%create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
+%return_T(Point3 value) : returns gtsam::Point3
+%return_Tptr(Point3 value) : returns gtsam::Point3
+%return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
+%templatedMethodPoint2(Point2 t) : returns void
+%templatedMethodPoint3(Point3 t) : returns void
+%
+classdef MyTemplatePoint3 < MyBase
+  properties
+    ptr_MyTemplatePoint3 = 0
+  end
+  methods
+    function obj = MyTemplatePoint3(varargin)
+      if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
+        if nargin == 2
+          my_ptr = varargin{2};
+        else
+          my_ptr = geometry_wrapper(57, varargin{2});
+        end
+        base_ptr = geometry_wrapper(56, my_ptr);
+      elseif nargin == 0
+        [ my_ptr, base_ptr ] = geometry_wrapper(58);
+      else
+        error('Arguments do not match any overload of MyTemplatePoint3 constructor');
+      end
+      obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
+      obj.ptr_MyTemplatePoint3 = my_ptr;
+    end
+
+    function delete(obj)
+      geometry_wrapper(59, obj.ptr_MyTemplatePoint3);
+    end
+
+    function display(obj), obj.print(''); end
+    %DISPLAY Calls print on the object
+    function disp(obj), obj.display; end
+    %DISP Calls print on the object
+    function varargout = accept_T(this, varargin)
+      % ACCEPT_T usage: accept_T(Point3 value) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        geometry_wrapper(60, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.accept_T');
+      end
+    end
+
+    function varargout = accept_Tptr(this, varargin)
+      % ACCEPT_TPTR usage: accept_Tptr(Point3 value) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        geometry_wrapper(61, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr');
+      end
+    end
+
+    function varargout = create_MixedPtrs(this, varargin)
+      % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      [ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:});
+    end
+
+    function varargout = create_ptrs(this, varargin)
+      % CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      [ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:});
+    end
+
+    function varargout = return_T(this, varargin)
+      % RETURN_T usage: return_T(Point3 value) : returns gtsam::Point3
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        varargout{1} = geometry_wrapper(64, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.return_T');
+      end
+    end
+
+    function varargout = return_Tptr(this, varargin)
+      % RETURN_TPTR usage: return_Tptr(Point3 value) : returns gtsam::Point3
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        varargout{1} = geometry_wrapper(65, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr');
+      end
+    end
+
+    function varargout = return_ptrs(this, varargin)
+      % RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 2 && isa(varargin{1},'gtsam.Point3') && isa(varargin{2},'gtsam.Point3')
+        [ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs');
+      end
+    end
+
+    function varargout = templatedMethodPoint2(this, varargin)
+      % TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
+        geometry_wrapper(67, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
+      end
+    end
+
+    function varargout = templatedMethodPoint3(this, varargin)
+      % TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
+        geometry_wrapper(68, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
+      end
+    end
+
+  end
+
+  methods(Static = true)
+  end
+end