Loading...
Searching...
No Matches
CombinedImuFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.h>
namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* conditionally independent sets in factor graphs: a unifying perspective based
* on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
* Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
* High-Dynamic Motion in Built Environments Without Initial Conditions",
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* Available in this repo as "PreintegratedIMUJacobians.pdf".
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
* Robotics: Science and Systems (RSS), 2015.
*/
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasAccCovariance;
Matrix3 biasOmegaCovariance;
Matrix6 biasAccOmegaInt;
PreintegrationCombinedParams()
: 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
static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
return boost::shared_ptr<PreintegrationCombinedParams>(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<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
return boost::shared_ptr<PreintegrationCombinedParams>(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:
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(PreintegrationParams);
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
public:
typedef PreintegrationCombinedParams Params;
protected:
/* Covariance matrix of the preintegrated measurements
* COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
* (first-order propagation from *measurementCovariance*).
* PreintegratedCombinedMeasurements also include the biases and keep the correlation
* between the preintegrated measurements and the biases
*/
Eigen::Matrix<double, 15, 15> preintMeasCov_;
friend class CombinedImuFactor;
public:
PreintegratedCombinedMeasurements() {
preintMeasCov_.setZero();
}
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
~PreintegratedCombinedMeasurements() override {}
void resetIntegration() override;
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
Matrix preintMeasCov() const { return preintMeasCov_; }
void print(const std::string& s = "Preintegrated Measurements:") const override;
bool equals(const PreintegratedCombinedMeasurements& expected,
double tol = 1e-9) const;
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt) override;
private:
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_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public:
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base;
PreintegratedCombinedMeasurements _PIM_;
public:
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
CombinedImuFactor() {}
CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& preintegratedMeasurements);
~CombinedImuFactor() override {}
gtsam::NonlinearFactor::shared_ptr clone() const override;
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&);
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(_PIM_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// class CombinedImuFactor
template <>
struct traits<PreintegrationCombinedParams>
: public Testable<PreintegrationCombinedParams> {};
template <>
struct traits<PreintegratedCombinedMeasurements>
: public Testable<PreintegratedCombinedMeasurements> {};
template <>
struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
} // namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
serialization.h
Convenience functions for serializing data structures via boost.serialization.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
TangentPreintegration.h
ManifoldPreintegration.h
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::PreintegrationCombinedParams
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition CombinedImuFactor.h:62
gtsam::PreintegrationCombinedParams::biasAccOmegaInt
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition CombinedImuFactor.h:65
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition CombinedImuFactor.h:75
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition CombinedImuFactor.h:69
gtsam::PreintegrationCombinedParams::biasOmegaCovariance
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition CombinedImuFactor.h:64
gtsam::PreintegrationCombinedParams::biasAccCovariance
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition CombinedImuFactor.h:63
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition CombinedImuFactor.h:171
gtsam::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition CombinedImuFactor.h:177
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition CombinedImuFactor.h:150
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition CombinedImuFactor.h:188
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition CombinedImuFactor.h:159
gtsam::CombinedImuFactor
CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
Definition CombinedImuFactor.h:259
gtsam::CombinedImuFactor::shared_ptr
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition CombinedImuFactor.h:276
gtsam::CombinedImuFactor::CombinedImuFactor
CombinedImuFactor()
Default constructor - only use for serialization.
Definition CombinedImuFactor.h:280
gtsam::CombinedImuFactor::preintegratedMeasurements
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition CombinedImuFactor.h:317
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams()
Default constructor for serialization only.
Definition PreintegrationParams.h:35
gtsam::PreintegrationParams::n_gravity
Vector3 n_gravity
Gravity vector in nav frame.
Definition PreintegrationParams.h:32
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition TangentPreintegration.h:28
gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias, imuBias::ConstantBias >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469