From b7e60c4043b39dd8d31510376e34bcb13f8cfa64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 17:30:20 -0500 Subject: [PATCH 1/8] suppress newer GCC warnings in Eigen --- gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h index fe0cfec0b..aa798c093 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h @@ -66,6 +66,7 @@ #endif #if __GNUC__>=6 #pragma GCC diagnostic ignored "-Wignored-attributes" + #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #endif #if __GNUC__==7 // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325 From ff4525582167e08d2a550c2662d596fb8656d409 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 17:30:25 -0500 Subject: [PATCH 2/8] fix warning --- gtsam/hybrid/tests/testHybridMotionModel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 747a1b688..a208febf7 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -100,7 +100,7 @@ std::pair approximateDiscreteMarginal( // Do importance sampling double w0 = 0.0, w1 = 0.0; std::mt19937_64 rng(42); - for (int i = 0; i < N; i++) { + for (size_t i = 0; i < N; i++) { HybridValues sample = q.sample(&rng); sample.insert(given); double weight = hbn.evaluate(sample) / q.evaluate(sample); From ac47d8f27c9bbcb88f4abef6fb400d7e2af52fa1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 17:31:32 -0500 Subject: [PATCH 3/8] fix class name in comments --- gtsam/navigation/ImuFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7254838fd..a3d7dce75 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -113,7 +113,7 @@ public: /// equals bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; - /// Re-initialize PreintegratedIMUMeasurements + /// Re-initialize PreintegratedImuMeasurements void resetIntegration() override; /** @@ -159,7 +159,7 @@ public: * the vehicle at previous time step), current state (pose and velocity at * current time step), and the bias estimate. Following the preintegration * scheme proposed in [2], the ImuFactor includes many IMU measurements, which - * are "summarized" using the PreintegratedIMUMeasurements class. + * are "summarized" using the PreintegratedImuMeasurements class. * Note that this factor does not model "temporal consistency" of the biases * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. From 1a96544308807b1f3eaa36ec476c3569723b6a0d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 17:34:41 -0500 Subject: [PATCH 4/8] more comprehensive initialization of ImuFactors --- gtsam/navigation/CombinedImuFactor.h | 10 +++++++--- gtsam/navigation/ImuFactor.h | 5 +++-- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0ffb386a0..5772fc5ae 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -84,7 +84,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements /// @{ /// Default constructor only for serialization and wrappers - PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } + PreintegratedCombinedMeasurements() { resetIntegration(); } /** * Default constructor, initializes the class with no measurements @@ -97,7 +97,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(), const Eigen::Matrix& preintMeasCov = Eigen::Matrix::Zero()) - : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {} + : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); + } /** * Construct preintegrated directly from members: base class and @@ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements PreintegratedCombinedMeasurements( const PreintegrationType& base, const Eigen::Matrix& preintMeasCov) - : PreintegrationType(base), preintMeasCov_(preintMeasCov) {} + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); + } /// Virtual destructor ~PreintegratedCombinedMeasurements() override {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index a3d7dce75..fd087aaee 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -79,7 +79,7 @@ public: /// Default constructor for serialization and wrappers PreintegratedImuMeasurements() { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -90,7 +90,7 @@ public: PreintegratedImuMeasurements(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationType(p, biasHat) { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -101,6 +101,7 @@ public: PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); } /// Virtual destructor From afc6868ca39f238e930101c03cfd52e2a46795c5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Dec 2024 18:17:09 -0500 Subject: [PATCH 5/8] wrap ImuFactor2 --- gtsam/navigation/navigation.i | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index adb8bf2bb..ceeab3b35 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -156,6 +156,22 @@ virtual class ImuFactor: gtsam::NonlinearFactor { void serialize() const; }; +virtual class ImuFactor2: gtsam::NonlinearFactor { + ImuFactor2(); + ImuFactor2(size_t state_i, size_t state_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + gtsam::Vector evaluateError(const gtsam::NavState& state_i, + gtsam::NavState& state_j, + const gtsam::imuBias::ConstantBias& bias_i); + + // enable serialization functionality + void serialize() const; +}; + #include virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(gtsam::Vector n_gravity); From 28065b737b62c3b0c32119ba73c6058c8e99f147 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Dec 2024 10:31:39 -0500 Subject: [PATCH 6/8] Helper macro for cout printing --- gtsam/base/Testable.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 8eb6326c0..eb8d851d1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -41,6 +41,7 @@ #include #define GTSAM_PRINT(x)((x).print(#x)) +#define GTSAM_COUT(x) (std::cout << #x ": " << x << std::endl) namespace gtsam { From 9142200a6e577f5a59d1f0904228cd000274abeb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jan 2025 10:27:34 -0500 Subject: [PATCH 7/8] Revert "suppress newer GCC warnings in Eigen" This reverts commit b7e60c4043b39dd8d31510376e34bcb13f8cfa64. --- gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h index aa798c093..fe0cfec0b 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h @@ -66,7 +66,6 @@ #endif #if __GNUC__>=6 #pragma GCC diagnostic ignored "-Wignored-attributes" - #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #endif #if __GNUC__==7 // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325 From 9910bb63623b5b0423b4b032da81814fdef349c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Jan 2025 10:28:36 -0500 Subject: [PATCH 8/8] Revert "Helper macro for cout printing" This reverts commit 28065b737b62c3b0c32119ba73c6058c8e99f147. --- gtsam/base/Testable.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index eb8d851d1..8eb6326c0 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -41,7 +41,6 @@ #include #define GTSAM_PRINT(x)((x).print(#x)) -#define GTSAM_COUT(x) (std::cout << #x ": " << x << std::endl) namespace gtsam {