diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 05439bafc..4289b4969 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { 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 shared_ptr; #endif + /** Default constructor - only use for serialization */ + AHRSFactor() {} + /** * Constructor * @param rot_i previous rot key diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 831788073..adb8bf2bb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams { std::optional getOmegaCoriolis() const; std::optional getBodyPSensor() const; + + // enabling serialization functionality + void serialize() const; }; #include @@ -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 @@ -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 - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); + // Constructors + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params); + 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, - double deltaT); + void integrateMeasurement(gtsam::Vector measuredAcc, + 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 @@ -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 @@ -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 @@ -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 @@ -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 diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py new file mode 100644 index 000000000..4f5c57b0a --- /dev/null +++ b/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() diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index a6a5745bc..e51617b00 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/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) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 50af004f4..74eaff1db 100644 --- a/python/gtsam/utils/test_case.py +++ b/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( - "Values are not equal:\n{}!={}".format(actual, expected)) + raise self.failureException("Values are not equal:\n{}!={}".format( + 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)