From aaddf52cb102f2a5ab5440c807a1c0871ff81246 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 8 Jul 2020 12:23:01 -0400 Subject: [PATCH 1/7] Abstracted out serialization code for PreintegrationBase --- gtsam/navigation/ManifoldPreintegration.h | 4 +--- gtsam/navigation/PreintegrationBase.h | 10 ++++++++++ gtsam/navigation/TangentPreintegration.h | 4 +--- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 22897b9d4..97ad04744 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -118,10 +118,8 @@ private: template 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_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(deltaXij_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index eb30c1f13..29d7814b5 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -213,6 +213,16 @@ class GTSAM_EXPORT PreintegrationBase { /// @} #endif + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + } + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index edf76e562..99aa10b3f 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -132,9 +132,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); From 8d921c82a0af925586f1a3e4f58640ad47afa4db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 8 Jul 2020 16:10:33 -0400 Subject: [PATCH 2/7] Updated PreintegratedImuMeasurements docstring --- 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 8e3f8f0a4..b5cff8147 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -87,8 +87,8 @@ public: /** * Constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param p Parameters, typically fixed in a single application + * @param p Parameters, typically fixed in a single application + * @param biasHat Current estimate of acceleration and rotation rate biases */ PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : From cc2456678fb97a1d912f8f419b945a46efba9730 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 8 Jul 2020 23:37:32 -0400 Subject: [PATCH 3/7] Replace scoped name with direct name and instantiate base class in constructor --- gtsam/navigation/PreintegratedRotation.cpp | 6 +++--- gtsam/navigation/PreintegrationParams.cpp | 6 +++--- gtsam/navigation/PreintegrationParams.h | 13 +++++++------ 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 8c29d85dd..c5d48b734 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -25,7 +25,7 @@ using namespace std; namespace gtsam { -void PreintegratedRotation::Params::print(const string& s) const { +void PreintegratedRotationParams::print(const string& s) const { cout << s << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; if (omegaCoriolis) @@ -34,8 +34,8 @@ void PreintegratedRotation::Params::print(const string& s) const { body_P_sensor->print("body_P_sensor"); } -bool PreintegratedRotation::Params::equals( - const PreintegratedRotation::Params& other, double tol) const { +bool PreintegratedRotationParams::equals( + const PreintegratedRotationParams& other, double tol) const { if (body_P_sensor) { if (!other.body_P_sensor || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol)) diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp index 61cd1617c..2298bb696 100644 --- a/gtsam/navigation/PreintegrationParams.cpp +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -27,7 +27,7 @@ namespace gtsam { //------------------------------------------------------------------------------ void PreintegrationParams::print(const string& s) const { - PreintegratedRotation::Params::print(s); + PreintegratedRotationParams::print(s); cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" << endl; cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" @@ -39,10 +39,10 @@ void PreintegrationParams::print(const string& s) const { } //------------------------------------------------------------------------------ -bool PreintegrationParams::equals(const PreintegratedRotation::Params& other, +bool PreintegrationParams::equals(const PreintegratedRotationParams& other, double tol) const { auto e = dynamic_cast(&other); - return e != nullptr && PreintegratedRotation::Params::equals(other, tol) && + return e != nullptr && PreintegratedRotationParams::equals(other, tol) && use2ndOrderCoriolis == e->use2ndOrderCoriolis && equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, tol) && diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 4bff625ca..de9950e7d 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -31,7 +31,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { /// Default constructor for serialization only PreintegrationParams() - : accelerometerCovariance(I_3x3), + : PreintegratedRotationParams(), + accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(false), n_gravity(0, 0, -1) {} @@ -39,7 +40,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { /// The Params constructor insists on getting the navigation frame gravity vector /// For convenience, two commonly used conventions are provided by named constructors below PreintegrationParams(const Vector3& n_gravity) - : accelerometerCovariance(I_3x3), + : PreintegratedRotationParams(), + accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(false), n_gravity(n_gravity) {} @@ -54,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, -g))); } - void print(const std::string& s) const; - bool equals(const PreintegratedRotation::Params& other, double tol) const; + void print(const std::string& s="") const; + bool equals(const PreintegratedRotationParams& other, double tol) const; void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } @@ -73,8 +75,7 @@ protected: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams); 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); From 95b4a49f643efe0a8bdae4c04132cf6b110deab2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 00:22:05 -0400 Subject: [PATCH 4/7] Major updates to CombinedImuFactor to make it Testable as well as serializable --- gtsam/navigation/CombinedImuFactor.cpp | 34 ++++++++ gtsam/navigation/CombinedImuFactor.h | 109 +++++++++++++++++++------ 2 files changed, 116 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 149067269..7a8c73013 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -17,9 +17,11 @@ * @author Vadim Indelman * @author David Jensen * @author Frank Dellaert + * @author Varun Agrawal **/ #include +#include /* External or standard includes */ #include @@ -28,6 +30,31 @@ namespace gtsam { using namespace std; +//------------------------------------------------------------------------------ +// Inner class PreintegrationCombinedParams +//------------------------------------------------------------------------------ +void PreintegrationCombinedParams::print(const string& s) const { + PreintegrationParams::print(s); + cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]" + << endl; + cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]" + << endl; + cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]" + << endl; +} + +//------------------------------------------------------------------------------ +bool PreintegrationCombinedParams::equals(const PreintegrationParams& other, + double tol) const { + auto e = dynamic_cast(&other); + return e != nullptr && PreintegrationParams::equals(other, tol) && + equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance, + tol) && + equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance, + tol) && + equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol); +} + //------------------------------------------------------------------------------ // Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ @@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, return r; } +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { + f._PIM_.print("combined preintegrated measurements:\n"); + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; +} + //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 CombinedImuFactor::CombinedImuFactor( diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6b3bf979a..7a3a801f1 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -17,6 +17,7 @@ * @author Vadim Indelman * @author David Jensen * @author Frank Dellaert + * @author Varun Agrawal **/ #pragma once @@ -26,6 +27,7 @@ #include #include #include +#include namespace gtsam { @@ -61,10 +63,19 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration + /// Default constructor makes unitialized params struct. + /// Used for serialization. + PreintegrationCombinedParams() + : PreintegrationParams(), + biasAccCovariance(I_3x3), + biasOmegaCovariance(I_3x3), + biasAccOmegaInt(I_6x6) {} + /// See two named constructors below for good values of n_gravity in body frame -PreintegrationCombinedParams(const Vector3& n_gravity) : - PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( - I_3x3), biasAccOmegaInt(I_6x6) { + PreintegrationCombinedParams(const Vector3& n_gravity) : + PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), + biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { + } // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis @@ -77,6 +88,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) : return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); } + void print(const std::string& s="") const; + bool equals(const PreintegrationParams& other, double tol) const; + void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } @@ -86,24 +100,25 @@ PreintegrationCombinedParams(const Vector3& n_gravity) : const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } private: - /// Default constructor makes unitialized params struct - PreintegrationCombinedParams() {} /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); - ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); - ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); + ar & bs::make_nvp("biasAccCovariance", + bs::make_array(biasAccCovariance.data(), biasAccCovariance.size())); + ar & bs::make_nvp("biasOmegaCovariance", + bs::make_array(biasOmegaCovariance.data(), biasOmegaCovariance.size())); + ar & bs::make_nvp("biasAccOmegaInt", bs::make_array(biasAccOmegaInt.data(), + biasAccOmegaInt.size())); } public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; - /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. @@ -128,7 +143,6 @@ public: */ Eigen::Matrix preintMeasCov_; - friend class CombinedImuFactor; public: @@ -136,11 +150,14 @@ public: /// @{ /// Default constructor only for serialization and Cython wrapper - PreintegratedCombinedMeasurements() {} + PreintegratedCombinedMeasurements() { + preintMeasCov_.setZero(); + } /** * Default constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases + * @param p Parameters, typically fixed in a single application + * @param biasHat Current estimate of acceleration and rotation rate biases */ PreintegratedCombinedMeasurements( const boost::shared_ptr& p, @@ -149,6 +166,19 @@ public: preintMeasCov_.setZero(); } + /** + * Construct preintegrated directly from members: base class and preintMeasCov + * @param base PreintegrationType instance + * @param preintMeasCov Covariance matrix used in noise model. + */ + PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix& preintMeasCov) + : PreintegrationType(base), + preintMeasCov_(preintMeasCov) { + } + + /// Virtual destructor + virtual ~PreintegratedCombinedMeasurements() {} + /// @} /// @name Basic utilities @@ -158,20 +188,25 @@ public: void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(this->p_);} + Params& p() const { return *boost::static_pointer_cast(this->p_); } /// @} /// @name Access instance variables /// @{ + /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } /// @} /// @name Testable /// @{ + /// print void print(const std::string& s = "Preintegrated Measurements:") const override; - bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; + /// equals + bool equals(const PreintegratedCombinedMeasurements& expected, + double tol = 1e-9) const; /// @} + /// @name Main functionality /// @{ @@ -205,8 +240,10 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); - ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); + ar& bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), + preintMeasCov_.size())); } public: @@ -244,9 +281,6 @@ private: PreintegratedCombinedMeasurements _PIM_; - /** Default constructor - only use for serialization */ - CombinedImuFactor() {} - public: /** Shorthand for a smart pointer to a factor */ @@ -256,6 +290,9 @@ public: typedef boost::shared_ptr shared_ptr; #endif + /** Default constructor - only use for serialization */ + CombinedImuFactor() {} + /** * Constructor * @param pose_i Previous pose key @@ -277,12 +314,17 @@ public: /** implement functions needed for Testable */ + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const CombinedImuFactor&); /// print virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} /** Access the preintegrated measurements. */ @@ -321,14 +363,12 @@ public: #endif private: - /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(_PIM_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6); + ar& BOOST_SERIALIZATION_NVP(_PIM_); } public: @@ -336,4 +376,19 @@ public: }; // class CombinedImuFactor -} /// namespace gtsam +template <> +struct traits + : public Testable {}; + +template <> +struct traits + : public Testable {}; + +template <> +struct traits : public Testable {}; + +} // namespace gtsam + +/// Add Boost serialization export for derived class +BOOST_CLASS_EXPORT_GUID(gtsam::PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams"); +// BOOST_CLASS_EXPORT_GUID(gtsam::CombinedImuFactor, "gtsam_CombinedImuFactor"); From d519d24b67bed4495f23098ec458102fd2e575e7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 00:22:13 -0400 Subject: [PATCH 5/7] Fix typo --- gtsam/navigation/ImuFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b5cff8147..408cefdf0 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -59,7 +59,7 @@ typedef ManifoldPreintegration PreintegrationType; */ /** - * PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements + * PreintegratedImuMeasurements accumulates (integrates) the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. * The measurements are then used to build the Preintegrated IMU factor. * Integration is done incrementally (ideally, one integrates the measurement From 23e2b29dbe428ccda610cff133142fbc04cd533f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 00:22:42 -0400 Subject: [PATCH 6/7] Added and updated serialization tests to include all IMU factors --- .../tests/testImuFactorSerialization.cpp | 60 +++++++++++++++---- 1 file changed, 49 insertions(+), 11 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 59d0ac199..ed72e18e9 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -16,15 +16,19 @@ * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams + * @author Varun Agrawal */ -#include -#include #include +#include +#include +#include + #include using namespace std; using namespace gtsam; +using namespace gtsam::serializationTestHelpers; BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); @@ -38,23 +42,23 @@ BOOST_CLASS_EXPORT_GUID(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; - +template +P getPreintegratedMeasurements() { // Create default parameters with Z-down and above noise paramaters - auto p = PreintegrationParams::MakeSharedD(9.81); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); + auto p = P::Params::MakeSharedD(9.81); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; const double deltaT = 0.005; - const imuBias::ConstantBias priorBias( - Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - PreintegratedImuMeasurements pim(p, priorBias); + // Biases (acc, rot) + const imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); - // measurements are needed for non-inf noise model, otherwise will throw err + P pim(p, priorBias); + + // measurements are needed for non-inf noise model, otherwise will throw error // when deserialize const Vector3 measuredOmega(0, 0.01, 0); const Vector3 measuredAcc(0, 0, -9.81); @@ -62,6 +66,16 @@ TEST(ImuFactor, serialization) { for (int j = 0; j < 200; ++j) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + return pim; +} + +TEST(ImuFactor, serialization) { + auto pim = getPreintegratedMeasurements(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + ImuFactor factor(1, 2, 3, 4, 5, pim); EXPECT(equalsObj(factor)); @@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) { EXPECT(equalsBinary(factor)); } +TEST(ImuFactor2, serialization) { + auto pim = getPreintegratedMeasurements(); + + ImuFactor2 factor(1, 2, 3, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(CombinedImuFactor, Serialization) { + auto pim = getPreintegratedMeasurements(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + + const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 904ecf4f1f341db840f1edfed0e9e42fedef25d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Jul 2020 23:11:20 -0400 Subject: [PATCH 7/7] Use built in Matrix serialization --- gtsam/navigation/CombinedImuFactor.h | 18 ++++++------------ gtsam/navigation/ImuFactor.h | 2 +- gtsam/navigation/ManifoldPreintegration.h | 10 +++++----- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/PreintegrationParams.h | 4 ++-- gtsam/navigation/TangentPreintegration.h | 6 +++--- 6 files changed, 18 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 7a3a801f1..f47ce8846 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -63,11 +63,10 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration - /// Default constructor makes unitialized params struct. + /// Default constructor makes uninitialized params struct. /// Used for serialization. PreintegrationCombinedParams() - : PreintegrationParams(), - biasAccCovariance(I_3x3), + : biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {} @@ -107,12 +106,9 @@ private: void serialize(ARCHIVE& ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); - ar & bs::make_nvp("biasAccCovariance", - bs::make_array(biasAccCovariance.data(), biasAccCovariance.size())); - ar & bs::make_nvp("biasOmegaCovariance", - bs::make_array(biasOmegaCovariance.data(), biasOmegaCovariance.size())); - ar & bs::make_nvp("biasAccOmegaInt", bs::make_array(biasAccOmegaInt.data(), - biasAccOmegaInt.size())); + ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); } public: @@ -242,8 +238,7 @@ public: void serialize(ARCHIVE& ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); - ar& bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), - preintMeasCov_.size())); + ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } public: @@ -391,4 +386,3 @@ struct traits : public Testable {}; /// Add Boost serialization export for derived class BOOST_CLASS_EXPORT_GUID(gtsam::PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams"); -// BOOST_CLASS_EXPORT_GUID(gtsam::CombinedImuFactor, "gtsam_CombinedImuFactor"); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 408cefdf0..7e080ffd5 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -164,7 +164,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); - ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 97ad04744..a290972e4 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -120,11 +120,11 @@ private: namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(deltaXij_); - ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); - ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); - ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); - ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); - ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 12938a625..9346f749a 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -61,7 +61,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams { template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); } diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index de9950e7d..9ae66e678 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -76,8 +76,8 @@ protected: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams); - 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(accelerometerCovariance); + ar & BOOST_SERIALIZATION_NVP(integrationCovariance); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(n_gravity); } diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 99aa10b3f..1b51b4e1e 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -133,9 +133,9 @@ private: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); - ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); - ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); + ar & BOOST_SERIALIZATION_NVP(preintegrated_); + ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_); + ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_); } public: