Merge remote-tracking branch 'origin/feature/small_Rot3_optimizations' into feature/ImuFactorPush2

Conflicts:
	gtsam/geometry/SO3.cpp
	gtsam/navigation/ImuFactor.h
	gtsam/navigation/PreintegratedRotation.h

gtsam/base/Matrix.cpp

@@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
   return M;
 }
 
-/* ************************************************************************* */
-Matrix3 skewSymmetric(double wx, double wy, double wz)
-{
-  return (Matrix3() <<
-      0.0, -wz, +wy,
-      +wz, 0.0, -wx,
-      -wy, +wx, 0.0).finished();
-}
-
 /* ************************************************************************* */
 Matrix LLt(const Matrix& A)
 {

gtsam/base/Matrix.h

@@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask
  * @param wz
  * @return a 3*3 skew symmetric matrix
 */
-GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz);
+
-template<class Derived>
+inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
-inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) { return skewSymmetric(w(0),w(1),w(2));}
+  return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
+}
+
+template <class Derived>
+inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
+  return skewSymmetric(w(0), w(1), w(2));
+}
 
 /** Use Cholesky to calculate inverse square root of a matrix */
 GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);

gtsam/geometry/Rot3Q.cpp

@@ -27,14 +27,12 @@ using namespace std;
 
 namespace gtsam {
 
-  static const Matrix I3 = eye(3);
-
   /* ************************************************************************* */
   Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
 
   /* ************************************************************************* */
   Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
-      quaternion_((Eigen::Matrix3d() <<
+      quaternion_((Matrix3() <<
           col1.x(), col2.x(), col3.x(),
           col1.y(), col2.y(), col3.y(),
           col1.z(), col2.z(), col3.z()).finished()) {}
@@ -43,7 +41,7 @@ namespace gtsam {
   Rot3::Rot3(double R11, double R12, double R13,
       double R21, double R22, double R23,
       double R31, double R32, double R33) :
-        quaternion_((Eigen::Matrix3d() <<
+        quaternion_((Matrix3() <<
             R11, R12, R13,
             R21, R22, R23,
             R31, R32, R33).finished()) {}
@@ -91,10 +89,10 @@ namespace gtsam {
   /* ************************************************************************* */
   Point3 Rot3::rotate(const Point3& p,
         OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
-    Matrix R = matrix();
+    const Matrix3 R = matrix();
     if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
     if (H2) *H2 = R;
-    Eigen::Vector3d r = R * p.vector();
+    const Vector3 r = R * p.vector();
     return Point3(r.x(), r.y(), r.z());
   }
 

gtsam/geometry/SO3.cpp

@@ -132,9 +132,9 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
 Matrix3 SO3::ExpmapDerivative(const Vector3& omega)    {
   using std::sin;
 
-  double theta2 = omega.dot(omega);
+  const double theta2 = omega.dot(omega);
   if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
-  double theta = std::sqrt(theta2);  // rotation angle
+  const double theta = std::sqrt(theta2);  // rotation angle
 #ifdef DUY_VERSION
   /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
   Matrix3 X = skewSymmetric(omega);
@@ -153,10 +153,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega)    {
    * a perturbation on the manifold (expmap(Jr * omega))
    */
   // element of Lie algebra so(3): X = omega^, normalized by normx
-  const Matrix3 Y = skewSymmetric(omega) / theta;
+  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
+  Matrix3 Y;
+  Y << 0.0, -wz, +wy,
+       +wz, 0.0, -wx,
+       -wy, +wx, 0.0;
   const double s2 = sin(theta / 2.0);
-  return I_3x3 - (2.0 * s2 * s2 / theta) * Y +
+  const double a = (2.0 * s2 * s2 / theta2);
-         (1.0 - sin(theta) / theta) * Y * Y;  // right Jacobian
+  const double b = (1.0 - sin(theta) / theta) / theta2;
+  return I_3x3 - a * Y + b * Y * Y;
 #endif
 }
 

gtsam/geometry/triangulation.h

@@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional<Point3> {
       status_(s) {
   }
 public:
+
+  /**
+   * Default constructor, only for serialization
+   */
+  TriangulationResult() {}
+
+  /**
+   * Constructor
+   */
   TriangulationResult(const Point3& p) :
       status_(VALID) {
     reset(p);

gtsam/navigation/ImuBias.h

@@ -78,19 +78,19 @@ public:
 
   /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
   Vector3 correctAccelerometer(const Vector3& measurement,
-      OptionalJacobian<3, 6> H = boost::none) const {
+                               OptionalJacobian<3, 6> H1 = boost::none,
-    if (H) {
+                               OptionalJacobian<3, 3> H2 = boost::none) const {
-      (*H) << -I_3x3, Z_3x3;
+    if (H1) (*H1) << -I_3x3, Z_3x3;
-    }
+    if (H2) (*H2) << I_3x3;
     return measurement - biasAcc_;
   }
 
   /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
   Vector3 correctGyroscope(const Vector3& measurement,
-      OptionalJacobian<3, 6> H = boost::none) const {
+                           OptionalJacobian<3, 6> H1 = boost::none,
-    if (H) {
+                           OptionalJacobian<3, 3> H2 = boost::none) const {
-      (*H) << Z_3x3, -I_3x3;
+    if (H1) (*H1) << Z_3x3, -I_3x3;
-    }
+    if (H2) (*H2) << I_3x3;
     return measurement - biasGyro_;
   }
 
@@ -201,7 +201,6 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
     ar & BOOST_SERIALIZATION_NVP(biasAcc_);
     ar & BOOST_SERIALIZATION_NVP(biasGyro_);
   }

gtsam/navigation/ImuFactor.h

@@ -140,8 +140,9 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
-    ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
+    ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
   }
 };
 

gtsam/navigation/PreintegratedRotation.h

@@ -47,10 +47,12 @@ class PreintegratedRotation {
    private:
     /** Serialization function */
     friend class boost::serialization::access;
-    template <class ARCHIVE>
+    template<class ARCHIVE>
-    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar& BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
+      namespace bs = ::boost::serialization;
-      ar& BOOST_SERIALIZATION_NVP(omegaCoriolis);
+      ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
+      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
+      ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
       ar& BOOST_SERIALIZATION_NVP(body_P_sensor);
     }
   };

gtsam/navigation/PreintegrationBase.h

@@ -86,15 +86,17 @@ public:
 
   protected:
     /// Default constructor for serialization only: uninitialized!
-    Params();
+    Params() {}
 
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
+      namespace bs = ::boost::serialization;
-      ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
+      ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
-      ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
+           boost::serialization::base_object<PreintegratedRotation::Params>(*this));
+      ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
+      ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
       ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
       ar & BOOST_SERIALIZATION_NVP(n_gravity);
     }
@@ -300,15 +302,16 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_NVP(p_);
     ar & BOOST_SERIALIZATION_NVP(deltaTij_);
     ar & BOOST_SERIALIZATION_NVP(deltaXij_);
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
-    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
+    ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
-    ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
+    ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
-    ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
+    ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
-    ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
+    ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
-    ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
+    ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
   }
 };
 

gtsam/navigation/tests/testImuBias.cpp

@@ -16,113 +16,135 @@
  */
 
 #include <gtsam/navigation/ImuBias.h>
+#include <gtsam/base/numericalDerivative.h>
+
 #include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
 
 using namespace std;
 using namespace gtsam;
 
+typedef imuBias::ConstantBias Bias;
+
 Vector biasAcc1(Vector3(0.2, -0.1, 0));
 Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
-imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
+Bias bias1(biasAcc1, biasGyro1);
 
 Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
 Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
-imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
+Bias bias2(biasAcc2, biasGyro2);
 
 /* ************************************************************************* */
-TEST( ImuBias, Constructor) {
+TEST(ImuBias, Constructor) {
   // Default Constructor
-  imuBias::ConstantBias bias1;
+  Bias bias1;
 
   // Acc + Gyro Constructor
-  imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
+  Bias bias2(biasAcc2, biasGyro2);
 
   // Copy Constructor
-  imuBias::ConstantBias bias3(bias2);
+  Bias bias3(bias2);
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, inverse) {
+TEST(ImuBias, inverse) {
-  imuBias::ConstantBias biasActual = bias1.inverse();
+  Bias biasActual = bias1.inverse();
-  imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1,
+  Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
-      -biasGyro1);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, compose) {
+TEST(ImuBias, compose) {
-  imuBias::ConstantBias biasActual = bias2.compose(bias1);
+  Bias biasActual = bias2.compose(bias1);
-  imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
+  Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
-      biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, between) {
+TEST(ImuBias, between) {
   // p.between(q) == q - p
-  imuBias::ConstantBias biasActual = bias2.between(bias1);
+  Bias biasActual = bias2.between(bias1);
-  imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
+  Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
-      biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, localCoordinates) {
+TEST(ImuBias, localCoordinates) {
   Vector deltaActual = Vector(bias2.localCoordinates(bias1));
-  Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2,
+  Vector deltaExpected =
-      biasGyro1 - biasGyro2)).vector();
+      (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector();
   EXPECT(assert_equal(deltaExpected, deltaActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, retract) {
+TEST(ImuBias, retract) {
   Vector6 delta;
   delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
-  imuBias::ConstantBias biasActual = bias2.retract(delta);
+  Bias biasActual = bias2.retract(delta);
-  imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
+  Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0),
-      biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0));
+                           biasGyro2 + delta.block<3, 1>(3, 0));
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, Logmap) {
+TEST(ImuBias, Logmap) {
   Vector deltaActual = bias2.Logmap(bias1);
   Vector deltaExpected = bias1.vector();
   EXPECT(assert_equal(deltaExpected, deltaActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, Expmap) {
+TEST(ImuBias, Expmap) {
   Vector6 delta;
   delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
-  imuBias::ConstantBias biasActual = bias2.Expmap(delta);
+  Bias biasActual = bias2.Expmap(delta);
-  imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
+  Bias biasExpected = Bias(delta);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, operatorSub) {
+TEST(ImuBias, operatorSub) {
-  imuBias::ConstantBias biasActual = -bias1;
+  Bias biasActual = -bias1;
-  imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
+  Bias biasExpected(-biasAcc1, -biasGyro1);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, operatorAdd) {
+TEST(ImuBias, operatorAdd) {
-  imuBias::ConstantBias biasActual = bias2 + bias1;
+  Bias biasActual = bias2 + bias1;
-  imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1,
+  Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1);
-      biasGyro2 + biasGyro1);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
 /* ************************************************************************* */
-TEST( ImuBias, operatorSubB) {
+TEST(ImuBias, operatorSubB) {
-  imuBias::ConstantBias biasActual = bias2 - bias1;
+  Bias biasActual = bias2 - bias1;
-  imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1,
+  Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1);
-      biasGyro2 - biasGyro1);
   EXPECT(assert_equal(biasExpected, biasActual));
 }
 
+/* ************************************************************************* */
+TEST(ImuBias, Correct1) {
+  Matrix aH1, aH2;
+  const Vector3 measurement(1, 2, 3);
+  boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
+      &Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
+  bias1.correctAccelerometer(measurement, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
+}
+
+/* ************************************************************************* */
+TEST(ImuBias, Correct2) {
+  Matrix aH1, aH2;
+  const Vector3 measurement(1, 2, 3);
+  boost::function<Vector3(const Bias&, const Vector3&)> f =
+      boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
+  bias1.correctGyroscope(measurement, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/tests/testImuFactor.cpp

@@ -859,6 +859,52 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
 }
 
+/* ************************************************************************** */
+#include <gtsam/base/serializationTestHelpers.h>
+
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+TEST(ImuFactor, serialization) {
+  using namespace gtsam::serializationTestHelpers;
+
+  Vector3 n_gravity(0, 0, -9.81);
+  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
+  Matrix3 accCov = 1e-7 * I_3x3;
+  Matrix3 gyroCov = 1e-8 * I_3x3;
+  Matrix3 integrationCov = 1e-9 * I_3x3;
+  double deltaT = 0.005;
+  imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
+
+  ImuFactor::PreintegratedMeasurements pim =
+      ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
+          integrationCov, true);
+
+  // measurements are needed for non-inf noise model, otherwise will throw err when deserialize
+
+  // Sensor frame is z-down
+  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
+  Vector3 measuredOmega(0, 0.01, 0);
+  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
+  // table exerts an equal and opposite force w.r.t gravity
+  Vector3 measuredAcc(0, 0, -9.81);
+
+  for (int j = 0; j < 200; ++j)
+    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
+        body_P_sensor);
+
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/SmartFactorBase.h

@@ -93,6 +93,9 @@ public:
   /// We use the new CameraSte data structure to refer to a set of cameras
   typedef CameraSet<CAMERA> Cameras;
 
+  /// Default Constructor, for serialization
+  SmartFactorBase() {}
+
   /// Constructor
   SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
       boost::optional<Pose3> body_P_sensor = boost::none) :

gtsam/slam/SmartProjectionFactor.h

@@ -137,7 +137,7 @@ protected:
 
   /// @name Parameters
   /// @{
-  const SmartProjectionParams params_;
+  SmartProjectionParams params_;
   /// @}
 
   /// @name Caching triangulation
@@ -154,6 +154,11 @@ public:
   /// shorthand for a set of cameras
   typedef CameraSet<CAMERA> Cameras;
 
+  /**
+   * Default constructor, only for serialization
+   */
+  SmartProjectionFactor() {}
+
   /**
    * Constructor
    * @param body_P_sensor pose of the camera in the body frame

gtsam/slam/SmartProjectionPoseFactor.h

@@ -59,6 +59,11 @@ public:
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
+  /**
+   * Default constructor, only for serialization
+   */
+  SmartProjectionPoseFactor() {}
+
   /**
    * Constructor
    * @param Isotropic measurement noise

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -18,6 +18,7 @@
 
 #include <gtsam/slam/SmartFactorBase.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
 /* ************************************************************************* */
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+
+namespace gtsam {
+
 class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
 public:
   typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
+  PinholeFactor() {}
   PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
   }
   virtual double error(const Values& values) const {
@@ -43,6 +48,11 @@ public:
   }
 };
 
+/// traits
+template<>
+struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
+}
+
 TEST(SmartFactorBase, Pinhole) {
   PinholeFactor f= PinholeFactor(unit2);
   f.add(Point2(), 1);
@@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) {
 
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
+
+namespace gtsam {
+
 class StereoFactor: public SmartFactorBase<StereoCamera> {
 public:
   typedef SmartFactorBase<StereoCamera> Base;
+  StereoFactor() {}
   StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
   }
   virtual double error(const Values& values) const {
@@ -66,6 +80,11 @@ public:
   }
 };
 
+/// traits
+template<>
+struct traits<StereoFactor> : public Testable<StereoFactor> {};
+}
+
 TEST(SmartFactorBase, Stereo) {
   StereoFactor f(unit3);
   f.add(StereoPoint2(), 1);
@@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) {
   EXPECT_LONGS_EQUAL(2 * 3, f.dim());
 }
 
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+TEST(SmartFactorBase, serialize) {
+  using namespace gtsam::serializationTestHelpers;
+  PinholeFactor factor(unit2);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -22,6 +22,7 @@
 #include "smartFactorScenarios.h"
 #include <gtsam/slam/SmartProjectionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
 #include <iostream>
@@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
   EXPECT(assert_equal(yActual, yExpected, 1e-7));
 }
 
+
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+TEST( SmartProjectionCameraFactor, serialize) {
+  using namespace vanilla;
+  using namespace gtsam::serializationTestHelpers;
+  SmartFactor factor(unit2);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/slam/PoseTranslationPrior.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
 #include <iostream>
@@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
           values.at<Pose3>(x3)));
 }
 
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+TEST(SmartProjectionPoseFactor, serialize) {
+  using namespace vanillaPose;
+  using namespace gtsam::serializationTestHelpers;
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  SmartFactor factor(model, sharedK, boost::none, params);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;