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: