Merged in fix/SmartFactorSerialization (pull request #202)

fixed smart factors / IMU factor serialization, add unit tests

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

@@ -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

@@ -128,8 +128,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()));
   }
 };
 
@@ -167,7 +168,7 @@ public:
 #endif
 
   /** Default constructor - only use for serialization */
-  ImuFactor();
+  ImuFactor() {}
 
   /**
    * Constructor
@@ -241,4 +242,7 @@ private:
 };
 // class ImuFactor
 
+/// traits
+template<> struct traits<ImuFactor> : public Testable<ImuFactor> {};
+
 } /// namespace gtsam

gtsam/navigation/PreintegratedRotation.h

@@ -52,7 +52,8 @@ public:
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
+      namespace bs = ::boost::serialization;
+      ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
       ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
       ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
     }

gtsam/navigation/PreintegrationBase.h

@@ -84,15 +84,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);
     }
@@ -246,15 +248,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/testImuFactor.cpp

@@ -1010,6 +1010,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;