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 David Jensen
* @author Frank Dellaert
* @author Varun Agrawal
**/
#include <gtsam/navigation/CombinedImuFactor.h>
#include <boost/serialization/export.hpp>
/* External or standard includes */
#include <ostream>
@ -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<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
//------------------------------------------------------------------------------
@ -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(

101
gtsam/navigation/CombinedImuFactor.h
View File

@ -17,6 +17,7 @@
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
* @author Varun Agrawal
**/
#pragma once
@ -26,6 +27,7 @@
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.h>
namespace gtsam {
@ -61,10 +63,18 @@ 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 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) {
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 +87,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
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 setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
@ -86,24 +99,22 @@ 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 <class ARCHIVE>
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 & 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.
@ -128,7 +139,6 @@ public:
*/
Eigen::Matrix<double, 15, 15> preintMeasCov_;
friend class CombinedImuFactor;
public:
@ -136,11 +146,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<Params>& p,
@ -149,6 +162,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<double, 15, 15>& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
/// Virtual destructor
virtual ~PreintegratedCombinedMeasurements() {}
/// @}
/// @name Basic utilities
@ -158,20 +184,25 @@ public:
void resetIntegration() override;
/// 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
/// @{
/// 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,6 +236,7 @@ public:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
@ -244,9 +276,6 @@ private:
PreintegratedCombinedMeasurements _PIM_;
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
public:
/** Shorthand for a smart pointer to a factor */
@ -256,6 +285,9 @@ public:
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
/**
* Constructor
* @param pose_i Previous pose key
@ -277,12 +309,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 +358,12 @@ public:
#endif
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
ar& BOOST_SERIALIZATION_NVP(_PIM_);
}
public:
@ -336,4 +371,18 @@ public:
};
// 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.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
@ -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<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :

4
gtsam/navigation/ManifoldPreintegration.h
View File

@ -118,10 +118,8 @@ private:
template<class ARCHIVE>
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 & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);

6
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -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))

10
gtsam/navigation/PreintegrationBase.h
View File

@ -213,6 +213,16 @@ class GTSAM_EXPORT PreintegrationBase {
/// @}
#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:
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 {
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<const PreintegrationParams*>(&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) &&

13
gtsam/navigation/PreintegrationParams.h
View File

@ -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<PreintegrationParams>(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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

4
gtsam/navigation/TangentPreintegration.h
View File

@ -132,9 +132,7 @@ private:
template<class ARCHIVE>
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 & BOOST_SERIALIZATION_NVP(preintegrated_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);

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

@ -16,15 +16,19 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
*/
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <fstream>
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 <typename P>
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<PreintegratedImuMeasurements>();
EXPECT(equalsObj<PreintegratedImuMeasurements>(pim));
EXPECT(equalsXML<PreintegratedImuMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedImuMeasurements>(pim));
ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj<ImuFactor>(factor));
@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) {
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() {
TestResult tr;