diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 4acccb578..7664c7fd5 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -201,7 +201,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 855c14365..46306f8c7 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -128,8 +128,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); + ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; @@ -167,7 +168,7 @@ public: #endif /** Default constructor - only use for serialization */ - ImuFactor(); + ImuFactor() {} /** * Constructor @@ -241,4 +242,7 @@ private: }; // class ImuFactor +/// traits +template<> struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 002afea76..eb25b35e2 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -52,7 +52,8 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 07225dd9a..c53a8c878 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -84,15 +84,17 @@ public: protected: /// Default constructor for serialization only: uninitialized! - Params(); + Params() {} /** 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(accelerometerCovariance); - ar & BOOST_SERIALIZATION_NVP(integrationCovariance); + namespace bs = ::boost::serialization; + ar & boost::serialization::make_nvp("PreintegratedRotation_Params", + boost::serialization::base_object(*this)); + 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); ar & BOOST_SERIALIZATION_NVP(n_gravity); } @@ -246,15 +248,16 @@ private: friend class boost::serialization::access; 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_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(biasHat_); - 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_); + 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())); } }; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 92cb92e70..25a6e732c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -1010,6 +1010,52 @@ TEST(ImuFactor, bodyPSensorWithBias) { EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); } +/* ************************************************************************** */ +#include + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "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; + + Vector3 n_gravity(0, 0, -9.81); + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, + integrationCov, true); + + // measurements are needed for non-inf noise model, otherwise will throw err when deserialize + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr;