12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #386 from borglab/fix/imu-factor-serialization 

CombinedImuFactor Serialization Fix
release/4.3a0
Varun Agrawal 2020-07-11 15:35:26 -04:00 committed by GitHub
parent 0d4f4bf06e 2c67f6fd11
commit 54a354b104
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 186 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -17,9 +17,11 @@
* @author Vadim Indelman * @author Vadim Indelman
* @author David Jensen * @author David Jensen
* @author Frank Dellaert * @author Frank Dellaert
* @author Varun Agrawal
**/ **/
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
#include <boost/serialization/export.hpp>
/* External or standard includes */ /* External or standard includes */
#include <ostream> #include <ostream>
@ -28,6 +30,31 @@ namespace gtsam {
using namespace std; 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<const PreintegrationCombinedParams*>(&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 // Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
return r; 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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
CombinedImuFactor::CombinedImuFactor( CombinedImuFactor::CombinedImuFactor(

101
gtsam/navigation/CombinedImuFactor.h
View File

@ -17,6 +17,7 @@
* @author Vadim Indelman * @author Vadim Indelman
* @author David Jensen * @author David Jensen
* @author Frank Dellaert * @author Frank Dellaert
* @author Varun Agrawal
**/ **/
#pragma once #pragma once
@ -26,6 +27,7 @@
#include <gtsam/navigation/TangentPreintegration.h> #include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.h>
namespace gtsam { namespace gtsam {
@ -61,10 +63,18 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
/// 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 /// See two named constructors below for good values of n_gravity in body frame
PreintegrationCombinedParams(const Vector3& n_gravity) : PreintegrationCombinedParams(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
I_3x3), biasAccOmegaInt(I_6x6) { biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
} }
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
@ -77,6 +87,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g))); return boost::shared_ptr<PreintegrationCombinedParams>(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 setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
@ -86,24 +99,22 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
private: private:
/// Default constructor makes unitialized params struct
PreintegrationCombinedParams() {}
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); namespace bs = ::boost::serialization;
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
} }
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**
* PreintegratedCombinedMeasurements integrates the IMU measurements * PreintegratedCombinedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix. * (rotation rates and accelerations) and the corresponding covariance matrix.
@ -128,7 +139,6 @@ public:
*/ */
Eigen::Matrix<double, 15, 15> preintMeasCov_; Eigen::Matrix<double, 15, 15> preintMeasCov_;
friend class CombinedImuFactor; friend class CombinedImuFactor;
public: public:
@ -136,11 +146,14 @@ public:
/// @{ /// @{
/// Default constructor only for serialization and Cython wrapper /// Default constructor only for serialization and Cython wrapper
PreintegratedCombinedMeasurements() {} PreintegratedCombinedMeasurements() {
preintMeasCov_.setZero();
}
/** /**
* Default constructor, initializes the class with no measurements * 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( PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p, const boost::shared_ptr<Params>& p,
@ -149,6 +162,19 @@ public:
preintMeasCov_.setZero(); 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<double, 15, 15>& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
/// Virtual destructor
virtual ~PreintegratedCombinedMeasurements() {}
/// @} /// @}
/// @name Basic utilities /// @name Basic utilities
@ -158,20 +184,25 @@ public:
void resetIntegration() override; void resetIntegration() override;
/// const reference to params, shadows definition in base class /// const reference to params, shadows definition in base class
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);} Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
/// @} /// @}
/// @name Access instance variables /// @name Access instance variables
/// @{ /// @{
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/// print
void print(const std::string& s = "Preintegrated Measurements:") const override; 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 /// @name Main functionality
/// @{ /// @{
@ -205,6 +236,7 @@ public:
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
} }
@ -244,9 +276,6 @@ private:
PreintegratedCombinedMeasurements _PIM_; PreintegratedCombinedMeasurements _PIM_;
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
public: public:
/** Shorthand for a smart pointer to a factor */ /** Shorthand for a smart pointer to a factor */
@ -256,6 +285,9 @@ public:
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr; typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif #endif
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
/** /**
* Constructor * Constructor
* @param pose_i Previous pose key * @param pose_i Previous pose key
@ -277,12 +309,17 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&);
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const; DefaultKeyFormatter) const;
/// equals /// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/// @}
/** Access the preintegrated measurements. */ /** Access the preintegrated measurements. */
@ -321,14 +358,12 @@ public:
#endif #endif
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor6", ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
boost::serialization::base_object<Base>(*this)); ar& BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
} }
public: public:
@ -336,4 +371,18 @@ public:
}; };
// class CombinedImuFactor // class CombinedImuFactor
} /// namespace gtsam template <>
struct traits<PreintegrationCombinedParams>
: public Testable<PreintegrationCombinedParams> {};
template <>
struct traits<PreintegratedCombinedMeasurements>
: public Testable<PreintegratedCombinedMeasurements> {};
template <>
struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
} // namespace gtsam
/// Add Boost serialization export for derived class
BOOST_CLASS_EXPORT_GUID(gtsam::PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams");

6
gtsam/navigation/ImuFactor.h
View File

@ -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. * (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor. * The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement * Integration is done incrementally (ideally, one integrates the measurement
@ -87,8 +87,8 @@ public:
/** /**
* Constructor, initializes the class with no measurements * 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<PreintegrationParams>& p, PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :

4
gtsam/navigation/ManifoldPreintegration.h
View File

@ -118,10 +118,8 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization; namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);

6
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -25,7 +25,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
void PreintegratedRotation::Params::print(const string& s) const { void PreintegratedRotationParams::print(const string& s) const {
cout << s << endl; cout << s << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis) if (omegaCoriolis)
@ -34,8 +34,8 @@ void PreintegratedRotation::Params::print(const string& s) const {
body_P_sensor->print("body_P_sensor"); body_P_sensor->print("body_P_sensor");
} }
bool PreintegratedRotation::Params::equals( bool PreintegratedRotationParams::equals(
const PreintegratedRotation::Params& other, double tol) const { const PreintegratedRotationParams& other, double tol) const {
if (body_P_sensor) { if (body_P_sensor) {
if (!other.body_P_sensor if (!other.body_P_sensor
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol)) || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))

10
gtsam/navigation/PreintegrationBase.h
View File

@ -213,6 +213,16 @@ class GTSAM_EXPORT PreintegrationBase {
/// @} /// @}
#endif #endif
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
}
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };

6
gtsam/navigation/PreintegrationParams.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegrationParams::print(const string& s) const { void PreintegrationParams::print(const string& s) const {
PreintegratedRotation::Params::print(s); PreintegratedRotationParams::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl; << endl;
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" 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 { double tol) const {
auto e = dynamic_cast<const PreintegrationParams*>(&other); auto e = dynamic_cast<const PreintegrationParams*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol) && return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
use2ndOrderCoriolis == e->use2ndOrderCoriolis && use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol) && tol) &&

13
gtsam/navigation/PreintegrationParams.h
View File

@ -31,7 +31,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
/// Default constructor for serialization only /// Default constructor for serialization only
PreintegrationParams() PreintegrationParams()
: accelerometerCovariance(I_3x3), : PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3), integrationCovariance(I_3x3),
use2ndOrderCoriolis(false), use2ndOrderCoriolis(false),
n_gravity(0, 0, -1) {} 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 /// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below /// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity) PreintegrationParams(const Vector3& n_gravity)
: accelerometerCovariance(I_3x3), : PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3), integrationCovariance(I_3x3),
use2ndOrderCoriolis(false), use2ndOrderCoriolis(false),
n_gravity(n_gravity) {} n_gravity(n_gravity) {}
@ -54,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g))); return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
} }
void print(const std::string& s) const; void print(const std::string& s="") const;
bool equals(const PreintegratedRotation::Params& other, double tol) const; bool equals(const PreintegratedRotationParams& other, double tol) const;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
@ -73,8 +75,7 @@ protected:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization; namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params", ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance); ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

4
gtsam/navigation/TangentPreintegration.h
View File

@ -132,9 +132,7 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization; namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_); ar & BOOST_SERIALIZATION_NVP(preintegrated_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_); ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_); ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);

60
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -16,15 +16,19 @@
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @author Stephen Williams * @author Stephen Williams
* @author Varun Agrawal
*/ */
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <fstream> #include <fstream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"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::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(ImuFactor, serialization) { template <typename P>
using namespace gtsam::serializationTestHelpers; P getPreintegratedMeasurements() {
// Create default parameters with Z-down and above noise paramaters // Create default parameters with Z-down and above noise paramaters
auto p = PreintegrationParams::MakeSharedD(9.81); auto p = P::Params::MakeSharedD(9.81);
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
p->accelerometerCovariance = 1e-7 * I_3x3; p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3;
const double deltaT = 0.005; 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 // when deserialize
const Vector3 measuredOmega(0, 0.01, 0); const Vector3 measuredOmega(0, 0.01, 0);
const Vector3 measuredAcc(0, 0, -9.81); const Vector3 measuredAcc(0, 0, -9.81);
@ -62,6 +66,16 @@ TEST(ImuFactor, serialization) {
for (int j = 0; j < 200; ++j) for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
return pim;
}
TEST(ImuFactor, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
EXPECT(equalsObj<PreintegratedImuMeasurements>(pim));
EXPECT(equalsXML<PreintegratedImuMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedImuMeasurements>(pim));
ImuFactor factor(1, 2, 3, 4, 5, pim); ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj<ImuFactor>(factor)); EXPECT(equalsObj<ImuFactor>(factor));
@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) {
EXPECT(equalsBinary<ImuFactor>(factor)); EXPECT(equalsBinary<ImuFactor>(factor));
} }
TEST(ImuFactor2, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
ImuFactor2 factor(1, 2, 3, pim);
EXPECT(equalsObj<ImuFactor2>(factor));
EXPECT(equalsXML<ImuFactor2>(factor));
EXPECT(equalsBinary<ImuFactor2>(factor));
}
TEST(CombinedImuFactor, Serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
EXPECT(equalsObj<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsXML<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedCombinedMeasurements>(pim));
const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim);
EXPECT(equalsObj<CombinedImuFactor>(factor));
EXPECT(equalsXML<CombinedImuFactor>(factor));
EXPECT(equalsBinary<CombinedImuFactor>(factor));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;