Merge pull request #1939 from borglab/fix-pim-serialization

gtsam/navigation/AHRSFactor.h

@@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
   PreintegratedAhrsMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  AHRSFactor() {}
-
 public:
 
   // Provide access to the Matrix& version of evaluateError:
@@ -154,6 +151,9 @@ public:
   typedef std::shared_ptr<AHRSFactor> shared_ptr;
 #endif
 
+  /** Default constructor - only use for serialization */
+  AHRSFactor() {}
+
   /**
    * Constructor
    * @param rot_i previous rot key

gtsam/navigation/navigation.i

@@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams {
 
   std::optional<gtsam::Vector> getOmegaCoriolis() const;
   std::optional<gtsam::Pose3> getBodyPSensor() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/PreintegrationParams.h>
@@ -148,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
   gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
@@ -170,22 +176,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
   gtsam::Matrix getBiasAccCovariance() const ;
   gtsam::Matrix getBiasOmegaCovariance() const ;
   gtsam::Matrix getBiasAccOmegaInit() const;
- 
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class PreintegratedCombinedMeasurements {
-// Constructors
+  // Constructors
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
+  PreintegratedCombinedMeasurements(
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
+      const gtsam::PreintegrationCombinedParams* params);
-				    const gtsam::imuBias::ConstantBias& bias);
+  PreintegratedCombinedMeasurements(
+      const gtsam::PreintegrationCombinedParams* params,
+      const gtsam::imuBias::ConstantBias& bias);
   // Testable
   void print(string s = "Preintegrated Measurements:") const;
   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
-      double tol);
+              double tol);
 
   // Standard Interface
-  void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
+  void integrateMeasurement(gtsam::Vector measuredAcc,
-      double deltaT);
+                            gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration();
   void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
 
@@ -197,7 +207,10 @@ class PreintegratedCombinedMeasurements {
   gtsam::imuBias::ConstantBias biasHat() const;
   gtsam::Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
+                          const gtsam::imuBias::ConstantBias& bias) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
@@ -211,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::imuBias::ConstantBias& bias_j);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AHRSFactor.h>
@@ -237,6 +253,9 @@ class PreintegratedAhrsMeasurements {
   // Standard Interface
   void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration() ;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class AHRSFactor : gtsam::NonlinearFactor {
@@ -253,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
   gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
       const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
       gtsam::Vector omegaCoriolis) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AttitudeFactor.h>
@@ -266,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
@@ -280,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/GPSFactor.h>
@@ -294,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class GPSFactor2 : gtsam::NonlinearFactor {
@@ -307,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/BarometricFactor.h>
@@ -324,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
   const double& measurementIn() const;
   double heightOut(double n) const;
   double baroOut(const double& meters) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/Scenario.h>

python/gtsam/tests/test_Serialization.py

@@ -0,0 +1,64 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Serialization and deep copy tests.
+
+Author: Varun Agrawal
+"""
+import unittest
+
+import numpy as np
+from gtsam.symbol_shorthand import B, V, X
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
+
+
+class TestDeepCopy(GtsamTestCase):
+    """Tests for deep copy of various GTSAM objects."""
+
+    def test_PreintegratedImuMeasurements(self):
+        """
+        Test the deep copy of `PreintegratedImuMeasurements`.
+        """
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
+        pim = gtsam.PreintegratedImuMeasurements(params)
+
+        self.assertDeepCopyEquality(pim)
+
+    def test_ImuFactor(self):
+        """
+        Test the deep copy of `ImuFactor`.
+        """
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
+        params.setAccelerometerCovariance(1e-7 * np.eye(3))
+        params.setGyroscopeCovariance(1e-8 * np.eye(3))
+        params.setIntegrationCovariance(1e-9 * np.eye(3))
+        priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3))
+        pim = gtsam.PreintegratedImuMeasurements(params, priorBias)
+
+        # Preintegrate a single measurement for serialization to work.
+        pim.integrateMeasurement(measuredAcc=np.zeros(3),
+                                 measuredOmega=np.zeros(3),
+                                 deltaT=0.005)
+
+        factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim)
+
+        self.assertDeepCopyEquality(factor)
+
+    def test_PreintegratedCombinedMeasurements(self):
+        """
+        Test the deep copy of `PreintegratedCombinedMeasurements`.
+        """
+        params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81]))
+        pim = gtsam.PreintegratedCombinedMeasurements(params)
+
+        self.assertDeepCopyEquality(pim)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam/tests/test_pickle.py

@@ -9,24 +9,26 @@ Unit tests to check pickling.
 
 Author: Ayush Baid
 """
-from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 
-
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3,
+                   Pose3, Rot3, SfmTrack, Unit3)
+
+
 class TestPickle(GtsamTestCase):
     """Tests pickling on some of the classes."""
 
     def test_cal3Bundler_roundtrip(self):
         obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70)
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_pinholeCameraCal3Bundler_roundtrip(self):
         obj = PinholeCameraCal3Bundler(
             Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)),
             Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70),
         )
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_rot3_roundtrip(self):
         obj = Rot3.RzRyRx(0, 0.05, 0.1)
         self.assertEqualityOnPickleRoundtrip(obj)

python/gtsam/utils/test_case.py

@@ -10,6 +10,7 @@ Author: Frank Dellaert
 """
 import pickle
 import unittest
+from copy import deepcopy
 
 
 class GtsamTestCase(unittest.TestCase):
@@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase):
         else:
             equal = actual.equals(expected, tol)
         if not equal:
-            raise self.failureException(
+            raise self.failureException("Values are not equal:\n{}!={}".format(
-                "Values are not equal:\n{}!={}".format(actual, expected))
+                actual, expected))
 
     def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None:
         """ Performs a round-trip using pickle and asserts equality.
@@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase):
         """
         roundTripObj = pickle.loads(pickle.dumps(obj))
         self.gtsamAssertEquals(roundTripObj, obj)
+
+    def assertDeepCopyEquality(self, obj):
+        """Perform assertion by checking if a
+        deep copied version of `obj` is equal to itself.
+
+        Args:
+            obj: The object to check is deep-copyable.
+        """
+        # If deep copy failed, then this will throw an error
+        obj2 = deepcopy(obj)
+        self.gtsamAssertEquals(obj, obj2)