From 843f53c6cc84fb206f95e74e862529990349b7a0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 14 Oct 2022 16:50:19 -0400 Subject: [PATCH] move PreintegrationCombinedParams to its own file --- gtsam/navigation/CombinedImuFactor.h | 66 +---------- .../navigation/PreintegrationCombinedParams.h | 104 ++++++++++++++++++ 2 files changed, 108 insertions(+), 62 deletions(-) create mode 100644 gtsam/navigation/PreintegrationCombinedParams.h diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 69d72ad9b..05e4b481e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,11 +23,12 @@ #pragma once /* GTSAM includes */ -#include -#include -#include #include #include +#include +#include +#include +#include namespace gtsam { @@ -57,65 +58,6 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ -/// Parameters for pre-integration using PreintegratedCombinedMeasurements: -/// Usage: Create just a single Params and pass a shared pointer to the constructor -struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { - Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk - Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. - - /// Default constructor makes uninitialized params struct. - /// Used for serialization. - PreintegrationCombinedParams() - : 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) { - - } - - // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); - } - - void print(const std::string& s="") const override; - bool equals(const PreintegratedRotationParams& other, double tol) const override; - - void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } - void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } - void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } - - const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } - const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); - ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); - ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); - } - -public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; - /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h new file mode 100644 index 000000000..2c99463bc --- /dev/null +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationCombinedParams.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Varun Agrawal + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Parameters for pre-integration using PreintegratedCombinedMeasurements: +/// Usage: Create just a single Params and pass a shared pointer to the +/// constructor +struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { + Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing + ///< accelerometer bias random walk + Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing + ///< gyroscope bias random walk + Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. + + /// Default constructor makes uninitialized params struct. + /// Used for serialization. + PreintegrationCombinedParams() + : 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) {} + + // Default Params for a Z-down navigation frame, such as NED: gravity points + // along positive Z-axis + static boost::shared_ptr MakeSharedD( + double g = 9.81) { + return boost::shared_ptr( + new PreintegrationCombinedParams(Vector3(0, 0, g))); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points + // along negative Z-axis + static boost::shared_ptr MakeSharedU( + double g = 9.81) { + return boost::shared_ptr( + new PreintegrationCombinedParams(Vector3(0, 0, -g))); + } + + void print(const std::string& s = "") const override; + bool equals(const PreintegratedRotationParams& other, + double tol) const override; + + void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; } + void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; } + void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; } + + const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } + const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace gtsam