Merged from branch 'trunk'

.cproject

@@ -1,5 +1,7 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+<?fileVersion 4.0.0?>
+
+<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
 	<storageModule moduleId="org.eclipse.cdt.core.settings">
 		<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
 			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
@@ -451,14 +453,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testVelocityConstraint3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -619,6 +613,54 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testPlanarSLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testPose2SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testPose3SLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSimulated2D.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSimulated2DOriented.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testVisualSLAM.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -627,18 +669,18 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
+				<buildTarget>testSerializationSLAM.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSerialization.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testSerialization.run</buildTarget>
+				<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -795,6 +837,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testImuFactor.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j8</buildArguments>
+				<buildTarget>testImuFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -875,14 +925,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBayesTreeOperations.run" path="build/gtsam_unstable/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBayesTreeOperations.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1115,14 +1157,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSerializationSLAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testSerializationSLAM.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1587,14 +1621,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testRot3Q.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testRot3Q.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1912,6 +1938,22 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="RangeISAMExample_plaza2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>RangeISAMExample_plaza2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testImuFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

CppUnitLite/Test.h

@@ -60,7 +60,7 @@ protected:
 /**
  * Normal test will wrap execution in a try/catch block to catch exceptions more effectively
  */
-#define TEST(testName, testGroup)\
+#define TEST(testGroup, testName)\
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
             virtual ~testGroup##testName##Test () {};\
@@ -72,7 +72,7 @@ protected:
  * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
  * will not wrap execution with a try/catch block
  */
-#define TEST_UNSAFE(testName, testGroup)\
+#define TEST_UNSAFE(testGroup, testName)\
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
             virtual ~testGroup##testName##Test () {};\

examples/RangeISAMExample_plaza2.cpp

@@ -0,0 +1,207 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 RangeISAMExample_plaza1.cpp
+ * @brief A 2D Range SLAM example
+ * @date June 20, 2013
+ * @author FRank Dellaert
+ */
+
+// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
+#include <gtsam/geometry/Pose2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+// Here we will use Symbols
+#include <gtsam/nonlinear/Symbol.h>
+
+// We want to use iSAM2 to solve the range-SLAM problem incrementally
+#include <gtsam/nonlinear/ISAM2.h>
+
+// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
+// and initial guesses for any new variables used in the added factors
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+// We will use a non-liear solver to batch-inituialize from the first 150 frames
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// have been provided with the library for solving robotics SLAM problems.
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/RangeFactor.h>
+
+// Standard headers, added last, so we know headers above work on their own
+#include <boost/foreach.hpp>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+namespace NM = gtsam::noiseModel;
+
+// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+
+// load the odometry
+// DR: Odometry Input (delta distance traveled and delta heading change)
+//    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
+typedef pair<double, Pose2> TimedOdometry;
+list<TimedOdometry> readOdometry() {
+  list<TimedOdometry> odometryList;
+  ifstream is("../../examples/Data/Plaza2_DR.txt");
+  if (!is)
+    throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
+
+  while (is) {
+    double t, distance_traveled, delta_heading;
+    is >> t >> distance_traveled >> delta_heading;
+    odometryList.push_back(
+        TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return odometryList;
+}
+
+// load the ranges from TD
+//    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
+typedef boost::tuple<double, size_t, double> RangeTriple;
+vector<RangeTriple> readTriples() {
+  vector<RangeTriple> triples;
+  ifstream is("../../examples/Data/Plaza2_TD.txt");
+  if (!is)
+    throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
+
+  while (is) {
+    double t, sender, receiver, range;
+    is >> t >> sender >> receiver >> range;
+    triples.push_back(RangeTriple(t, receiver, range));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return triples;
+}
+
+// main
+int main (int argc, char** argv) {
+
+  // load Plaza2 data
+  list<TimedOdometry> odometry = readOdometry();
+//  size_t M = odometry.size();
+
+  vector<RangeTriple> triples = readTriples();
+  size_t K = triples.size();
+
+  // parameters
+  size_t minK = 150; // minimum number of range measurements to process initially
+  size_t incK = 25; // minimum number of range measurements to process after
+  bool groundTruth = false;
+  bool robust = true;
+
+  // Set Noise parameters
+  Vector priorSigmas = Vector3(1,1,M_PI);
+  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
+  double sigmaR = 100; // range standard deviation
+  const NM::Base::shared_ptr // all same type
+  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
+  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
+  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
+  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
+  rangeNoise = robust ? tukey : gaussian;
+
+  // Initialize iSAM
+  ISAM2 isam;
+
+  // Add prior on first pose
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+      M_PI - 2.02108900000000);
+  NonlinearFactorGraph newFactors;
+  newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
+  Values initial;
+  initial.insert(0, pose0);
+
+  //  initialize points
+  if (groundTruth) { // from TL file
+    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
+    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
+    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
+    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
+  } else { // drawn from sigma=1 Gaussian in matlab version
+    initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
+    initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
+    initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
+    initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
+  }
+
+  // set some loop variables
+  size_t i = 1; // step counter
+  size_t k = 0; // range measurement counter
+  bool initialized = false;
+  Pose2 lastPose = pose0;
+  size_t countK = 0;
+
+  // Loop over odometry
+  gttic_(iSAM);
+  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+    //--------------------------------- odometry loop -----------------------------------------
+    double t;
+    Pose2 odometry;
+    boost::tie(t, odometry) = timedOdometry;
+
+    // add odometry factor
+    newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
+
+    // predict pose and add as initial estimate
+    Pose2 predictedPose = lastPose.compose(odometry);
+    lastPose = predictedPose;
+    initial.insert(i, predictedPose);
+
+    // Check if there are range factors to be added
+    while (k < K && t >= boost::get<0>(triples[k])) {
+      size_t j = boost::get<1>(triples[k]);
+      double range = boost::get<2>(triples[k]);
+      newFactors.add(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
+      k = k + 1;
+      countK = countK + 1;
+    }
+
+    // Check whether to update iSAM 2
+    if ((k > minK) && (countK > incK)) {
+      if (!initialized) { // Do a full optimize for first minK ranges
+        gttic_(batchInitialization);
+        LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
+        initial = batchOptimizer.optimize();
+        gttoc_(batchInitialization);
+        initialized = true;
+      }
+      gttic_(update);
+      isam.update(newFactors, initial);
+      gttoc_(update);
+      gttic_(calculateEstimate);
+      Values result = isam.calculateEstimate();
+      gttoc_(calculateEstimate);
+      lastPose = result.at<Pose2>(i);
+      newFactors = NonlinearFactorGraph();
+      initial = Values();
+      countK = 0;
+    }
+    i += 1;
+    //--------------------------------- odometry loop -----------------------------------------
+  } // BOOST_FOREACH
+  gttoc_(iSAM);
+
+  // Print timings
+  tictoc_print_();
+
+  exit(0);
+}
+

examples/VisualISAM2Example.cpp

@@ -36,13 +36,6 @@
 // Here we will use Symbols
 #include <gtsam/nonlinear/Symbol.h>
 
-// In GTSAM, measurement functions are represented as 'factors'. Several common factors
-// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
-// Here we will use Projection factors to model the camera's landmark observations.
-// Also, we will initialize the robot at some location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/ProjectionFactor.h>
-
 // We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
 // include iSAM2 here
 #include <gtsam/nonlinear/ISAM2.h>
@@ -52,6 +45,13 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 
+// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
+// Here we will use Projection factors to model the camera's landmark observations.
+// Also, we will initialize the robot at some location using a Prior factor.
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
 #include <vector>
 
 using namespace std;

gtsam.h

@@ -76,6 +76,18 @@
  *     - When forward-declaring template instantiations, use the generated/typedefed name, e.g.
  *       class gtsam::Class1Pose2;
  *       class gtsam::MyInstantiatedClass;
+ *   Boost.serialization within Matlab:
+ *     - you need to mark classes as being serializable in the markup file (see this file for an example).
+ *     - There are two options currently, depending on the class.  To "mark" a class as serializable,
+ *       add a function with a particular signature so that wrap will catch it.
+ *        - Add "void serialize()" to a class to create serialization functions for a class.
+ *          Adding this flag subsumes the serializable() flag below. Requirements:
+ *             - A default constructor must be publicly accessible
+ *             - Must not be an abstract base class
+ *             - The class must have an actual boost.serialization serialize() function.
+ *        - Add "void serializable()" to a class if you only want the class to be serialized as a
+ *          part of a container (such as noisemodel). This version does not require a publicly
+ *          accessible default constructor.
  */
 
 /**
@@ -200,6 +212,9 @@ virtual class LieVector : gtsam::Value {
   // Lie group
   static gtsam::LieVector Expmap(Vector v);
   static Vector Logmap(const gtsam::LieVector& p);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/base/LieMatrix.h>
@@ -229,6 +244,9 @@ virtual class LieMatrix : gtsam::Value {
   // Lie group
   static gtsam::LieMatrix Expmap(Vector v);
   static Vector Logmap(const gtsam::LieMatrix& p);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 //*************************************************************************
@@ -267,6 +285,9 @@ virtual class Point2 : gtsam::Value {
   Vector vector() const;
   double dist(const gtsam::Point2& p2) const;
   double norm() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class StereoPoint2 : gtsam::Value {
@@ -296,6 +317,9 @@ virtual class StereoPoint2 : gtsam::Value {
 
   // Standard Interface
   Vector vector() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Point3 : gtsam::Value {
@@ -329,6 +353,9 @@ virtual class Point3 : gtsam::Value {
   double x() const;
   double y() const;
   double z() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Rot2 : gtsam::Value {
@@ -371,6 +398,9 @@ virtual class Rot2 : gtsam::Value {
   double c() const;
   double s() const;
   Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Rot3 : gtsam::Value {
@@ -424,11 +454,15 @@ virtual class Rot3 : gtsam::Value {
   double yaw() const;
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
   Vector quaternion() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Pose2 : gtsam::Value {
   // Standard Constructor
   Pose2();
+  Pose2(const gtsam::Pose2& pose);
   Pose2(double x, double y, double theta);
   Pose2(double theta, const gtsam::Point2& t);
   Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
@@ -470,6 +504,9 @@ virtual class Pose2 : gtsam::Value {
   gtsam::Point2 translation() const;
   gtsam::Rot2 rotation() const;
   Matrix matrix() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Pose3 : gtsam::Value {
@@ -518,6 +555,9 @@ virtual class Pose3 : gtsam::Value {
   gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
   double range(const gtsam::Point3& point);
   double range(const gtsam::Pose3& pose);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class Cal3_S2 : gtsam::Value {
@@ -551,6 +591,9 @@ virtual class Cal3_S2 : gtsam::Value {
   Vector vector() const;
   Matrix matrix() const;
   Matrix matrix_inverse() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/geometry/Cal3DS2.h>
@@ -585,6 +628,9 @@ virtual class Cal3DS2 : gtsam::Value {
   Vector vector() const;
   Vector k() const;
   //Matrix K() const; //FIXME: Uppercase
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class Cal3_S2Stereo {
@@ -634,6 +680,9 @@ virtual class CalibratedCamera : gtsam::Value {
   // Standard Interface
   gtsam::Pose3 pose() const;
   double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class SimpleCamera : gtsam::Value {
@@ -669,6 +718,9 @@ virtual class SimpleCamera : gtsam::Value {
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
   double range(const gtsam::Point3& point);
   double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 template<CALIBRATION = {gtsam::Cal3DS2}>
@@ -705,6 +757,9 @@ virtual class PinholeCamera : gtsam::Value {
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
   double range(const gtsam::Point3& point);
   double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 //*************************************************************************
@@ -988,6 +1043,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
   Matrix R() const;
   bool equals(gtsam::noiseModel::Base& expected, double tol);
   void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Diagonal : gtsam::noiseModel::Gaussian {
@@ -996,6 +1054,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
   Matrix R() const;
   void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Constrained : gtsam::noiseModel::Diagonal {
@@ -1010,6 +1071,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
     static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
 
     gtsam::noiseModel::Constrained* unit() const;
+
+    // enabling serialization functionality
+    void serializable() const;
 };
 
 virtual class Isotropic : gtsam::noiseModel::Diagonal {
@@ -1017,11 +1081,17 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
   static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
   static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
   void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Unit : gtsam::noiseModel::Isotropic {
   static gtsam::noiseModel::Unit* Create(size_t dim);
   void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 namespace mEstimator {
@@ -1032,24 +1102,36 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
   Null();
   void print(string s) const;
   static gtsam::noiseModel::mEstimator::Null* Create();
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   Fair(double c);
   void print(string s) const;
   static gtsam::noiseModel::mEstimator::Fair* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   Huber(double k);
   void print(string s) const;
   static gtsam::noiseModel::mEstimator::Huber* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   Tukey(double k);
   void print(string s) const;
   static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 }///\namespace mEstimator
@@ -1058,6 +1140,9 @@ virtual class Robust : gtsam::noiseModel::Base {
   Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
   static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
   void print(string s) const;
+
+  // enabling serialization functionality
+  void serializable() const;
 };
 
 }///\namespace noiseModel
@@ -1112,6 +1197,9 @@ class VectorValues {
     bool hasSameStructure(const gtsam::VectorValues& other)  const;
     double dot(const gtsam::VectorValues& V) const;
     double norm() const;
+
+    // enabling serialization functionality
+    void serialize() const;
 };
 
 class GaussianConditional {
@@ -1134,6 +1222,9 @@ class GaussianConditional {
   void solveInPlace(gtsam::VectorValues& x) const;
   void solveTransposeInPlace(gtsam::VectorValues& gy) const;
   void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class GaussianDensity {
@@ -1180,6 +1271,15 @@ virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase {
   GaussianBayesTree();
   GaussianBayesTree(const gtsam::GaussianBayesNet& bn);
   GaussianBayesTree(const gtsam::GaussianBayesNet& other);
+  bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
+  void print(string s);
+  size_t size() const;
+  size_t nrNodes() const;
+  bool empty() const;
+  gtsam::GaussianBayesTreeClique* root() const;
+  gtsam::GaussianBayesTreeClique* clique(size_t j) const;
+  size_t numCachedSeparatorMarginals() const;
+  void saveGraph(string s) const;
 };
 
 // namespace functions for GaussianBayesTree
@@ -1244,6 +1344,9 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   void assertInvariants() const;
 
   //gtsam::SharedDiagonal& get_model();
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class HessianFactor : gtsam::GaussianFactor {
@@ -1277,6 +1380,9 @@ virtual class HessianFactor : gtsam::GaussianFactor {
   void partialCholesky(size_t nrFrontals);
   gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals);
   void assertInvariants() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class GaussianFactorGraph {
@@ -1323,6 +1429,9 @@ class GaussianFactorGraph {
   pair<Matrix,Vector> jacobian() const;
   Matrix augmentedHessian() const;
   pair<Matrix,Vector> hessian() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 //Non-Class functions in GaussianFactorGraph.h
@@ -1502,6 +1611,9 @@ class Ordering {
   void push_back(size_t key);
   void permuteInPlace(const gtsam::Permutation& permutation);
   void permuteInPlace(const gtsam::Permutation& selector, const gtsam::Permutation& permutation);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class NonlinearFactorGraph {
@@ -1529,22 +1641,34 @@ class NonlinearFactorGraph {
       const gtsam::Ordering& ordering) const;
   gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const;
   gtsam::NonlinearFactorGraph clone() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 virtual class NonlinearFactor {
+  // Factor base class
+  size_t size() const;
+  gtsam::KeyVector keys() const;
   void print(string s) const;
   void printKeys(string s) const;
+  // NonlinearFactor
   void equals(const gtsam::NonlinearFactor& other, double tol) const;
-  gtsam::KeyVector keys() const;
-  size_t size() const;
-  size_t dim() const;
   double error(const gtsam::Values& c) const;
+  size_t dim() const;
   bool active(const gtsam::Values& c) const;
   gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
   gtsam::NonlinearFactor* clone() const;
   // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
 };
 
+virtual class NoiseModelFactor: gtsam::NonlinearFactor {
+  void equals(const gtsam::NoiseModelFactor& other, double tol) const;
+  gtsam::noiseModel::Base* get_noiseModel() const;
+  Vector unwhitenedError(const gtsam::Values& x) const;
+  Vector whitenedError(const gtsam::Values& x) const;
+};
+
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();
@@ -1552,29 +1676,32 @@ class Values {
 
   size_t size() const;
   bool empty() const;
-    void clear();
+  void clear();
-    size_t dim() const;
+  size_t dim() const;
 
   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 insert(const gtsam::Values& values);
-    void update(size_t j, const gtsam::Value& val);
+  void update(size_t j, const gtsam::Value& val);
-    void update(const gtsam::Values& values);
+  void update(const gtsam::Values& values);
-    void erase(size_t j);
+  void erase(size_t j);
-    void swap(gtsam::Values& values);
+  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 gtsam::Ordering& ordering) const;
+  gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const;
-    gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
+  gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
 
-    gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
+  gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
-    gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
+  gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
-    void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
+  void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 // Actually a FastList<Key>
@@ -1691,6 +1818,8 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
   static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
       const gtsam::Ordering& ordering);
 
+  // enabling serialization functionality
+  void serializable() const;
 }; // \class LinearContainerFactor
 
 // Summarization functionality
@@ -1961,37 +2090,43 @@ class NonlinearISAM {
 
 #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}>
-virtual class PriorFactor : gtsam::NonlinearFactor {
+virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 
 #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}>
-virtual class BetweenFactor : gtsam::NonlinearFactor {
+virtual class BetweenFactor : gtsam::NoiseModelFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
   T measured() const;
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 
 #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}>
-virtual class NonlinearEquality : gtsam::NonlinearFactor {
+virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
   // Constructor - allows inexact evaluation
   NonlinearEquality(size_t j, const T& feasible, double error_gain);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 
 #include <gtsam/slam/RangeFactor.h>
 template<POSE, POINT>
-virtual class RangeFactor : gtsam::NonlinearFactor {
+virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
-  gtsam::noiseModel::Base* get_noiseModel() const;
 };
 
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
@@ -2006,9 +2141,11 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
 
 #include <gtsam/slam/BearingFactor.h>
 template<POSE, POINT, ROTATION>
-virtual class BearingFactor : gtsam::NonlinearFactor {
+virtual class BearingFactor : gtsam::NoiseModelFactor {
   BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
@@ -2016,9 +2153,11 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
 
 #include <gtsam/slam/BearingRangeFactor.h>
 template<POSE, POINT, ROTATION>
-virtual class BearingRangeFactor : gtsam::NonlinearFactor {
+virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
   BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
@@ -2026,7 +2165,7 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> Bear
 
 #include <gtsam/slam/ProjectionFactor.h>
 template<POSE, LANDMARK, CALIBRATION>
-virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
+virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
   GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
     size_t poseKey, size_t pointKey, const CALIBRATION* k);
   GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
@@ -2042,7 +2181,9 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
   CALIBRATION* calibration() const;
   bool verboseCheirality() const;
   bool throwCheirality() const;
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
@@ -2050,7 +2191,7 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3D
 
 #include <gtsam/slam/GeneralSFMFactor.h>
 template<CAMERA, LANDMARK>
-virtual class GeneralSFMFactor : gtsam::NonlinearFactor {
+virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
   GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
   gtsam::Point2 measured() const;
 };
@@ -2058,28 +2199,32 @@ typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFa
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 
 template<CALIBRATION = {gtsam::Cal3_S2}>
-virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor {
+virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
   gtsam::Point2 measured() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 
 #include <gtsam/slam/StereoFactor.h>
 template<POSE, LANDMARK>
-virtual class GenericStereoFactor : gtsam::NonlinearFactor {
+virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
   GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel,
     size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
   gtsam::StereoPoint2 measured() const;
   gtsam::Cal3_S2Stereo* calibration() const;
-  gtsam::noiseModel::Base* get_noiseModel() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
 
 #include <gtsam/slam/PoseTranslationPrior.h>
 template<POSE>
-virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
+virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
   PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
-  gtsam::noiseModel::Base* get_noiseModel() const;
 };
 
 typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
@@ -2087,9 +2232,8 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
 
 #include <gtsam/slam/PoseRotationPrior.h>
 template<POSE>
-virtual class PoseRotationPrior : gtsam::NonlinearFactor {
+virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
   PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
-  gtsam::noiseModel::Base* get_noiseModel() const;
 };
 
 typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
@@ -2107,51 +2251,6 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
     gtsam::noiseModel::Base* model);
 
-//*************************************************************************
-// Serialization
-//*************************************************************************
-#include <gtsam/slam/serialization.h>
-
-// Serialize/Deserialize a NonlinearFactorGraph
-string serializeGraph(const gtsam::NonlinearFactorGraph& graph);
-
-gtsam::NonlinearFactorGraph* deserializeGraph(string serialized_graph);
-
-string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph);
-string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph, string name);
-
-gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph);
-gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph, string name);
-
-// Serialize/Deserialize a Values
-string serializeValues(const gtsam::Values& values);
-
-gtsam::Values* deserializeValues(string serialized_values);
-
-string serializeValuesXML(const gtsam::Values& values);
-string serializeValuesXML(const gtsam::Values& values, string name);
-
-gtsam::Values* deserializeValuesXML(string serialized_values);
-gtsam::Values* deserializeValuesXML(string serialized_values, string name);
-
-// Serialize
-bool serializeGraphToFile(const gtsam::NonlinearFactorGraph& graph, string fname);
-bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname);
-bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname, string name);
-
-bool serializeValuesToFile(const gtsam::Values& values, string fname);
-bool serializeValuesToXMLFile(const gtsam::Values& values, string fname);
-bool serializeValuesToXMLFile(const gtsam::Values& values, string fname, string name);
-
-// Deserialize
-gtsam::NonlinearFactorGraph* deserializeGraphFromFile(string fname);
-gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname);
-gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname, string name);
-
-gtsam::Values* deserializeValuesFromFile(string fname);
-gtsam::Values* deserializeValuesFromXMLFile(string fname);
-gtsam::Values* deserializeValuesFromXMLFile(string fname, string name);
-
 //*************************************************************************
 // Utilities
 //*************************************************************************

gtsam/CMakeLists.txt

@@ -33,11 +33,13 @@ endif()
 # To exclude a source from the library build (in any subfolder)
 # Add the full name to this list, as in the following example
 # Sources to remove from builds
-set (excluded_sources "")
+set (excluded_sources #"")
-#    "${CMAKE_CURRENT_SOURCE_DIR}/discrete/TypedDiscreteFactor.cpp" 
+    "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
-#    "${CMAKE_CURRENT_SOURCE_DIR}/discrete/TypedDiscreteFactorGraph.cpp" 
+)
-#    "${CMAKE_CURRENT_SOURCE_DIR}/discrete/parseUAI.cpp" 
+
-#    "${CMAKE_CURRENT_SOURCE_DIR}/discrete/PotentialTable.cpp")
+set (excluded_headers #"")
+    "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
+)
     
 if(GTSAM_USE_QUATERNIONS)
     set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3M.cpp")
@@ -54,9 +56,10 @@ foreach(subdir ${gtsam_subdirs})
     # Build convenience libraries
     file(GLOB subdir_cpp_srcs "${subdir}/*.cpp")
     file(GLOB subdir_headers "${subdir}/*.h")
+    list(REMOVE_ITEM subdir_cpp_srcs ${excluded_sources})
+    list(REMOVE_ITEM subdir_headers ${excluded_headers})
     set(subdir_srcs ${subdir_cpp_srcs} ${subdir_headers}) # Include header files so they show up in Visual Studio
     gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure
-    list(REMOVE_ITEM subdir_srcs ${excluded_sources})
     set(${subdir}_srcs ${subdir_srcs})
     if (GTSAM_BUILD_CONVENIENCE_LIBRARIES AND (subdir_cpp_srcs)) # Only build convenience library if sources exist
         message(STATUS "Building Convenience Library: ${subdir}")

gtsam/base/Vector.h

@@ -34,6 +34,7 @@ namespace gtsam {
 typedef Eigen::VectorXd Vector;
 
 // Commonly used fixed size vectors
+typedef Eigen::Vector2d Vector2;
 typedef Eigen::Vector3d Vector3;
 typedef Eigen::Matrix<double, 6, 1> Vector6;
 
@@ -373,7 +374,21 @@ namespace boost {
       ar >> make_nvp("data", make_array(v.data(), v.size()));
     }
 
+    // split version - copies into an STL vector for serialization
+    template<class Archive, int D>
+    void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int version) {
+      ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
+    }
+
+    template<class Archive, int D>
+    void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int version) {
+      ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
+    }
+
   } // namespace serialization
 } // namespace boost
 
 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)

gtsam/base/tests/testSerializationBase.cpp

@@ -85,12 +85,21 @@ TEST (Serialization, FastVector) {
 /* ************************************************************************* */
 TEST (Serialization, matrix_vector) {
   EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equality<Vector2>(Vector2(1.0, 2.0)));
+  EXPECT(equality<Vector3>(Vector3(1.0, 2.0, 3.0)));
+  EXPECT(equality<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
   EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
 
   EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equalityXML<Vector2>(Vector2(1.0, 2.0)));
+  EXPECT(equalityXML<Vector3>(Vector3(1.0, 2.0, 3.0)));
+  EXPECT(equalityXML<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
   EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
 
   EXPECT(equalityBinary<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
+  EXPECT(equalityBinary<Vector2>(Vector2(1.0, 2.0)));
+  EXPECT(equalityBinary<Vector3>(Vector3(1.0, 2.0, 3.0)));
+  EXPECT(equalityBinary<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
   EXPECT(equalityBinary<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
 }
 

gtsam/geometry/Point2.cpp

@@ -17,6 +17,8 @@
 
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/Lie-inl.h>
+#include <boost/foreach.hpp>
+#include <cmath>
 
 using namespace std;
 
@@ -25,6 +27,8 @@ namespace gtsam {
 /** Explicit instantiation of base class to export members */
 INSTANTIATE_LIE(Point2);
 
+static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0);
+
 /* ************************************************************************* */
 void Point2::print(const string& s) const {
   cout << s << *this << endl;
@@ -36,8 +40,107 @@ bool Point2::equals(const Point2& q, double tol) const {
 }
 
 /* ************************************************************************* */
-double Point2::norm() const {
+double Point2::norm(boost::optional<Matrix&> H) const {
-  return sqrt(x_*x_ + y_*y_);
+  double r = sqrt(x_ * x_ + y_ * y_);
+  if (H) {
+    Matrix D_result_d;
+    if (fabs(r) > 1e-10)
+      D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
+    else
+      D_result_d = oneOne; // TODO: really infinity, why 1 here??
+    *H = D_result_d;
+  }
+  return r;
+}
+
+/* ************************************************************************* */
+static const Matrix I2 = eye(2);
+double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  Point2 d = point - *this;
+  if (H1 || H2) {
+    Matrix H;
+    double r = d.norm(H);
+    if (H1) *H1 = -H;
+    if (H2) *H2 =  H;
+    return r;
+  } else
+    return d.norm();
+}
+
+/*
+ * Calculate f and h, respectively the parallel and perpendicular distance of
+ * the intersections of two circles along and from the line connecting the centers.
+ * Both are dimensionless fractions of the distance d between the circle centers.
+ * If the circles do not intersect or they are identical, returns boost::none.
+ * If one solution (touching circles, as determined by tol), h will be exactly zero.
+ * h is a good measure for how accurate the intersection will be, as when circles touch
+ * or nearly touch, the intersection is ill-defined with noisy radius measurements.
+ * @param R_d : R/d, ratio of radius of first circle to distance between centers
+ * @param r_d : r/d, ratio of radius of second circle to distance between centers
+ * @param tol: absolute tolerance below which we consider touching circles
+ */
+/* ************************************************************************* */
+// Math inspired by http://paulbourke.net/geometry/circlesphere/
+boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
+    double tol) {
+
+  double R2_d2 = R_d*R_d; // Yes, RD-D2 !
+  double f = 0.5 + 0.5*(R2_d2 - r_d*r_d);
+  double h2 = R2_d2 - f*f; // just right triangle rule
+
+  // h^2<0 is equivalent to (d > (R + r) || d < (R - r))
+  // Hence, there are only solutions if >=0
+  if (h2<-tol) return boost::none; // allow *slightly* negative
+  else if (h2<tol) return Point2(f,0.0); // one solution
+  else return Point2(f,sqrt(h2)); // two solutions
+}
+
+/* ************************************************************************* */
+list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
+    boost::optional<Point2> fh) {
+
+  list<Point2> solutions;
+  // If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
+  if (fh) {
+    // vector between circle centers
+    Point2 c12 = c2-c1;
+
+    // Determine p2, the point where the line through the circle
+    // intersection points crosses the line between the circle centers.
+    Point2 p2 = c1 + fh->x() * c12;
+
+    // If h == 0, the circles are touching, so just return one point
+    if (fh->y()==0.0)
+      solutions.push_back(p2);
+    else {
+      // determine the offsets of the intersection points from p
+      Point2 offset = fh->y() * Point2(-c12.y(), c12.x());
+
+      // Determine the absolute intersection points.
+      solutions.push_back(p2 + offset);
+      solutions.push_back(p2 - offset);
+    }
+  }
+  return solutions;
+}
+
+/* ************************************************************************* */
+list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
+    double r2, double tol) {
+
+  // distance between circle centers.
+  double d = c1.dist(c2);
+
+  // centers coincide, either no solution or infinite number of solutions.
+  if (d<1e-9) return list<Point2>();
+
+  // Calculate f and h given normalized radii
+  double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
+  boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d);
+
+  // Call version that takes fh
+  return CircleCircleIntersection(c1, c2, fh);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point2.h

@@ -65,6 +65,43 @@ public:
     y_ = v(1);
   }
 
+  /*
+   * @brief Circle-circle intersection, given normalized radii.
+   * Calculate f and h, respectively the parallel and perpendicular distance of
+   * the intersections of two circles along and from the line connecting the centers.
+   * Both are dimensionless fractions of the distance d between the circle centers.
+   * If the circles do not intersect or they are identical, returns boost::none.
+   * If one solution (touching circles, as determined by tol), h will be exactly zero.
+   * h is a good measure for how accurate the intersection will be, as when circles touch
+   * or nearly touch, the intersection is ill-defined with noisy radius measurements.
+   * @param R_d : R/d, ratio of radius of first circle to distance between centers
+   * @param r_d : r/d, ratio of radius of second circle to distance between centers
+   * @param tol: absolute tolerance below which we consider touching circles
+   * @return optional Point2 with f and h, boost::none if no solution.
+   */
+  static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d,
+      double tol = 1e-9);
+
+  /*
+   * @brief Circle-circle intersection, from the normalized radii solution.
+   * @param c1 center of first circle
+   * @param c2 center of second circle
+   * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
+   */
+  static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);
+
+  /**
+   * @brief Intersect 2 circles
+   * @param c1 center of first circle
+   * @param r1 radius of first circle
+   * @param c2 center of second circle
+   * @param r2 radius of second circle
+   * @param tol: absolute tolerance below which we consider touching circles
+   * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
+   */
+  static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1,
+      Point2 c2, double r2, double tol = 1e-9);
+
   /// @}
   /// @name Testable
   /// @{
@@ -144,16 +181,15 @@ public:
   /// @name Vector Space
   /// @{
 
-  /** norm of point */
-  double norm() const;
-
   /** creates a unit vector */
   Point2 unit() const { return *this/norm(); }
 
+  /** norm of point */
+  double norm(boost::optional<Matrix&> H = boost::none) const;
+
   /** distance between two points */
-  inline double distance(const Point2& p2) const {
+  double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
-    return (p2 - *this).norm();
+      boost::optional<Matrix&> H2 = boost::none) const;
-  }
 
   /** @deprecated The following function has been deprecated, use distance above */
   inline double dist(const Point2& p2) const {
@@ -180,7 +216,7 @@ public:
   double y() const {return y_;}
 
   /// return vectorized form (column-wise). TODO: why does this function exist?
-  Vector vector() const { return Vector_(2, x_, y_); }
+  Vector2 vector() const { return Vector2(x_, y_); }
 
   /// @}
   /// @name Deprecated (non-const, non-functional style. Do not use).
@@ -190,7 +226,7 @@ public:
   /// @}
 
   /// Streaming
-  friend std::ostream &operator<<(std::ostream &os, const Point2& p);
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
 
 private:
 

gtsam/geometry/Point3.h

@@ -180,9 +180,7 @@ namespace gtsam {
     bool   operator ==(const Point3& q) const;
 
     /** return vectorized form (column-wise)*/
-    Vector3 vector() const {
+    Vector3 vector() const { return Vector3(x_,y_,z_); }
-      return Vector3(x_,y_,z_);
-    }
 
     /// get x
     inline double x() const {return x_;}
@@ -204,7 +202,7 @@ namespace gtsam {
     /// @}
 
     /// Output stream operator
-    friend std::ostream &operator<<(std::ostream &os, const Point3& p);
+    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
 
   private:
 

gtsam/geometry/Pose2.cpp

@@ -221,32 +221,31 @@ Rot2 Pose2::bearing(const Pose2& point,
 /* ************************************************************************* */
 double Pose2::range(const Point2& point,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (!H1 && !H2)  return transform_to(point).norm();
+  Point2 d = point - t_;
-  Point2 d = transform_to(point, H1, H2);
+  if (!H1 && !H2) return d.norm();
-  double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
+  Matrix H;
-  Matrix D_result_d;
+  double r = d.norm(H);
-  if(std::abs(r) > 1e-10)
+  if (H1) *H1 = H * Matrix_(2, 3,
-    D_result_d = Matrix_(1, 2, x / r, y / r);
+      -r_.c(),  r_.s(),  0.0,
-  else {
+      -r_.s(), -r_.c(),  0.0);
-    D_result_d = Matrix_(1,2, 1.0, 1.0);
+  if (H2) *H2 = H;
-  }
-  if (H1) *H1 = D_result_d * (*H1);
-  if (H2) *H2 = D_result_d * (*H2);
   return r;
 }
 
 /* ************************************************************************* */
-double Pose2::range(const Pose2& point,
+double Pose2::range(const Pose2& pose2,
     boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
-  double r = range(point.t(), H1, H2);
+  Point2 d = pose2.t() - t_;
-  if (H2) {
+  if (!H1 && !H2) return d.norm();
-    // NOTE: expmap changes the orientation of expmap direction,
+  Matrix H;
-    // so we must rotate the jacobian
+  double r = d.norm(H);
-    Matrix H2_ = *H2 * point.r().matrix();
+  if (H1) *H1 = H * Matrix_(2, 3,
-    *H2 = zeros(1, 3);
+      -r_.c(),  r_.s(),  0.0,
-    insertSub(*H2, H2_, 0, 0);
+      -r_.s(), -r_.c(),  0.0);
-  }
+  if (H2) *H2 = H * Matrix_(2, 3,
+      pose2.r_.c(), -pose2.r_.s(),  0.0,
+      pose2.r_.s(),  pose2.r_.c(),  0.0);
   return r;
 }
 

gtsam/geometry/Pose3.h

@@ -291,7 +291,7 @@ namespace gtsam {
     static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
 
     /// Output stream operator
-    friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
+    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
 
   private:
     /** Serialization function */

gtsam/geometry/Rot3.h

@@ -356,7 +356,7 @@ namespace gtsam {
     Vector quaternion() const;
 
     /// Output stream operator
-    friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
+    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
   private:
     /** Serialization function */

gtsam/geometry/tests/testPoint2.cpp

@@ -15,9 +15,11 @@
  * @author Frank Dellaert
  **/
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/lieProxies.h>
+#include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
@@ -27,13 +29,13 @@ GTSAM_CONCEPT_LIE_INST(Point2)
 
 /* ************************************************************************* */
 TEST(Point2, constructor) {
-  Point2 p1(1,2), p2 = p1;
+  Point2 p1(1, 2), p2 = p1;
   EXPECT(assert_equal(p1, p2));
 }
 
 /* ************************************************************************* */
 TEST(Point2, Lie) {
-  Point2 p1(1,2), p2(4,5);
+  Point2 p1(1, 2), p2(4, 5);
   Matrix H1, H2;
 
   EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
@@ -49,8 +51,7 @@ TEST(Point2, Lie) {
 }
 
 /* ************************************************************************* */
-TEST( Point2, expmap)
+TEST( Point2, expmap) {
-{
   Vector d(2);
   d(0) = 1;
   d(1) = -1;
@@ -59,8 +60,7 @@ TEST( Point2, expmap)
 }
 
 /* ************************************************************************* */
-TEST( Point2, arithmetic)
+TEST( Point2, arithmetic) {
-{
   EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) ));
   EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1)));
   EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) ));
@@ -70,34 +70,138 @@ TEST( Point2, arithmetic)
 }
 
 /* ************************************************************************* */
-TEST( Point2, norm)
+TEST( Point2, unit) {
-{
+  Point2 p0(10, 0), p1(0, -10), p2(10, 10);
-  Point2 p0(cos(5.0), sin(5.0));
-  DOUBLES_EQUAL(1,p0.norm(),1e-6);
-  Point2 p1(4, 5), p2(1, 1);
-  DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
-  DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
-}
-
-/* ************************************************************************* */
-TEST( Point2, unit)
-{
-  Point2 p0(10, 0), p1(0,-10), p2(10, 10);
   EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
   EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
   EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
 }
 
 /* ************************************************************************* */
-TEST( Point2, stream)
+// some shared test values
-{
+Point2 x1, x2(1, 1), x3(1, 1);
-  Point2 p(1,2);
+Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
+
+/* ************************************************************************* */
+LieVector norm_proxy(const Point2& point) {
+  return LieVector(point.norm());
+}
+TEST( Point2, norm ) {
+  Point2 p0(cos(5.0), sin(5.0));
+  DOUBLES_EQUAL(1, p0.norm(), 1e-6);
+  Point2 p1(4, 5), p2(1, 1);
+  DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
+  DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
+
+  Matrix expectedH, actualH;
+  double actual;
+
+  // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
+  actual = x1.norm(actualH);
+  EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
+  expectedH = Matrix_(1, 2, 1.0, 1.0);
+  EXPECT(assert_equal(expectedH,actualH));
+
+  actual = x2.norm(actualH);
+  EXPECT_DOUBLES_EQUAL(sqrt(2), actual, 1e-9);
+  expectedH = numericalDerivative11(norm_proxy, x2);
+  EXPECT(assert_equal(expectedH,actualH));
+}
+
+/* ************************************************************************* */
+LieVector distance_proxy(const Point2& location, const Point2& point) {
+  return LieVector(location.distance(point));
+}
+TEST( Point2, distance ) {
+  Matrix expectedH1, actualH1, expectedH2, actualH2;
+
+  // establish distance is indeed zero
+  EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
+
+  // establish distance is indeed 45 degrees
+  EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
+
+  // Another pair
+  double actual23 = x2.distance(l3, actualH1, actualH2);
+  EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
+  expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+
+  // Another test
+  double actual34 = x3.distance(l4, actualH1, actualH2);
+  EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
+
+  // Check numerical derivatives
+  expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
+  expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
+  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
+}
+
+/* ************************************************************************* */
+TEST( Point2, circleCircleIntersection) {
+
+  double offset = 0.994987;
+  // Test intersections of circle moving from inside to outside
+
+  list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
+  EXPECT_LONGS_EQUAL(0,inside.size());
+
+  list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
+  EXPECT_LONGS_EQUAL(1,touching1.size());
+  EXPECT(assert_equal(Point2(5,0), touching1.front()));
+
+  list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
+  EXPECT_LONGS_EQUAL(2,common.size());
+  EXPECT(assert_equal(Point2(4.9,  offset), common.front(), 1e-6));
+  EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
+
+  list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
+  EXPECT_LONGS_EQUAL(1,touching2.size());
+  EXPECT(assert_equal(Point2(5,0), touching2.front()));
+
+  // test rotated case
+  list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
+  EXPECT_LONGS_EQUAL(2,rotated.size());
+  EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
+  EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
+
+  // test r1<r2
+  list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
+  EXPECT_LONGS_EQUAL(2,smaller.size());
+  EXPECT(assert_equal(Point2(0.1,  offset), smaller.front(), 1e-6));
+  EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
+
+  // test offset case, r1>r2
+  list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
+  EXPECT_LONGS_EQUAL(2,offset1.size());
+  EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
+  EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
+
+  // test offset case, r1<r2
+  list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
+  EXPECT_LONGS_EQUAL(2,offset2.size());
+  EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
+  EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
+
+}
+
+/* ************************************************************************* */
+TEST( Point2, stream) {
+  Point2 p(1, 2);
   std::ostringstream os;
   os << p;
   EXPECT(os.str() == "(1, 2)");
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main () {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 

gtsam/geometry/tests/testPose2.cpp

@@ -14,22 +14,21 @@
  * @brief  Unit tests for Pose2 class
  */
 
-#include <cmath>
-#include <iostream>
-
-#include <boost/foreach.hpp>
-#include <boost/optional.hpp>
-#include <boost/assign/std/vector.hpp> // for operator +=
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/lieProxies.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/lieProxies.h>
 
+#include <CppUnitLite/TestHarness.h>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <boost/assign/std/vector.hpp> // for operator +=
+#include <cmath>
+#include <iostream>
+
+using namespace boost::assign;
 using namespace gtsam;
 using namespace std;
 

gtsam/inference/VariableIndex.h

@@ -161,6 +161,16 @@ protected:
   template<class FG> void fill(const FG& factorGraph);
 
   /// @}
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(index_);
+    ar & BOOST_SERIALIZATION_NVP(nFactors_);
+    ar & BOOST_SERIALIZATION_NVP(nEntries_);
+  }
 };
 
 /* ************************************************************************* */

gtsam/linear/CMakeLists.txt

@@ -22,6 +22,11 @@ if (GTSAM_BUILD_TESTS)
     gtsam_add_subdir_tests(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}") 
 endif(GTSAM_BUILD_TESTS)
 
+if(MSVC)
+	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerializationLinear.cpp"
+		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
+endif()
+
 # Build timing scripts
 if (GTSAM_BUILD_TIMING)
     gtsam_add_subdir_timing(linear "${linear_local_libs}" "${gtsam-default}" "${linear_excluded_files}") 

gtsam/linear/GaussianConditional.h

@@ -27,12 +27,12 @@
 #include <gtsam/linear/VectorValues.h>
 
 // Forward declaration to friend unit tests
-class eliminate2JacobianFactorTest;
+class JacobianFactoreliminate2Test;
-class constructorGaussianConditionalTest;
+class GaussianConditionalconstructorTest;
-class eliminationGaussianFactorGraphTest;
+class GaussianFactorGrapheliminationTest;
-class complicatedMarginalGaussianJunctionTreeTest;
+class GaussianJunctionTreecomplicatedMarginalTest;
-class informationGaussianConditionalTest;
+class GaussianConditionalinformationTest;
-class isGaussianFactorGaussianConditionalTest;
+class GaussianConditionalisGaussianFactorTest;
 
 namespace gtsam {
 
@@ -223,12 +223,12 @@ private:
 
   // Friends
   friend class JacobianFactor;
-  friend class ::eliminate2JacobianFactorTest;
+  friend class ::JacobianFactoreliminate2Test;
-  friend class ::constructorGaussianConditionalTest;
+  friend class ::GaussianConditionalconstructorTest;
-  friend class ::eliminationGaussianFactorGraphTest;
+  friend class ::GaussianFactorGrapheliminationTest;
-  friend class ::complicatedMarginalGaussianJunctionTreeTest;
+  friend class ::GaussianJunctionTreecomplicatedMarginalTest;
-  friend class ::informationGaussianConditionalTest;
+  friend class ::GaussianConditionalinformationTest;
-  friend class ::isGaussianFactorGaussianConditionalTest;
+  friend class ::GaussianConditionalisGaussianFactorTest;
 
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/linear/HessianFactor.h

@@ -23,10 +23,10 @@
 #include <gtsam/linear/GaussianFactor.h>
 
 // Forward declarations for friend unit tests
-class ConversionConstructorHessianFactorTest;
+class HessianFactorConversionConstructorTest;
-class Constructor1HessianFactorTest;
+class HessianFactorConstructor1Test;
-class Constructor1bHessianFactorTest;
+class HessianFactorConstructor1bTest;
-class combineHessianFactorTest;
+class HessianFactorcombineTest;
 
 
 namespace gtsam {
@@ -325,10 +325,10 @@ namespace gtsam {
     virtual Matrix information() const;
 
     // Friend unit test classes
-    friend class ::ConversionConstructorHessianFactorTest;
+    friend class ::HessianFactorConversionConstructorTest;
-    friend class ::Constructor1HessianFactorTest;
+    friend class ::HessianFactorConstructor1Test;
-    friend class ::Constructor1bHessianFactorTest;
+    friend class ::HessianFactorConstructor1bTest;
-    friend class ::combineHessianFactorTest;
+    friend class ::HessianFactorcombineTest;
 
     // Friend JacobianFactor for conversion
     friend class JacobianFactor;

gtsam/linear/JacobianFactor.h

@@ -27,9 +27,9 @@
 #include <boost/tuple/tuple.hpp>
 
 // Forward declarations of friend unit tests
-class Combine2JacobianFactorTest;
+class JacobianFactorCombine2Test;
-class eliminateFrontalsJacobianFactorTest;
+class JacobianFactoreliminateFrontalsTest;
-class constructor2JacobianFactorTest;
+class JacobianFactorconstructor2Test;
 
 namespace gtsam {
 
@@ -316,9 +316,9 @@ namespace gtsam {
     friend class HessianFactor;
 
     // Friend unit tests (see also forward declarations above)
-    friend class ::Combine2JacobianFactorTest;
+    friend class ::JacobianFactorCombine2Test;
-    friend class ::eliminateFrontalsJacobianFactorTest;
+    friend class ::JacobianFactoreliminateFrontalsTest;
-    friend class ::constructor2JacobianFactorTest;
+    friend class ::JacobianFactorconstructor2Test;
 
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/nonlinear/ISAM2.cpp

@@ -1043,7 +1043,7 @@ VectorValues optimize(const ISAM2& isam) {
 
 /* ************************************************************************* */
 void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
-  // We may need to update the solution calcaulations
+  // We may need to update the solution calculations
   if(!isam.deltaDoglegUptodate_) {
     gttic(UpdateDoglegDeltas);
     double wildfireThreshold = 0.0;

gtsam/nonlinear/ISAM2.h

@@ -586,7 +586,7 @@ public:
   VALUE calculateEstimate(Key key) const;
 
   /// @name Public members for non-typical usage
-  //@{
+  /// @{
 
   /** Internal implementation functions */
   struct Impl;
@@ -619,7 +619,7 @@ public:
   /** prints out clique statistics */
   GTSAM_EXPORT void printStats() const { getCliqueData().getStats().print(); }
 
-  //@}
+  /// @}
 
 private:
 

gtsam/nonlinear/LinearContainerFactor.h

@@ -26,6 +26,9 @@ protected:
   GaussianFactor::shared_ptr factor_;
   boost::optional<Values> linearizationPoint_;
 
+  /** Default constructor - necessary for serialization */
+  LinearContainerFactor() {}
+
   /** direct copy constructor */
   LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
       const boost::optional<Values>& linearizationPoint);
@@ -148,6 +151,18 @@ protected:
   void rekeyFactor(const Ordering& ordering);
   void initializeLinearizationPoint(const Values& linearizationPoint);
 
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("NonlinearFactor",
+        boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(factor_);
+    ar & BOOST_SERIALIZATION_NVP(linearizationPoint_);
+  }
+
 }; // \class LinearContainerFactor
 
 } // \namespace gtsam

gtsam/nonlinear/WhiteNoiseFactor.h

@@ -24,7 +24,7 @@
 
 namespace gtsam {
 
-  const double logSqrt2PI = log(sqrt(2.0 * M_PI)); ///< constant needed below
+  const double logSqrt2PI = log(std::sqrt(2.0 * M_PI)); ///< constant needed below
 
   /**
    * @brief Binary factor to estimate parameters of zero-mean Gaussian white noise
@@ -32,22 +32,21 @@ namespace gtsam {
    * This factor uses the mean-precision parameterization.
    *
    * Takes three template arguments:
-   * MKEY: key type to use for mean
+   * Key: key type to use for mean
-   * PKEY: key type to use for precision
+   * Key: key type to use for precision
-   * VALUES: Values type for optimization
+   * Values: Values type for optimization
    * \nosubgrouping
    */
-  template<class MKEY, class PKEY, class VALUES>
+  class WhiteNoiseFactor: public NonlinearFactor {
-  class WhiteNoiseFactor: public NonlinearFactor<VALUES> {
 
   private:
 
     double z_; ///< Measurement
 
-    MKEY meanKey_; ///< key by which to access mean variable
+    Key meanKey_; ///< key by which to access mean variable
-    PKEY precisionKey_; ///< key by which to access precision variable
+    Key precisionKey_; ///< key by which to access precision variable
 
-    typedef NonlinearFactor<VALUES> Base;
+    typedef NonlinearFactor Base;
 
   public:
 
@@ -93,7 +92,7 @@ namespace gtsam {
      * @param meanKey Key for mean variable
      * @param precisionKey Key for precision variable
      */
-    WhiteNoiseFactor(double z, const MKEY& meanKey, const PKEY& precisionKey) :
+    WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
         Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
     }
 
@@ -110,8 +109,9 @@ namespace gtsam {
     /// @{
 
     /// Print
-    void print(const std::string& p = "WhiteNoiseFactor") const {
+    void print(const std::string& p = "WhiteNoiseFactor",
-      Base::print(p);
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      Base::print(p, keyFormatter);
       std::cout << p + ".z: " << z_ << std::endl;
     }
 
@@ -125,8 +125,8 @@ namespace gtsam {
     }
 
     /// Calculate the error of the factor, typically equal to log-likelihood
-    inline double error(const VALUES& x) const {
+    inline double error(const Values& x) const {
-      return f(z_, x[meanKey_].value(), x[precisionKey_].value());
+      return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_));
     }
 
     /**
@@ -136,8 +136,8 @@ namespace gtsam {
      * Here we shoehorn sqrt(2*error(p))
      * TODO: Where is this used? should disappear.
      */
-    virtual Vector unwhitenedError(const VALUES& x) const {
+    virtual Vector unwhitenedError(const Values& x) const {
-      return Vector_(1, sqrt(2 * error(x)));
+      return Vector_(1, std::sqrt(2 * error(x)));
     }
 
     /**
@@ -154,19 +154,20 @@ namespace gtsam {
     /// @{
 
     /// linearize returns a Hessianfactor that is an approximation of error(p)
-    virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x,
+    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x,
         const Ordering& ordering) const {
-      double u = x[meanKey_].value();
+      double u = x.at<LieScalar>(meanKey_);
-      double p = x[precisionKey_].value();
+      double p = x.at<LieScalar>(precisionKey_);
       Index j1 = ordering[meanKey_];
       Index j2 = ordering[precisionKey_];
       return linearize(z_, u, p, j1, j2);
     }
 
-    /// @return a deep copy of this factor
+    // TODO: Frank commented this out for now, can it go?
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    //    /// @return a deep copy of this factor
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+    //    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+    //      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+    //          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /// @}
 

gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp

@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testWhiteNoiseFactor.cpp
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/nonlinear/WhiteNoiseFactor.h>
+#include <gtsam/base/Testable.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
+using namespace gtsam;
+using namespace std;
+
+/* ************************************************************************* */
+TEST( WhiteNoiseFactor, constructor )
+{
+  double z = 0.1;
+  Key meanKey=1, precisionKey=2;
+  WhiteNoiseFactor factor(z,meanKey, precisionKey);
+  LONGS_EQUAL(2,factor.dim());
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

gtsam/slam/CMakeLists.txt

@@ -1,5 +1,10 @@
 # Install headers
+set (slam_excluded_headers #"")
+    "${CMAKE_CURRENT_SOURCE_DIR}/serialization.h"
+)
+
 file(GLOB slam_headers "*.h")
+list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
 install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
 
 # Components to link tests in this subfolder against
@@ -15,8 +20,8 @@ set(slam_local_libs
 
 # Files to exclude from compilation of tests and timing scripts
 set(slam_excluded_files
-# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
+ "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"      
-    ""  # Add to this list, with full path, to exclude
+#    ""  # Add to this list, with full path, to exclude
 )
 
 # Build tests

gtsam_unstable/CMakeLists.txt

@@ -14,11 +14,24 @@ set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRA
 
 add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
 
+# To exclude a source from the library build (in any subfolder)
+# Add the full name to this list, as in the following example
+# Sources to remove from builds
+set (excluded_sources # "")
+    "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
+)
+
+set (excluded_headers # "")
+    "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
+)
+
 # assemble core libaries
 foreach(subdir ${gtsam_unstable_subdirs})
     # Build convenience libraries
     file(GLOB subdir_srcs "${subdir}/*.cpp")
 	file(GLOB subdir_headers "${subdir}/*.h")
+    list(REMOVE_ITEM subdir_srcs ${excluded_sources})
+    list(REMOVE_ITEM subdir_headers ${excluded_headers})
     set(${subdir}_srcs ${subdir_srcs} ${subdir_headers})
     gtsam_assign_source_folders("${${subdir}_srcs}") # Create MSVC structure
     if (subdir_srcs AND GTSAM_BUILD_CONVENIENCE_LIBRARIES)

gtsam_unstable/examples/SmartRangeExample_plaza2.cpp

@@ -0,0 +1,210 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SmartRangeExample_plaza2.cpp
+ * @brief A 2D Range SLAM example
+ * @date June 20, 2013
+ * @author FRank Dellaert
+ */
+
+// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
+#include <gtsam/geometry/Pose2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+// Here we will use Symbols
+#include <gtsam/nonlinear/Symbol.h>
+
+// We want to use iSAM2 to solve the range-SLAM problem incrementally
+#include <gtsam/nonlinear/ISAM2.h>
+
+// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
+// and initial guesses for any new variables used in the added factors
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+// We will use a non-liear solver to batch-inituialize from the first 150 frames
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// have been provided with the library for solving robotics SLAM problems.
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/RangeFactor.h>
+
+// Standard headers, added last, so we know headers above work on their own
+#include <boost/foreach.hpp>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+namespace NM = gtsam::noiseModel;
+
+// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+
+// load the odometry
+// DR: Odometry Input (delta distance traveled and delta heading change)
+//    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
+typedef pair<double, Pose2> TimedOdometry;
+list<TimedOdometry> readOdometry() {
+  list<TimedOdometry> odometryList;
+  ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
+  if (!is)
+    throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
+
+  while (is) {
+    double t, distance_traveled, delta_heading;
+    is >> t >> distance_traveled >> delta_heading;
+    odometryList.push_back(
+        TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return odometryList;
+}
+
+// load the ranges from TD
+//    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
+typedef boost::tuple<double, size_t, double> RangeTriple;
+vector<RangeTriple> readTriples() {
+  vector<RangeTriple> triples;
+  ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
+  if (!is)
+    throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
+
+  while (is) {
+    double t, sender, receiver, range;
+    is >> t >> sender >> receiver >> range;
+    triples.push_back(RangeTriple(t, receiver, range));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return triples;
+}
+
+// main
+int main (int argc, char** argv) {
+
+  // load Plaza1 data
+  list<TimedOdometry> odometry = readOdometry();
+//  size_t M = odometry.size();
+
+  vector<RangeTriple> triples = readTriples();
+  size_t K = triples.size();
+
+  // parameters
+  size_t incK = 50; // minimum number of range measurements to process after
+  bool robust = false;
+
+  // Set Noise parameters
+  Vector priorSigmas = Vector3(1,1,M_PI);
+  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
+  double sigmaR = 100; // range standard deviation
+  const NM::Base::shared_ptr // all same type
+  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
+  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
+  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
+  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
+  rangeNoise = robust ? tukey : gaussian;
+
+  // Initialize iSAM
+  ISAM2 isam;
+
+  // Add prior on first pose
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+      M_PI - 2.02108900000000);
+  NonlinearFactorGraph newFactors;
+  newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
+
+  //  initialize points (Gaussian)
+  Values initial;
+  initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
+  initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
+  initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
+  initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
+  Values landmarkEstimates = initial; // copy landmarks
+  initial.insert(0, pose0);
+
+  // set some loop variables
+  size_t i = 1; // step counter
+  size_t k = 0; // range measurement counter
+  Pose2 lastPose = pose0;
+  size_t countK = 0;
+
+  // Loop over odometry
+  gttic_(iSAM);
+  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+    //--------------------------------- odometry loop -----------------------------------------
+    double t;
+    Pose2 odometry;
+    boost::tie(t, odometry) = timedOdometry;
+
+    // add odometry factor
+    newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
+
+    // predict pose and add as initial estimate
+    Pose2 predictedPose = lastPose.compose(odometry);
+    lastPose = predictedPose;
+    initial.insert(i, predictedPose);
+    landmarkEstimates.insert(i, predictedPose);
+
+    // Check if there are range factors to be added
+    while (k < K && t >= boost::get<0>(triples[k])) {
+      size_t j = boost::get<1>(triples[k]);
+      double range = boost::get<2>(triples[k]);
+      RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,rangeNoise);
+      // Throw out obvious outliers based on current landmark estimates
+      Vector error=factor.unwhitenedError(landmarkEstimates);
+      if (k<=200 || fabs(error[0])<5)
+        newFactors.add(factor);
+      k = k + 1;
+      countK = countK + 1;
+    }
+
+    // Check whether to update iSAM 2
+    if (countK > incK) {
+      gttic_(update);
+      isam.update(newFactors, initial);
+      gttoc_(update);
+      gttic_(calculateEstimate);
+      Values result = isam.calculateEstimate();
+      gttoc_(calculateEstimate);
+      lastPose = result.at<Pose2>(i);
+      // update landmark estimates
+      landmarkEstimates = Values();
+      landmarkEstimates.insert(symbol('L', 1), result.at(symbol('L', 1)));
+      landmarkEstimates.insert(symbol('L', 6), result.at(symbol('L', 6)));
+      landmarkEstimates.insert(symbol('L', 0), result.at(symbol('L', 0)));
+      landmarkEstimates.insert(symbol('L', 5), result.at(symbol('L', 5)));
+      newFactors = NonlinearFactorGraph();
+      initial = Values();
+      countK = 0;
+    }
+    i += 1;
+    //--------------------------------- odometry loop -----------------------------------------
+  } // BOOST_FOREACH
+  gttoc_(iSAM);
+
+  // Print timings
+  tictoc_print_();
+
+  // Write result to file
+  Values result = isam.calculateEstimate();
+  ofstream os2("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
+  BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
+    os2 << it.key << "\t" <<  it.value.x() << "\t" <<  it.value.y() << "\t1" << endl;
+  ofstream os("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
+    os << it.key << "\t" <<  it.value.x() << "\t" <<  it.value.y() << "\t" <<  it.value.theta() << endl;
+  exit(0);
+}
+

gtsam_unstable/examples/plotRangeResults.p

@@ -0,0 +1,6 @@
+#!/opt/local/bin/gnuplot
+reset
+#set terminal pdf
+set title "Smart range SLAM result"
+set size ratio 1
+plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles

gtsam_unstable/gtsam_unstable.h

@@ -86,6 +86,8 @@ virtual class PoseRTV : gtsam::Value {
   Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
   gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
   Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
+
+  void serializable() const; // enabling serialization functionality
 };
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
@@ -122,6 +124,8 @@ virtual class Pose3Upright : gtsam::Value {
 
   static gtsam::Pose3Upright Expmap(Vector xi);
   static Vector Logmap(const gtsam::Pose3Upright& p);
+
+  void serializable() const; // enabling serialization functionality
 }; // \class Pose3Upright
 
 #include <gtsam_unstable/geometry/BearingS2.h>
@@ -142,6 +146,8 @@ virtual class BearingS2 : gtsam::Value {
   size_t dim() const;
   gtsam::BearingS2 retract(Vector v) const;
   Vector localCoordinates(const gtsam::BearingS2& p2) const;
+
+  void serializable() const; // enabling serialization functionality
 };
 
 // std::vector<gtsam::Point2>
@@ -288,6 +294,8 @@ class SimPolygon2D {
 template<T = {gtsam::PoseRTV}>
 virtual class PriorFactor : gtsam::NonlinearFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+
+  void serializable() const; // enabling serialization functionality
 };
 
 
@@ -295,6 +303,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor {
 template<T = {gtsam::PoseRTV}>
 virtual class BetweenFactor : gtsam::NonlinearFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
+
+  void serializable() const; // enabling serialization functionality
 };
 
 
@@ -302,6 +312,8 @@ virtual class BetweenFactor : gtsam::NonlinearFactor {
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NonlinearFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+
+  void serializable() const; // enabling serialization functionality
 };
 
 typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
@@ -314,6 +326,8 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
   NonlinearEquality(size_t j, const T& feasible);
   // Constructor - allows inexact evaluation
   NonlinearEquality(size_t j, const T& feasible, double error_gain);
+
+  void serializable() const; // enabling serialization functionality
 };
 
 #include <gtsam_unstable/dynamics/IMUFactor.h>

gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h

@@ -111,7 +111,7 @@ public:
 
 protected:
 
-  friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
+  GTSAM_UNSTABLE_EXPORT friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother);
 
   /** Default constructor */
   ConcurrentSmoother() {};

gtsam_unstable/slam/CMakeLists.txt

@@ -1,5 +1,10 @@
 # Install headers
+set (slam_excluded_headers #"")
+    "${CMAKE_CURRENT_SOURCE_DIR}/serialization.h"
+)
+
 file(GLOB slam_headers "*.h")
+list(REMOVE_ITEM slam_headers ${slam_excluded_headers})
 install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
 
 # Components to link tests in this subfolder against
@@ -19,8 +24,10 @@ set (slam_full_libs
     ${gtsam_unstable-default})
 
 # Exclude tests that don't work
-set (slam_excluded_tests "")
+set (slam_excluded_tests
-
+ "${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"      
+#    ""  # Add to this list, with full path, to exclude
+)
 # Add all tests
 gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}") 
 add_dependencies(check.unstable check.slam_unstable)

gtsam_unstable/slam/Mechanization_bRn2.cpp

@@ -83,14 +83,21 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u,
     const double dt) const {
   // integrate rotation nRb based on gyro measurement u and bias x_g
 
+#ifndef MODEL_NAV_FRAME_ROTATION
   // get body to inertial (measured in b) from gyro, subtract bias
   Vector b_omega_ib = u - x_g_;
 
-  // assume nav to inertial through movement is unknown
+  // nav to inertial due to Earth's rotation and our movement on Earth surface
+  // TODO: figure out how to do this if we need it
   Vector b_omega_in = zero(3);
 
-  // get angular velocity on bRn measured in body frame
+  // calculate angular velocity on bRn measured in body frame
   Vector b_omega_bn = b_omega_in - b_omega_ib;
+#else
+  // Assume a non-rotating nav frame (probably an approximation)
+  // calculate angular velocity on bRn measured in body frame
+  Vector b_omega_bn = x_g_ - u;
+#endif
 
   // convert to navigation frame
   Rot3 nRb = bRn_.inverse();

gtsam_unstable/slam/SmartRangeFactor.h

@@ -0,0 +1,130 @@
+/**
+ * @file SmartRangeFactor.h
+ *
+ * @brief A smart factor for range-only SLAM that does initialization and marginalization
+ * 
+ * @date Sep 10, 2012
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam_unstable/base/dllexport.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/Key.h>
+#include <gtsam/geometry/Pose2.h>
+#include <boost/foreach.hpp>
+#include <map>
+
+namespace gtsam {
+
+/**
+ * Smart factor for range SLAM
+ * @addtogroup SLAM
+ */
+class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor {
+protected:
+
+  struct Circle2 {
+    Circle2(const Point2& p, double r) :
+        center(p), radius(r) {
+    }
+    Point2 center;
+    double radius;
+  };
+
+  /// Range measurements
+  std::vector<double> measurements_;
+
+public:
+
+  /** Default constructor: don't use directly */
+  SmartRangeFactor() {
+  }
+
+  /** standard binary constructor */
+  SmartRangeFactor(const SharedNoiseModel& model) {
+  }
+
+  virtual ~SmartRangeFactor() {
+  }
+
+  /// Add a range measurement to a pose with given key.
+  void addRange(Key key, double measuredRange) {
+    keys_.push_back(key);
+    measurements_.push_back(measuredRange);
+  }
+
+  // Testable
+
+  /** print */
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  }
+
+  /** Check if two factors are equal */
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+    return false;
+  }
+
+  // factor interface
+
+  /**
+   * Triangulate a point from at least three pose-range pairs
+   * Checks for best pair that includes first point
+   */
+  static Point2 triangulate(const std::list<Circle2>& circles) {
+    Circle2 circle1 = circles.front();
+    boost::optional<Point2> best_fh;
+    boost::optional<Circle2> best_circle;
+    BOOST_FOREACH(const Circle2& it, circles) {
+      // distance between circle centers.
+      double d = circle1.center.dist(it.center);
+      if (d < 1e-9)
+        continue;
+      boost::optional<Point2> fh = Point2::CircleCircleIntersection(
+          circle1.radius / d, it.radius / d);
+      if (fh && (!best_fh || fh->y() > best_fh->y())) {
+        best_fh = fh;
+        best_circle = it;
+      }
+    }
+    std::list<Point2> solutions = Point2::CircleCircleIntersection(
+        circle1.center, best_circle->center, best_fh);
+    // TODO, pick winner based on other measurement
+    return solutions.front();
+  }
+
+  /**
+   * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
+   */
+  virtual Vector unwhitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    size_t K = size();
+    Vector errors = zero(K);
+    if (K >= 3) {
+      std::list<Circle2> circles;
+      for (size_t i = 0; i < K; i++) {
+        const Pose2& pose = x.at<Pose2>(keys_[i]);
+        circles.push_back(Circle2(pose.translation(), measurements_[i]));
+      }
+      Point2 optimizedPoint = triangulate(circles);
+      if (H)
+        *H = std::vector<Matrix>();
+      for (size_t i = 0; i < K; i++) {
+        const Pose2& pose = x.at<Pose2>(keys_[i]);
+        if (H) {
+          Matrix Hi;
+          errors[i] = pose.range(optimizedPoint, Hi) - measurements_[i];
+          H->push_back(Hi);
+        } else
+          errors[i] = pose.range(optimizedPoint) - measurements_[i];
+      }
+    }
+    return errors;
+  }
+
+};
+
+} // \namespace gtsam
+

gtsam_unstable/slam/tests/testAHRS.cpp

@@ -5,11 +5,12 @@
  * @author Chris Beall
  */
 
-#include <list>
+#include "../AHRS.h"
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/base/Vector.h>
-#include "../AHRS.h"
+#include <gtsam/base/Testable.h>
 #include <CppUnitLite/TestHarness.h>
+#include <list>
 
 using namespace std;
 using namespace gtsam;
@@ -67,6 +68,21 @@ TEST (AHRS, constructor) {
   AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
 }
 
+/* ************************************************************************* */
+// TODO make a testMechanization_bRn2
+TEST (AHRS, Mechanization_integrate) {
+  AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
+  Mechanization_bRn2 mech;
+  KalmanFilter::State state;
+//  boost::tie(mech,state) = ahrs.initialize(g_e);
+//  Vector u = Vector_(3,0.05,0.0,0.0);
+//  double dt = 2;
+//  Rot3 expected;
+//  Mechanization_bRn2 mech2 = mech.integrate(u,dt);
+//  Rot3 actual = mech2.bRn();
+//  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 /* TODO: currently fails because of problem with ill-conditioned system
 TEST (AHRS, init) {

gtsam_unstable/slam/tests/testSmartRangeFactor.cpp

@@ -0,0 +1,89 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testSmartRangeFactor.cpp
+ *  @brief Unit tests for SmartRangeFactor Class
+ *  @author Frank Dellaert
+ *  @date Nov 2013
+ */
+
+#include <gtsam_unstable/slam/SmartRangeFactor.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1,
+    2.0);
+
+/* ************************************************************************* */
+
+TEST( SmartRangeFactor, constructor ) {
+  SmartRangeFactor f1;
+  LONGS_EQUAL(0, f1.size())
+  SmartRangeFactor f2(gaussian);
+  LONGS_EQUAL(0, f2.size())
+}
+
+/* ************************************************************************* */
+
+TEST( SmartRangeFactor, addRange ) {
+  SmartRangeFactor f(gaussian);
+  f.addRange(1, 10);
+  f.addRange(1, 12);
+  LONGS_EQUAL(2, f.size())
+}
+
+/* ************************************************************************* */
+
+TEST( SmartRangeFactor, unwhitenedError ) {
+  // Test situation:
+  Point2 p(0, 10);
+  Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
+  double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p);
+  DOUBLES_EQUAL(10, r1, 1e-9);
+  DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9);
+  DOUBLES_EQUAL(sqrt(50), r3, 1e-9);
+
+  Values values; // all correct
+  values.insert(1, pose1);
+  values.insert(2, pose2);
+  values.insert(3, pose3);
+
+  SmartRangeFactor f(gaussian);
+  f.addRange(1, r1);
+
+  // Whenever there are two ranges or less, error should be zero
+  Vector actual1 = f.unwhitenedError(values);
+  EXPECT(assert_equal(Vector_(1,0.0), actual1));
+  f.addRange(2, r2);
+  Vector actual2 = f.unwhitenedError(values);
+  EXPECT(assert_equal(Vector2(0,0), actual2));
+
+  f.addRange(3, r3);
+  vector<Matrix> H;
+  Vector actual3 = f.unwhitenedError(values,H);
+  EXPECT(assert_equal(Vector3(0,0,0), actual3));
+
+  // Check keys and Jacobian
+  EXPECT_LONGS_EQUAL(3,f.keys().size());
+  EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front()));
+  EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

matlab/+gtsam/Contents.m

@@ -37,6 +37,7 @@
 % 
 %% Linear Inference
 %   GaussianSequentialSolver          - class GaussianSequentialSolver, see Doxygen page for details
+%   GaussianMultifrontalSolver        - class GaussianMultifrontalSolver, see Doxygen page for details
 %   IterativeOptimizationParameters   - class IterativeOptimizationParameters, see Doxygen page for details
 %   KalmanFilter                      - class KalmanFilter, see Doxygen page for details
 %   SubgraphSolver                    - class SubgraphSolver, see Doxygen page for details

matlab/+gtsam/VisualISAMGenerateData.m

@@ -2,8 +2,9 @@ function [data,truth] = VisualISAMGenerateData(options)
 % VisualISAMGenerateData creates data for viusalSLAM::iSAM examples
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
-%% Generate simulated data
 import gtsam.*
+
+%% Generate simulated data
 if options.triangle % Create a triangle target, just 3 points on a plane
     nrPoints = 3;
     r = 10;
@@ -24,7 +25,6 @@ else % 3D landmarks as vertices of a cube
 end
 
 %% Create camera cameras on a circle around the triangle
-import gtsam.*
 height = 10; r = 40;
 truth.K = Cal3_S2(500,500,0,640/2,480/2);
 data.K = truth.K;

matlab/+gtsam/VisualISAMInitialize.m

@@ -2,16 +2,16 @@ function [noiseModels,isam,result,nextPoseIndex] = VisualISAMInitialize(data,tru
 % VisualISAMInitialize initializes visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
 
-%% Initialize iSAM
 import gtsam.*
+
+%% Initialize iSAM
 params = gtsam.ISAM2Params;
 if options.alwaysRelinearize
     params.setRelinearizeSkip(1);
 end
-isam = ISAM2;
+isam = ISAM2(params);
 
 %% Set Noise parameters
-import gtsam.*
 noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
 %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
 noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]');
@@ -20,7 +20,6 @@ noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
 
 %% Add constraints/priors
 % TODO: should not be from ground truth!
-import gtsam.*
 newFactors = NonlinearFactorGraph;
 initialEstimates = Values;
 for i=1:2
@@ -38,7 +37,6 @@ end
 nextPoseIndex = 3;
 
 %% Add visual measurement factors from two first poses and initialize observed landmarks
-import gtsam.*
 for i=1:2
     ii = symbol('x',i);
     for k=1:length(data.Z{i})
@@ -56,11 +54,9 @@ for i=1:2
 end
 
 %% Add odometry between frames 1 and 2
-import gtsam.*
 newFactors.add(BetweenFactorPose3(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry));
 
 %% Update ISAM
-import gtsam.*
 if options.batchInitialization % Do a full optimize for first two poses
     batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initialEstimates);
     fullyOptimized = batchOptimizer.optimize();

matlab/+gtsam/VisualISAMPlot.m

@@ -2,6 +2,7 @@ function VisualISAMPlot(truth, data, isam, result, options)
 % VisualISAMPlot plots current state of ISAM2 object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
+import gtsam.*
 h=gca;
 cla(h);
 hold on;
@@ -12,7 +13,6 @@ marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO
 gtsam.plot3DPoints(result, [], marginals);
 
 %% Plot cameras
-import gtsam.*
 M = 1;
 while result.exists(symbol('x',M))
     ii = symbol('x',M);

matlab/+gtsam/VisualISAMStep.m

@@ -2,19 +2,18 @@ function [isam,result,nextPoseIndex] = VisualISAMStep(data,noiseModels,isam,resu
 % VisualISAMStep executes one update step of visualSLAM::iSAM object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
+import gtsam.*
+
 % iSAM expects us to give it a new set of factors 
 % along with initial estimates for any new variables introduced.
-import gtsam.*
 newFactors = NonlinearFactorGraph;
 initialEstimates = Values;
 
 %% Add odometry
-import gtsam.*
 odometry = data.odometry{nextPoseIndex-1};
 newFactors.add(BetweenFactorPose3(symbol('x',nextPoseIndex-1), symbol('x',nextPoseIndex), odometry, noiseModels.odometry));
 
 %% Add visual measurement factors and initializations as necessary
-import gtsam.*
 for k=1:length(data.Z{nextPoseIndex})
     zij = data.Z{nextPoseIndex}{k};
     j = data.J{nextPoseIndex}{k};
@@ -28,7 +27,6 @@ for k=1:length(data.Z{nextPoseIndex})
 end
 
 %% Initial estimates for the new pose.
-import gtsam.*
 prevPose = result.at(symbol('x',nextPoseIndex-1));
 initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
 

matlab/CMakeLists.txt

@@ -19,6 +19,7 @@ install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINA
 message(STATUS "Installing Matlab Toolbox Examples (Data)")
 # Data files: *.graph and *.txt
 file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
+file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat")
 file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
-set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt}) 
+set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) 
 install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)

matlab/gtsam_examples/RangeISAMExample_plaza.m

@@ -0,0 +1,195 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Read Robotics Institute range-only Plaza2 dataset and do iSAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% preliminaries
+clear
+import gtsam.*
+
+%% Find and load data file
+% data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+% Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+% GT: Groundtruth path from GPS
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% DR: Odometry Input (delta distance traveled and delta heading change)
+%    Time (sec)	Delta Dist. Trav. (m)	Delta Heading (rad)
+% DRp: Dead Reckoned Path from Odometry
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% TL: Surveyed Node Locations
+%    Time (sec)	X_pose (m)	Y_pose (m)
+% TD
+%    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
+if false % switch between data files
+  datafile = findExampleDataFile('Plaza1_.mat');
+  headingOffset=0;
+  minK=200; % minimum number of range measurements to process initially
+  incK=5; % minimum number of range measurements to process after
+else
+  datafile = findExampleDataFile('Plaza2_.mat');
+  headingOffset=pi;
+  minK=150; % needs less for init
+  incK=25; % minimum number of range measurements to process after
+end
+load(datafile)
+M=size(DR,1);
+K=size(TD,1);
+sigmaR = 100; % range standard deviation
+sigmaInitial = 1; % draw initial landmark guess from Gaussian
+useGroundTruth = false;
+useRobust=true;
+addRange=true;
+batchInitialization=true;
+
+%% Set Noise parameters
+noiseModels.prior = noiseModel.Diagonal.Sigmas([1 1 pi]');
+noiseModels.pointPrior = noiseModel.Diagonal.Sigmas([1 1]');
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.01 0.2]');
+if useRobust
+  base = noiseModel.mEstimator.Tukey(15);
+  noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
+else
+  noiseModels.range = noiseModel.Isotropic.Sigma(1, sigmaR);
+end
+
+%% Initialize iSAM
+isam = ISAM2;
+
+%% Add prior on first pose
+pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4));
+newFactors = NonlinearFactorGraph;
+if ~addRange | ~useGroundTruth
+  newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
+end
+initial = Values;
+initial.insert(0,pose0);
+odo = Values;
+odo.insert(0,pose0);
+
+%% initialize points
+if addRange
+  landmarkEstimates = Values;
+  for i=1:size(TL,1)
+    j=TL(i,1);
+    if useGroundTruth
+      Lj = Point2(TL(i,2),TL(i,3));
+      newFactors.add(PriorFactorPoint2(symbol('L',j),Lj,noiseModels.pointPrior));
+    else
+      Lj = Point2(sigmaInitial*randn,sigmaInitial*randn);
+    end
+    initial.insert(symbol('L',j),Lj);
+    landmarkEstimates.insert(symbol('L',j),Lj);
+  end
+  XY = utilities.extractPoint2(initial);
+  plot(XY(:,1),XY(:,2),'g*');
+end
+
+%% Loop over odometry
+tic
+k = 1; % range measurement counter
+update = false;
+lastPose = pose0;
+odoPose = pose0;
+countK = 0;
+for i=1:M % M
+  
+  % get odometry measurement
+  t = DR(i,1);
+  distance_traveled = DR(i,2);
+  delta_heading = DR(i,3);
+  
+  % add odometry factor
+  odometry = Pose2(distance_traveled,0,delta_heading);
+  newFactors.add(BetweenFactorPose2(i-1, i, odometry, noiseModels.odometry));
+  
+  % predict pose and update odometry
+  predictedOdo = odoPose.compose(odometry);
+  odoPose = predictedOdo;
+  odo.insert(i,predictedOdo);
+  
+  % predict pose and add as initial estimate
+  predictedPose = lastPose.compose(odometry);
+  lastPose = predictedPose;
+  initial.insert(i,predictedPose);
+  landmarkEstimates.insert(i,predictedPose);
+  
+  % Check if there are range factors to be added
+  while k<=K && t>=TD(k,1)
+    j = TD(k,3);
+    range = TD(k,4);
+    if addRange
+      factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+      % Throw out obvious outliers based on current landmark estimates
+      error=factor.unwhitenedError(landmarkEstimates);
+      if k<=minK || abs(error)<5
+        newFactors.add(factor);
+      end
+    end
+    k=k+1; countK=countK+1; update = true;
+  end
+
+  % Check whether to update iSAM 2
+  if update && k>minK && countK>incK
+    if batchInitialization % Do a full optimize for first minK ranges
+      tic
+      batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initial);
+      initial = batchOptimizer.optimize();
+      toc
+      batchInitialization = false; % only once
+    end
+    isam.update(newFactors, initial);
+    result = isam.calculateEstimate();
+    lastPose = result.at(i);
+    % update landmark estimates
+    if addRange
+      landmarkEstimates = Values;
+      for jj=1:size(TL,1)
+        j=TL(jj,1);
+        key = symbol('L',j);
+        landmarkEstimates.insert(key,result.at(key));
+      end
+    end
+    newFactors = NonlinearFactorGraph;
+    initial = Values;
+    countK = 0;
+  end
+  
+  % visualize
+  if mod(i,50)==0 && k>minK
+    figure(1);clf;hold on
+    
+    % odometry
+    XYT = utilities.extractPose2(odo);
+    plot(XYT(:,1),XYT(:,2),'y-');
+    
+    % lin point
+    lin = isam.getLinearizationPoint();
+    XYT = utilities.extractPose2(lin);
+    plot(XYT(:,1),XYT(:,2),'r.');
+    XY = utilities.extractPoint2(lin);
+    plot(XY(:,1),XY(:,2),'r*');
+    
+    % result
+    result = isam.calculateEstimate();
+    XYT = utilities.extractPose2(result);
+    plot(XYT(:,1),XYT(:,2),'k-');
+    XY = utilities.extractPoint2(result);
+    plot(XY(:,1),XY(:,2),'k*');
+    axis equal
+    %     pause
+  end
+end
+toc
+
+%% Plot ground truth as well
+plot(GT(:,2),GT(:,3),'g-');
+plot(TL(:,2),TL(:,3),'g*');
+
+

matlab/gtsam_examples/RangeSLAMExample_plaza.m

@@ -0,0 +1,109 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Read Robotics Institute range-only Plaza2 dataset and do SAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% preliminaries
+clear
+import gtsam.*
+
+%% Find and load data file
+% data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+% Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+% GT: Groundtruth path from GPS
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% DR: Odometry Input (delta distance traveled and delta heading change)
+%    Time (sec)	Delta Dist. Trav. (m)	Delta Heading (rad)
+% DRp: Dead Reckoned Path from Odometry
+%    Time (sec)	X_pose (m)	Y_pose (m)	Heading (rad)
+% TL: Surveyed Node Locations
+%    Time (sec)	X_pose (m)	Y_pose (m)
+% TD
+%    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
+datafile = findExampleDataFile('Plaza1_.mat');
+load(datafile)
+M=size(DR,1);
+K=size(TD,1);
+sigmaR = 50; % range standard deviation
+sigmaInitial = 1; % draw initial landmark guess from Gaussian
+
+%% Set Noise parameters
+noiseModels.prior = noiseModel.Diagonal.Sigmas([1 1 pi]');
+noiseModels.pointPrior = noiseModel.Diagonal.Sigmas([1 1]');
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.01 0.2]');
+base = noiseModel.mEstimator.Tukey(5);
+noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
+
+%% Add prior on first pose
+pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4));
+graph = NonlinearFactorGraph;
+graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
+initial = Values;
+initial.insert(0,pose0);
+
+for i=1:size(TL,1)
+  j=TL(i,1);
+  initial.insert(symbol('L',j),Point2(sigmaInitial*randn,sigmaInitial*randn));
+end
+
+%% Loop over odometry
+tic
+k = 1; % range measurement counter
+lastPose = pose0;
+for i=1:M
+  
+  % get odometry measurement
+  t = DR(i,1);
+  distance_traveled = DR(i,2);
+  delta_heading = DR(i,3);
+  
+  % add odometry factor
+  odometry = Pose2(distance_traveled,0,delta_heading);
+  graph.add(BetweenFactorPose2(i-1, i, odometry, noiseModels.odometry));
+  
+  % predict pose and add as initial estimate
+  predictedPose = lastPose.compose(odometry);
+  lastPose = predictedPose;
+  initial.insert(i,predictedPose);
+  
+  while k<=K && t>=TD(k,1)
+    j = TD(k,3);
+    range = TD(k,4);
+    factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+    graph.add(factor);
+    k=k+1;
+  end
+  
+end
+toc
+
+%% GRaph was built, optimize !
+tic
+batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
+result = batchOptimizer.optimize();
+toc
+
+%% visualize
+figure(1);clf;hold on
+
+% odometry
+XYT = utilities.extractPose2(initial);
+plot(XYT(:,1),XYT(:,2),'y-');
+
+% GT
+plot(GT(:,2),GT(:,3),'g-');
+plot(TL(:,2),TL(:,3),'g*');
+
+% result
+XYT = utilities.extractPose2(result);
+plot(XYT(:,1),XYT(:,2),'k-');
+XY = utilities.extractPoint2(result);
+plot(XY(:,1),XY(:,2),'k*');
+axis equal

matlab/gtsam_tests/testGraphValuesSerialization.m

@@ -1,65 +0,0 @@
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% 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
-%
-% @brief Simple robotics example using the pre-built planar SLAM domain
-% @author Alex Cunningham
-% @author Frank Dellaert
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-import gtsam.*
-
-%% Create keys for variables
-i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
-j1 = symbol('l',1); j2 = symbol('l',2);
-
-%% Create graph and factors
-graph = NonlinearFactorGraph;
-
-% Prior factor - FAIL: unregistered class
-priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
-
-% Between Factors - FAIL: unregistered class
-odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
-graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
-
-% BearingRange Factors - FAIL: unregistered class
-degrees = pi/180;
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
-graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
-graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
-graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
-
-%% Create Values
-values = Values;
-values.insert(i1, Pose2(0.5, 0.0, 0.2));
-values.insert(i2, Pose2(2.3, 0.1,-0.2));
-values.insert(i3, Pose2(4.1, 0.1, 0.1));
-values.insert(j1, Point2(1.8, 2.1));
-values.insert(j2, Point2(4.1, 1.8));
-
-%% Check that serialization works properly
-serialized_values = serializeValues(values); % returns a string
-deserializedValues = deserializeValues(serialized_values); % returns a new values
-CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9));
-
-CHECK('serializeValuesToFile(values, values.dat)', serializeValuesToFile(values, 'values.dat')); 
-deserializedValuesFile = deserializeValuesFromFile('values.dat'); % returns a new values
-CHECK('values.equals(deserializedValuesFile)',values.equals(deserializedValuesFile,1e-9));
-
-% % FAIL: unregistered class - derived class not registered or exported
-% serialized_graph = serializeGraph(graph); % returns a string
-% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph
-% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9));
-%
-% CHECK('serializeGraphToFile(graph, graph.dat)', serializeGraphToFile(graph, 'graph.dat')); 
-% deserializedGraphFile = deserializeGraphFromFile('graph.dat'); % returns a new graph
-% CHECK('graph.equals(deserializedGraphFile)',graph.equals(deserializedGraphFile,1e-9));

matlab/gtsam_tests/testSerialization.m

@@ -0,0 +1,70 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+%
+% @brief Checks for serialization using basic string interface
+% @author Alex Cunningham
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+import gtsam.*
+
+%% Create keys for variables
+i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
+j1 = symbol('l',1); j2 = symbol('l',2);
+
+%% Create values and verify string serialization
+pose1=Pose2(0.5, 0.0, 0.2);
+pose2=Pose2(2.3, 0.1,-0.2);
+pose3=Pose2(4.1, 0.1, 0.1);
+landmark1=Point2(1.8, 2.1);
+landmark2=Point2(4.1, 1.8);
+
+serialized_pose1 = pose1.string_serialize();
+pose1ds = Pose2.string_deserialize(serialized_pose1);
+CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9));
+
+serialized_landmark1 = landmark1.string_serialize();
+landmark1ds = Point2.string_deserialize(serialized_landmark1);
+CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9));
+
+%% Create and serialize Values
+values = Values;
+values.insert(i1, pose1);
+values.insert(i2, pose2);
+values.insert(i3, pose3);
+values.insert(j1, landmark1);
+values.insert(j2, landmark2);
+
+serialized_values = values.string_serialize();
+valuesds = Values.string_deserialize(serialized_values);
+CHECK('valuesds.equals(values, 1e-9)', valuesds.equals(values, 1e-9));
+
+%% Create graph and factors and serialize
+graph = NonlinearFactorGraph;
+
+% Prior factor
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
+
+% Between Factors - FAIL: unregistered class
+odometry = Pose2(2.0, 0.0, 0.0);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
+graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
+ 
+% BearingRange Factors - FAIL: unregistered class
+degrees = pi/180;
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
+graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
+graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
+
+serialized_graph = graph.string_serialize();
+graphds = NonlinearFactorGraph.string_deserialize(serialized_graph);
+CHECK('graphds.equals(graph, 1e-9)', graphds.equals(graph, 1e-9));

matlab/gtsam_tests/test_gtsam.m

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

tests/CMakeLists.txt

@@ -22,10 +22,6 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Rich
 	list (APPEND tests_exclude "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp") 
 endif()
 
-if(MSVC)
-  add_definitions("/bigobj") # testSerializationSLAM needs this
-endif()
-
 # Build tests
 if (GTSAM_BUILD_TESTS)
     # Subdirectory target for tests
@@ -34,7 +30,7 @@ if (GTSAM_BUILD_TESTS)
 
     # Build grouped tests
     gtsam_add_grouped_scripts("tests"               # Use subdirectory as group label
-    "test*.cpp" check "Test"                      # Standard for all tests
+    "test*.cpp;*.h" check "Test"                      # Standard for all tests
     "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}"  # Pass in linking and exclusion lists
     ${is_test})                                         # Set all as tests
 endif (GTSAM_BUILD_TESTS)
@@ -51,3 +47,8 @@ if (GTSAM_BUILD_TIMING)
     "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}"   # Pass in linking and exclusion lists
     ${is_test})
 endif (GTSAM_BUILD_TIMING)
+
+if(MSVC)
+	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp"
+		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
+endif()

tests/simulated2D.h

@@ -83,42 +83,43 @@ namespace simulated2D {
     }
   };
 
+  namespace {
+    /// Prior on a single pose
+    inline Point2 prior(const Point2& x) {
+      return x;
+    }
 
-  /// Prior on a single pose
+    /// Prior on a single pose, optionally returns derivative
-  inline Point2 prior(const Point2& x) {
+    Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
-    return x;
+      if (H) *H = gtsam::eye(2);
-  }
+      return x;
+    }
 
-  /// Prior on a single pose, optionally returns derivative
+    /// odometry between two poses
-  Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
+    inline Point2 odo(const Point2& x1, const Point2& x2) {
-    if (H) *H = gtsam::eye(2);
+      return x2 - x1;
-    return x;
+    }
-  }
 
-  /// odometry between two poses
+    /// odometry between two poses, optionally returns derivative
-  inline Point2 odo(const Point2& x1, const Point2& x2) {
+    Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
-    return x2 - x1;
-  }
-
-  /// odometry between two poses, optionally returns derivative
-  Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
       boost::none, boost::optional<Matrix&> H2 = boost::none) {
-    if (H1) *H1 = -gtsam::eye(2);
+        if (H1) *H1 = -gtsam::eye(2);
-    if (H2) *H2 = gtsam::eye(2);
+        if (H2) *H2 = gtsam::eye(2);
-    return x2 - x1;
+        return x2 - x1;
-  }
+    }
 
-  /// measurement between landmark and pose
+    /// measurement between landmark and pose
-  inline Point2 mea(const Point2& x, const Point2& l) {
+    inline Point2 mea(const Point2& x, const Point2& l) {
-    return l - x;
+      return l - x;
-  }
+    }
 
-  /// measurement between landmark and pose, optionally returns derivative
+    /// measurement between landmark and pose, optionally returns derivative
-  Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
+    Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
       boost::none, boost::optional<Matrix&> H2 = boost::none) {
-    if (H1) *H1 = -gtsam::eye(2);
+        if (H1) *H1 = -gtsam::eye(2);
-    if (H2) *H2 = gtsam::eye(2);
+        if (H2) *H2 = gtsam::eye(2);
-    return l - x;
+        return l - x;
+    }
   }
 
   /**

tests/smallExample.h

@@ -28,6 +28,7 @@
 
 namespace gtsam {
 namespace example {
+  namespace {
 
 typedef NonlinearFactorGraph Graph;
 
@@ -150,13 +151,9 @@ Ordering planarOrdering(size_t N);
 std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
     size_t N, const GaussianFactorGraph& original);
 
-} // example
-} // gtsam
 
 
 // Implementations
-namespace gtsam {
-namespace example {
 
 //  using namespace gtsam::noiseModel;
 
@@ -649,5 +646,6 @@ std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N
 
 /* ************************************************************************* */
 
+} // anonymous namespace
 } // \namespace example
 } // \namespace gtsam

wrap/Class.cpp

@@ -107,10 +107,12 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
   // Methods 
   BOOST_FOREACH(const Methods::value_type& name_m, methods) { 
     const Method& m = name_m.second; 
-    m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); 
+    m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames);
     proxyFile.oss << "\n"; 
     wrapperFile.oss << "\n"; 
   } 
+  if (hasSerialization)
+    serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
  
   proxyFile.oss << "  end\n"; 
   proxyFile.oss << "\n"; 
@@ -123,7 +125,9 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
     proxyFile.oss << "\n"; 
     wrapperFile.oss << "\n"; 
   } 
- 
+  if (hasSerialization)
+    deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
+
   proxyFile.oss << "  end\n";
   proxyFile.oss << "end\n";
  
@@ -278,6 +282,7 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec
   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); 
@@ -388,6 +393,169 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
     }
   }
 
+  if (hasSerialization) {
+    proxyFile.oss << "%\n%-------Serialization Interface-------\n";
+    proxyFile.oss << "%string_serialize() : returns string\n";
+    proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n";
+  }
+
   proxyFile.oss << "%\n";
 }
+
 /* ************************************************************************* */ 
+void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
+    const std::string& wrapperName, std::vector<std::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;
+//  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 = qualifiedName(),
+    cppClassName = qualifiedName("::");
+  const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
+  functionNames.push_back(wrapFunctionNameSerialize);
+
+  // call
+  //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+  wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+  wrapperFile.oss << "{\n";
+  wrapperFile.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
+
+  // check arguments - for serialize, no arguments
+  // example: checkArguments("string_serialize",nargout,nargin-1,0);
+  wrapperFile.oss << "  checkArguments(\"string_serialize\",nargout,nargin-1,0);\n";
+
+  // get class pointer
+  // example: Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
+  wrapperFile.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
+
+  // Serialization boilerplate
+  wrapperFile.oss << "  std::ostringstream out_archive_stream;\n";
+  wrapperFile.oss << "  boost::archive::text_oarchive out_archive(out_archive_stream);\n";
+  wrapperFile.oss << "  out_archive << *obj;\n";
+  wrapperFile.oss << "  out[0] = wrap< string >(out_archive_stream.str());\n";
+
+  // finish
+  wrapperFile.oss << "}\n";
+
+  // Generate code for matlab function
+//  function varargout string_serialize(this, varargin)
+//  % STRING_SERIALIZE usage: string_serialize() : returns string
+//  % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+//    if length(varargin) == 0
+//      varargout{1} = geometry_wrapper(15, this, varargin{:});
+//    else
+//      error('Arguments do not match any overload of function Point3.string_serialize');
+//    end
+//  end
+
+  proxyFile.oss << "    function varargout = string_serialize(this, varargin)\n";
+  proxyFile.oss << "      % STRING_SERIALIZE usage: string_serialize() : returns string\n";
+  proxyFile.oss << "      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
+  proxyFile.oss << "      if length(varargin) == 0\n";
+  proxyFile.oss << "        varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(serialize_id) << ", this, varargin{:});\n";
+  proxyFile.oss << "      else\n";
+  proxyFile.oss << "        error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n";
+  proxyFile.oss << "      end\n";
+  proxyFile.oss << "    end\n\n";
+
+  // Generate code for matlab save function
+//  function sobj = saveobj(obj)
+//    % SAVEOBJ Saves the object to a matlab-readable format
+//    sobj = obj.string_serialize();
+//  end
+
+  proxyFile.oss << "    function sobj = saveobj(obj)\n";
+  proxyFile.oss << "      % SAVEOBJ Saves the object to a matlab-readable format\n";
+  proxyFile.oss << "      sobj = obj.string_serialize();\n";
+}
+
+/* ************************************************************************* */
+void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
+    const std::string& wrapperName, std::vector<std::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);
+  //  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 = qualifiedName(),
+      cppClassName = qualifiedName("::");
+    const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
+    functionNames.push_back(wrapFunctionNameDeserialize);
+
+    // call
+    wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
+    wrapperFile.oss << "{\n";
+    wrapperFile.oss << "  typedef boost::shared_ptr<"  << cppClassName  << "> Shared;" << endl;
+
+    // check arguments - for deserialize, 1 string argument
+    wrapperFile.oss << "  checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n";
+
+    // 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 << "  boost::archive::text_iarchive in_archive(in_archive_stream);\n";
+    wrapperFile.oss << "  Shared output(new " << cppClassName << "());\n";
+    wrapperFile.oss << "  in_archive >> *output;\n";
+    wrapperFile.oss << "  out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n";
+    wrapperFile.oss << "}\n";
+
+    // Generate matlab function
+//    function varargout = string_deserialize(varargin)
+//    % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
+//    % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+//      if length(varargin) == 1
+//        varargout{1} = geometry_wrapper(18, varargin{:});
+//      else
+//        error('Arguments do not match any overload of function Point3.string_deserialize');
+//      end
+//    end
+
+    proxyFile.oss << "    function varargout = string_deserialize(varargin)\n";
+    proxyFile.oss << "      % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n";
+    proxyFile.oss << "      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
+    proxyFile.oss << "      if length(varargin) == 1\n";
+    proxyFile.oss << "        varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(deserialize_id) << ", varargin{:});\n";
+    proxyFile.oss << "      else\n";
+    proxyFile.oss << "        error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n";
+    proxyFile.oss << "      end\n";
+    proxyFile.oss << "    end\n\n";
+
+
+    // Generate matlab load function
+//    function obj = loadobj(sobj)
+//      % LOADOBJ Saves the object to a matlab-readable format
+//      obj = Point3.string_deserialize(sobj);
+//    end
+
+    proxyFile.oss << "    function obj = loadobj(sobj)\n";
+    proxyFile.oss << "      % LOADOBJ Saves the object to a matlab-readable format\n";
+    proxyFile.oss << "      obj = " << matlabQualName << ".string_deserialize(sobj);\n";
+    proxyFile.oss << "    end" << endl;
+}
+
+/* ************************************************************************* */
+std::string Class::getSerializationExport() const {
+  //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
+  return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");";
+}
+/* ************************************************************************* */

wrap/Class.h

@@ -37,13 +37,15 @@ struct Class {
   typedef std::map<std::string, StaticMethod> StaticMethods;
 
   /// Constructor creates an empty class
-  Class(bool verbose=true) : isVirtual(false), verbose_(verbose) {}
+  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
   Methods methods;                          ///< Class methods
   StaticMethods static_methods;             ///< Static methods
@@ -55,14 +57,26 @@ struct Class {
   // 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,
     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
 
   std::vector<Class> expandTemplate(const std::string& templateArg, const std::vector<std::vector<std::string> >& 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;
 
   // 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
+  std::string getSerializationExport() const;
+
+  // Creates a member function that performs serialization
+  void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
+      const std::string& wrapperName, std::vector<std::string>& functionNames) const;
+
+  // 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;
 
 private:
   void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector<std::string>& functionNames) const;

wrap/Module.cpp

@@ -378,13 +378,28 @@ void Module::parseMarkup(const std::string& data) {
     throw ParseFailed((int)info.length);
   }
 
-  //Explicitly add methods to the classes from parents so it shows in documentation
+  // Post-process classes for serialization markers
+  BOOST_FOREACH(Class& cls, classes) {
+    Class::Methods::iterator serializable_it = cls.methods.find("serializable");
+    if (serializable_it != cls.methods.end()) {
+      cls.isSerializable = true;
+      cls.methods.erase(serializable_it);
+    }
+
+    Class::Methods::iterator serialize_it = cls.methods.find("serialize");
+    if (serialize_it != cls.methods.end()) {
+      cls.isSerializable = true;
+      cls.hasSerialization= true;
+      cls.methods.erase(serialize_it);
+    }
+  }
+
+  // Explicitly add methods to the classes from parents so it shows in documentation
   BOOST_FOREACH(Class& cls, classes)
   {
     map<string, Method> inhereted = appendInheretedMethods(cls, classes);
     cls.methods.insert(inhereted.begin(), inhereted.end());
   }
-
 } 
  
 /* ************************************************************************* */ 
@@ -440,16 +455,6 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 
   fs::create_directories(toolboxPath);
 
-  // create the unified .cpp switch file
-  const string wrapperName = name + "_wrapper";
-  string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
-  FileWriter wrapperFile(wrapperFileName, verbose, "//");
-  vector<string> functionNames; // Function names stored by index for switch
-  wrapperFile.oss << "#include <wrap/matlab.h>\n";
-  wrapperFile.oss << "#include <map>\n";
-  wrapperFile.oss << "#include <boost/foreach.hpp>\n";
-  wrapperFile.oss << "\n";
-
   // 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.
@@ -462,7 +467,9 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
   verifyArguments<GlobalFunction>(validTypes, global_functions);
   verifyReturnTypes<GlobalFunction>(validTypes, global_functions);
 
+  bool hasSerialiable = false;
   BOOST_FOREACH(const Class& cls, expandedClasses) {
+    hasSerialiable |= cls.isSerializable;
     // verify all of the function arguments
     //TODO:verifyArguments<ArgumentList>(validTypes, cls.constructor.args_list);
     verifyArguments<StaticMethod>(validTypes, cls.static_methods);
@@ -475,7 +482,6 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
     // 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
@@ -484,6 +490,22 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
   typeAttributes.addForwardDeclarations(forward_declarations);
   typeAttributes.checkValidity(expandedClasses);
 
+  // create the unified .cpp switch file
+  const string wrapperName = name + "_wrapper";
+  string wrapperFileName = toolboxPath + "/" + wrapperName + ".cpp";
+  FileWriter wrapperFile(wrapperFileName, verbose, "//");
+  wrapperFile.oss << "#include <wrap/matlab.h>\n";
+  wrapperFile.oss << "#include <map>\n";
+  wrapperFile.oss << "#include <boost/foreach.hpp>\n";
+  wrapperFile.oss << "\n";
+
+  // Include boost.serialization archive headers before other class headers
+  if (hasSerialiable) {
+    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n";
+    wrapperFile.oss << "#include <boost/archive/text_iarchive.hpp>\n";
+    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n\n";
+  }
+
   // Generate includes while avoiding redundant includes
   generateIncludes(wrapperFile);
 
@@ -494,12 +516,23 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
   }
   wrapperFile.oss << "\n";
 
+  // Generate boost.serialization export flags (needs typedefs from above)
+  if (hasSerialiable) {
+    BOOST_FOREACH(const Class& cls, expandedClasses) {
+      if(cls.isSerializable)
+        wrapperFile.oss << cls.getSerializationExport() << "\n";
+    }
+    wrapperFile.oss << "\n";
+  }
+
   // Generate collectors and cleanup function to be called from mexAtExit
   WriteCollectorsAndCleanupFcn(wrapperFile, name, expandedClasses);
 
   // generate RTTI registry (for returning derived-most types)
   WriteRTTIRegistry(wrapperFile, name, expandedClasses);
 
+  vector<string> functionNames; // Function names stored by index for switch
+
   // create proxy class and wrapper code
   BOOST_FOREACH(const Class& cls, expandedClasses) {
     cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);

wrap/tests/expected/Point3.m

@@ -11,6 +11,10 @@
 %StaticFunctionRet(double z) : returns Point3
 %staticFunction() : returns double
 %
+%-------Serialization Interface-------
+%string_serialize() : returns string
+%string_deserialize(string serialized) : returns Point3
+%
 classdef Point3 < handle
   properties
     ptr_Point3 = 0
@@ -49,6 +53,19 @@ classdef Point3 < handle
       end
     end
 
+    function varargout = string_serialize(this, varargin)
+      % STRING_SERIALIZE usage: string_serialize() : returns string
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 0
+        varargout{1} = geometry_wrapper(15, this, varargin{:});
+      else
+        error('Arguments do not match any overload of function Point3.string_serialize');
+      end
+    end
+
+    function sobj = saveobj(obj)
+      % SAVEOBJ Saves the object to a matlab-readable format
+      sobj = obj.string_serialize();
   end
 
   methods(Static = true)
@@ -59,7 +76,7 @@ classdef Point3 < handle
       % Usage
       % STATICFUNCTIONRET(double z)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(15, varargin{:});
+        varargout{1} = geometry_wrapper(16, varargin{:});
       else
         error('Arguments do not match any overload of function Point3.StaticFunctionRet');
       end
@@ -72,11 +89,25 @@ classdef Point3 < handle
       % Usage
       % STATICFUNCTION()
       if length(varargin) == 0
-        varargout{1} = geometry_wrapper(16, varargin{:});
+        varargout{1} = geometry_wrapper(17, varargin{:});
       else
         error('Arguments do not match any overload of function Point3.StaticFunction');
       end
     end
 
+    function varargout = string_deserialize(varargin)
+      % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
+      % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
+      if length(varargin) == 1
+        varargout{1} = geometry_wrapper(18, varargin{:});
+      else
+        error('Arguments do not match any overload of function Point3.string_deserialize');
+      end
+    end
+
+    function obj = loadobj(sobj)
+      % LOADOBJ Saves the object to a matlab-readable format
+      obj = Point3.string_deserialize(sobj);
+    end
   end
 end

wrap/tests/expected/Test.m

@@ -34,11 +34,11 @@ classdef Test < handle
     function obj = Test(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
-        geometry_wrapper(17, my_ptr);
+        geometry_wrapper(19, my_ptr);
       elseif nargin == 0
-        my_ptr = geometry_wrapper(18);
+        my_ptr = geometry_wrapper(20);
       elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        my_ptr = geometry_wrapper(19, varargin{1}, varargin{2});
+        my_ptr = geometry_wrapper(21, varargin{1}, varargin{2});
       else
         error('Arguments do not match any overload of Test constructor');
       end
@@ -46,7 +46,7 @@ classdef Test < handle
     end
 
     function delete(obj)
-      geometry_wrapper(20, obj.ptr_Test);
+      geometry_wrapper(22, obj.ptr_Test);
     end
 
     function display(obj), obj.print(''); end
@@ -60,7 +60,7 @@ classdef Test < handle
       % Method Overloads
       % arg_EigenConstRef(Matrix value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        geometry_wrapper(21, this, varargin{:});
+        geometry_wrapper(23, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.arg_EigenConstRef');
       end
@@ -73,7 +73,7 @@ classdef Test < handle
       % Method Overloads
       % create_MixedPtrs()
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = geometry_wrapper(22, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.create_MixedPtrs');
       end
@@ -86,7 +86,7 @@ classdef Test < handle
       % Method Overloads
       % create_ptrs()
       if length(varargin) == 0
-        [ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.create_ptrs');
       end
@@ -99,7 +99,7 @@ classdef Test < handle
       % Method Overloads
       % print()
       if length(varargin) == 0
-        geometry_wrapper(24, this, varargin{:});
+        geometry_wrapper(26, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.print');
       end
@@ -112,7 +112,7 @@ classdef Test < handle
       % Method Overloads
       % return_Point2Ptr(bool value)
       if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = geometry_wrapper(25, this, varargin{:});
+        varargout{1} = geometry_wrapper(27, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_Point2Ptr');
       end
@@ -125,7 +125,7 @@ classdef Test < handle
       % Method Overloads
       % return_Test(Test value)
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(26, this, varargin{:});
+        varargout{1} = geometry_wrapper(28, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_Test');
       end
@@ -138,7 +138,7 @@ classdef Test < handle
       % Method Overloads
       % return_TestPtr(Test value)
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(27, this, varargin{:});
+        varargout{1} = geometry_wrapper(29, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_TestPtr');
       end
@@ -151,7 +151,7 @@ classdef Test < handle
       % Method Overloads
       % return_bool(bool value)
       if length(varargin) == 1 && isa(varargin{1},'logical')
-        varargout{1} = geometry_wrapper(28, this, varargin{:});
+        varargout{1} = geometry_wrapper(30, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_bool');
       end
@@ -164,7 +164,7 @@ classdef Test < handle
       % Method Overloads
       % return_double(double value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(29, this, varargin{:});
+        varargout{1} = geometry_wrapper(31, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_double');
       end
@@ -177,7 +177,7 @@ classdef Test < handle
       % Method Overloads
       % return_field(Test t)
       if length(varargin) == 1 && isa(varargin{1},'Test')
-        varargout{1} = geometry_wrapper(30, this, varargin{:});
+        varargout{1} = geometry_wrapper(32, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_field');
       end
@@ -190,7 +190,7 @@ classdef Test < handle
       % Method Overloads
       % return_int(int value)
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(31, this, varargin{:});
+        varargout{1} = geometry_wrapper(33, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_int');
       end
@@ -203,7 +203,7 @@ classdef Test < handle
       % Method Overloads
       % return_matrix1(Matrix value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(32, this, varargin{:});
+        varargout{1} = geometry_wrapper(34, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_matrix1');
       end
@@ -216,7 +216,7 @@ classdef Test < handle
       % Method Overloads
       % return_matrix2(Matrix value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(33, this, varargin{:});
+        varargout{1} = geometry_wrapper(35, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_matrix2');
       end
@@ -229,7 +229,7 @@ classdef Test < handle
       % Method Overloads
       % return_pair(Vector v, Matrix A)
       if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(34, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_pair');
       end
@@ -242,7 +242,7 @@ classdef Test < handle
       % Method Overloads
       % return_ptrs(Test p1, Test p2)
       if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
-        [ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:});
+        [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_ptrs');
       end
@@ -255,7 +255,7 @@ classdef Test < handle
       % Method Overloads
       % return_size_t(size_t value)
       if length(varargin) == 1 && isa(varargin{1},'numeric')
-        varargout{1} = geometry_wrapper(36, this, varargin{:});
+        varargout{1} = geometry_wrapper(38, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_size_t');
       end
@@ -268,7 +268,7 @@ classdef Test < handle
       % Method Overloads
       % return_string(string value)
       if length(varargin) == 1 && isa(varargin{1},'char')
-        varargout{1} = geometry_wrapper(37, this, varargin{:});
+        varargout{1} = geometry_wrapper(39, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_string');
       end
@@ -281,7 +281,7 @@ classdef Test < handle
       % Method Overloads
       % return_vector1(Vector value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(38, this, varargin{:});
+        varargout{1} = geometry_wrapper(40, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_vector1');
       end
@@ -294,7 +294,7 @@ classdef Test < handle
       % Method Overloads
       % return_vector2(Vector value)
       if length(varargin) == 1 && isa(varargin{1},'double')
-        varargout{1} = geometry_wrapper(39, this, varargin{:});
+        varargout{1} = geometry_wrapper(41, this, varargin{:});
       else
         error('Arguments do not match any overload of function Test.return_vector2');
       end

wrap/tests/expected/aGlobalFunction.m

@@ -1,6 +1,6 @@
 function varargout = aGlobalFunction(varargin)
 if length(varargin) == 0
-    varargout{1} = geometry_wrapper(40, varargin{:});
+    varargout{1} = geometry_wrapper(42, varargin{:});
 else
     error('Arguments do not match any overload of function aGlobalFunction');
 end

wrap/tests/expected/geometry_wrapper.cpp

@@ -2,9 +2,16 @@
 #include <map>
 #include <boost/foreach.hpp>
 
+#include <boost/serialization/export.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+
 #include <folder/path/to/Test.h>
 
 
+BOOST_CLASS_EXPORT_GUID(Point2, "Point2");
+BOOST_CLASS_EXPORT_GUID(Point3, "Point3");
+
 typedef std::set<boost::shared_ptr<Point2>*> Collector_Point2;
 static Collector_Point2 collector_Point2;
 typedef std::set<boost::shared_ptr<Point3>*> Collector_Point3;
@@ -220,7 +227,17 @@ void Point3_norm_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]
   out[0] = wrap< double >(obj->norm());
 }
 
-void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Point3_string_serialize_15(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;
+  boost::archive::text_oarchive out_archive(out_archive_stream);
+  out_archive << *obj;
+  out[0] = wrap< string >(out_archive_stream.str());
+}
+void Point3_StaticFunctionRet_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> SharedPoint3;
   typedef boost::shared_ptr<Point3> Shared;
@@ -229,14 +246,25 @@ void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const
   out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false);
 }
 
-void Point3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Point3_staticFunction_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point3> Shared;
   checkArguments("Point3.staticFunction",nargout,nargin,0);
   out[0] = wrap< double >(Point3::staticFunction());
 }
 
-void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+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);
+  boost::archive::text_iarchive in_archive(in_archive_stream);
+  Shared output(new Point3());
+  in_archive >> *output;
+  out[0] = wrap_shared_ptr(output,"Point3", false);
+}
+void Test_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -245,7 +273,7 @@ void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin,
   collector_Test.insert(self);
 }
 
-void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -256,7 +284,7 @@ void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   mexAtExit(&_deleteAllObjects);
   typedef boost::shared_ptr<Test> Shared;
@@ -269,7 +297,7 @@ void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray
   *reinterpret_cast<Shared**> (mxGetData(out[0])) = self;
 }
 
-void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_deconstructor_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("delete_Test",nargout,nargin,1);
@@ -282,7 +310,7 @@ void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArra
   }
 }
 
-void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_arg_EigenConstRef_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
@@ -291,7 +319,7 @@ void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mx
   obj->arg_EigenConstRef(value);
 }
 
-void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_MixedPtrs_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> SharedTest;
   typedef boost::shared_ptr<Test> SharedTest;
@@ -303,7 +331,7 @@ void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxA
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_create_ptrs_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> SharedTest;
   typedef boost::shared_ptr<Test> SharedTest;
@@ -315,7 +343,7 @@ void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_print_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("print",nargout,nargin-1,0);
@@ -323,7 +351,7 @@ void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   obj->print();
 }
 
-void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Point2Ptr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Point2> SharedPoint2;
   typedef boost::shared_ptr<Test> Shared;
@@ -333,7 +361,7 @@ void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxA
   out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"Point2", false);
 }
 
-void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_Test_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> SharedTest;
   typedef boost::shared_ptr<Test> Shared;
@@ -343,7 +371,7 @@ void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false);
 }
 
-void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_TestPtr_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> SharedTest;
   typedef boost::shared_ptr<Test> Shared;
@@ -353,7 +381,7 @@ void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }
 
-void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_bool_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_bool",nargout,nargin-1,1);
@@ -362,7 +390,7 @@ void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_bool(value));
 }
 
-void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_double_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_double",nargout,nargin-1,1);
@@ -371,7 +399,7 @@ void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< double >(obj->return_double(value));
 }
 
-void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_field_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_field",nargout,nargin-1,1);
@@ -380,7 +408,7 @@ void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray
   out[0] = wrap< bool >(obj->return_field(t));
 }
 
-void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_int_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_int",nargout,nargin-1,1);
@@ -389,7 +417,7 @@ void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *
   out[0] = wrap< int >(obj->return_int(value));
 }
 
-void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_matrix1",nargout,nargin-1,1);
@@ -398,7 +426,7 @@ void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }
 
-void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_matrix2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_matrix2",nargout,nargin-1,1);
@@ -407,7 +435,7 @@ void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }
 
-void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_pair_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_pair",nargout,nargin-1,2);
@@ -419,7 +447,7 @@ void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
-void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_ptrs_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> SharedTest;
   typedef boost::shared_ptr<Test> SharedTest;
@@ -433,7 +461,7 @@ void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
-void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_size_t_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_size_t",nargout,nargin-1,1);
@@ -442,7 +470,7 @@ void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< size_t >(obj->return_size_t(value));
 }
 
-void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_string_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_string",nargout,nargin-1,1);
@@ -451,7 +479,7 @@ void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArra
   out[0] = wrap< string >(obj->return_string(value));
 }
 
-void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector1_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_vector1",nargout,nargin-1,1);
@@ -460,7 +488,7 @@ void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector1(value));
 }
 
-void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void Test_return_vector2_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_vector2",nargout,nargin-1,1);
@@ -469,7 +497,7 @@ void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArr
   out[0] = wrap< Vector >(obj->return_vector2(value));
 }
 
-void aGlobalFunction_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("aGlobalFunction",nargout,nargin,0);
   out[0] = wrap< Vector >(aGlobalFunction());
@@ -532,82 +560,88 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       Point3_norm_14(nargout, out, nargin-1, in+1);
       break;
     case 15:
-      Point3_StaticFunctionRet_15(nargout, out, nargin-1, in+1);
+      Point3_string_serialize_15(nargout, out, nargin-1, in+1);
       break;
     case 16:
-      Point3_staticFunction_16(nargout, out, nargin-1, in+1);
+      Point3_StaticFunctionRet_16(nargout, out, nargin-1, in+1);
       break;
     case 17:
-      Test_collectorInsertAndMakeBase_17(nargout, out, nargin-1, in+1);
+      Point3_staticFunction_17(nargout, out, nargin-1, in+1);
       break;
     case 18:
-      Test_constructor_18(nargout, out, nargin-1, in+1);
+      Point3_string_deserialize_18(nargout, out, nargin-1, in+1);
       break;
     case 19:
-      Test_constructor_19(nargout, out, nargin-1, in+1);
+      Test_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1);
       break;
     case 20:
-      Test_deconstructor_20(nargout, out, nargin-1, in+1);
+      Test_constructor_20(nargout, out, nargin-1, in+1);
       break;
     case 21:
-      Test_arg_EigenConstRef_21(nargout, out, nargin-1, in+1);
+      Test_constructor_21(nargout, out, nargin-1, in+1);
       break;
     case 22:
-      Test_create_MixedPtrs_22(nargout, out, nargin-1, in+1);
+      Test_deconstructor_22(nargout, out, nargin-1, in+1);
       break;
     case 23:
-      Test_create_ptrs_23(nargout, out, nargin-1, in+1);
+      Test_arg_EigenConstRef_23(nargout, out, nargin-1, in+1);
       break;
     case 24:
-      Test_print_24(nargout, out, nargin-1, in+1);
+      Test_create_MixedPtrs_24(nargout, out, nargin-1, in+1);
       break;
     case 25:
-      Test_return_Point2Ptr_25(nargout, out, nargin-1, in+1);
+      Test_create_ptrs_25(nargout, out, nargin-1, in+1);
       break;
     case 26:
-      Test_return_Test_26(nargout, out, nargin-1, in+1);
+      Test_print_26(nargout, out, nargin-1, in+1);
       break;
     case 27:
-      Test_return_TestPtr_27(nargout, out, nargin-1, in+1);
+      Test_return_Point2Ptr_27(nargout, out, nargin-1, in+1);
       break;
     case 28:
-      Test_return_bool_28(nargout, out, nargin-1, in+1);
+      Test_return_Test_28(nargout, out, nargin-1, in+1);
       break;
     case 29:
-      Test_return_double_29(nargout, out, nargin-1, in+1);
+      Test_return_TestPtr_29(nargout, out, nargin-1, in+1);
       break;
     case 30:
-      Test_return_field_30(nargout, out, nargin-1, in+1);
+      Test_return_bool_30(nargout, out, nargin-1, in+1);
       break;
     case 31:
-      Test_return_int_31(nargout, out, nargin-1, in+1);
+      Test_return_double_31(nargout, out, nargin-1, in+1);
       break;
     case 32:
-      Test_return_matrix1_32(nargout, out, nargin-1, in+1);
+      Test_return_field_32(nargout, out, nargin-1, in+1);
       break;
     case 33:
-      Test_return_matrix2_33(nargout, out, nargin-1, in+1);
+      Test_return_int_33(nargout, out, nargin-1, in+1);
       break;
     case 34:
-      Test_return_pair_34(nargout, out, nargin-1, in+1);
+      Test_return_matrix1_34(nargout, out, nargin-1, in+1);
       break;
     case 35:
-      Test_return_ptrs_35(nargout, out, nargin-1, in+1);
+      Test_return_matrix2_35(nargout, out, nargin-1, in+1);
       break;
     case 36:
-      Test_return_size_t_36(nargout, out, nargin-1, in+1);
+      Test_return_pair_36(nargout, out, nargin-1, in+1);
       break;
     case 37:
-      Test_return_string_37(nargout, out, nargin-1, in+1);
+      Test_return_ptrs_37(nargout, out, nargin-1, in+1);
       break;
     case 38:
-      Test_return_vector1_38(nargout, out, nargin-1, in+1);
+      Test_return_size_t_38(nargout, out, nargin-1, in+1);
       break;
     case 39:
-      Test_return_vector2_39(nargout, out, nargin-1, in+1);
+      Test_return_string_39(nargout, out, nargin-1, in+1);
       break;
     case 40:
-      aGlobalFunction_40(nargout, out, nargin-1, in+1);
+      Test_return_vector1_40(nargout, out, nargin-1, in+1);
+      break;
+    case 41:
+      Test_return_vector2_41(nargout, out, nargin-1, in+1);
+      break;
+    case 42:
+      aGlobalFunction_42(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/geometry.h

@@ -13,6 +13,8 @@ class Point2 {
  void argChar(char a) const;
  void argUChar(unsigned char a) const;
  VectorNotEigen vectorConfusion();
+
+ void serializable() const; // Sets flag and creates export, but does not make serialization functions
 };
 
 class Point3 {
@@ -22,6 +24,9 @@ class Point3 {
   // static functions - use static keyword and uppercase
   static double staticFunction();
   static Point3 StaticFunctionRet(double z);
+
+  // enabling serialization functionality
+  void serialize() const; // Just triggers a flag internally and removes actual function
 };
 
 // another comment

wrap/tests/testWrap.cpp

@@ -219,6 +219,10 @@ TEST( wrap, parse_geometry ) {
 
     EXPECT_LONGS_EQUAL(0, cls.static_methods.size());
     EXPECT_LONGS_EQUAL(0, cls.namespaces.size());
+
+    // check serialization flag
+    EXPECT(cls.isSerializable);
+    EXPECT(!cls.hasSerialization);
   }
 
   // check second class, Point3
@@ -251,6 +255,10 @@ TEST( wrap, parse_geometry ) {
     LONGS_EQUAL(1, m1.argLists.size());
     EXPECT_LONGS_EQUAL(0, m1.argLists.front().size());
     EXPECT(m1.is_const_);
+
+    // check serialization flag
+    EXPECT(cls.isSerializable);
+    EXPECT(cls.hasSerialization);
   }
 
   // Test class is the third one