From 004cab1542c807e16218bc8cf85c4feb759ca8cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:14:17 -0500 Subject: [PATCH 1/6] add serialize() to various preintegration classes --- gtsam/navigation/navigation.i | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 831788073..3e7aa0fd6 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 @@ -170,22 +173,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 +204,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; + + // enabling serialization functionality + void serialize() const; }; virtual class CombinedImuFactor: gtsam::NoiseModelFactor { From c5554c836f79ec52bebadf31659dde5baf48d004 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:15:09 -0500 Subject: [PATCH 2/6] python tests --- python/gtsam/tests/test_Serialization.py | 47 ++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 python/gtsam/tests/test_Serialization.py diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py new file mode 100644 index 000000000..be8dd7bec --- /dev/null +++ b/python/gtsam/tests/test_Serialization.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from copy import deepcopy + +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +import gtsam + + +class TestSerialization(GtsamTestCase): + """Tests for serialization of various GTSAM objects.""" + + def test_PreintegratedImuMeasurements(self): + """ + Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + def test_PreintegratedCombinedMeasurements(self): + """ + Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedCombinedMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + +if __name__ == "__main__": + unittest.main() From dfcbba822e673fa60527409cf33db7b8731cf08b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:20:33 -0500 Subject: [PATCH 3/6] move default AHRSFactor constructor to public for wrapper --- gtsam/navigation/AHRSFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 From 8f77bfa13b9688358abc476f467afdb3c9940493 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:22:47 -0500 Subject: [PATCH 4/6] serialize navigation factors --- gtsam/navigation/navigation.i | 29 +++++++++++++++++++++++- python/gtsam/tests/test_Serialization.py | 13 +++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 3e7aa0fd6..adb8bf2bb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -151,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 @@ -206,7 +209,7 @@ class PreintegratedCombinedMeasurements { gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; - // enabling serialization functionality + // enable serialization functionality void serialize() const; }; @@ -221,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 @@ -247,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 { @@ -263,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 @@ -276,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 { @@ -290,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 @@ -304,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 { @@ -317,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -334,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 index be8dd7bec..3e22e6603 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -12,6 +12,7 @@ import unittest from copy import deepcopy import numpy as np +from gtsam.symbol_shorthand import B, V, X from gtsam.utils.test_case import GtsamTestCase import gtsam @@ -31,6 +32,18 @@ class TestSerialization(GtsamTestCase): pim2 = deepcopy(pim) self.assertEqual(pim, pim2) + def test_ImuFactor(self): + """ + Test the serialization of `ImuFactor` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + + # If serialization failed, then this will throw an error + imu_odom2 = deepcopy(imu_odom) + self.assertEqual(imu_odom, imu_odom2) + def test_PreintegratedCombinedMeasurements(self): """ Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. From 60b00ebdda6da38bf26c35d112cee6d964bd5da8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 10:08:19 -0500 Subject: [PATCH 5/6] improve testing for deep copy --- python/gtsam/tests/test_Serialization.py | 30 ++++++++++-------------- python/gtsam/tests/test_pickle.py | 10 ++++---- python/gtsam/utils/test_case.py | 16 +++++++++++-- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index 3e22e6603..e935f1f62 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -5,11 +5,11 @@ All Rights Reserved See LICENSE for the license information -KalmanFilter unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Serialization and deep copy tests. + +Author: Varun Agrawal """ import unittest -from copy import deepcopy import numpy as np from gtsam.symbol_shorthand import B, V, X @@ -18,42 +18,36 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam -class TestSerialization(GtsamTestCase): - """Tests for serialization of various GTSAM objects.""" +class TestDeepCopy(GtsamTestCase): + """Tests for deep copy of various GTSAM objects.""" def test_PreintegratedImuMeasurements(self): """ - Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the serialization of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) - # If serialization failed, then this will throw an error - imu_odom2 = deepcopy(imu_odom) - self.assertEqual(imu_odom, imu_odom2) + self.assertDeepCopyEquality(imu_factor) def test_PreintegratedCombinedMeasurements(self): """ - Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) if __name__ == "__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) From 8a30aacf6ca997df0073d633cb3eaa4ff0a5510b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 21 Dec 2024 13:10:29 -0500 Subject: [PATCH 6/6] fix serialization of ImuFactor --- python/gtsam/tests/test_Serialization.py | 26 ++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index e935f1f62..4f5c57b0a 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -23,26 +23,36 @@ class TestDeepCopy(GtsamTestCase): def test_PreintegratedImuMeasurements(self): """ - Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + params = gtsam.PreintegrationParams.MakeSharedD(9.81) pim = gtsam.PreintegratedImuMeasurements(params) self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the deep copy of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) - pim = gtsam.PreintegratedImuMeasurements(params) - imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + 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) - self.assertDeepCopyEquality(imu_factor) + # 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` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements`. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params)