Loading...
Searching...
No Matches
ImuFactor.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/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/debug.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.
*
* 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.
*
* 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.
*/
class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
friend class ImuFactor;
friend class ImuFactor2;
protected:
Matrix9 preintMeasCov_;
public:
PreintegratedImuMeasurements() {
preintMeasCov_.setZero();
}
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
~PreintegratedImuMeasurements() override {
}
void print(const std::string& s = "Preintegrated Measurements:") const override;
bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
void resetIntegration() override;
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt) override;
void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts);
Matrix preintMeasCov() const { return preintMeasCov_; }
#ifdef GTSAM_TANGENT_PREINTEGRATION
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#endif
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_);
}
};
class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> {
private:
typedef ImuFactor This;
typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_;
public:
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
ImuFactor() {}
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedImuMeasurements& preintegratedMeasurements);
~ImuFactor() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override;
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
const PreintegratedImuMeasurements& 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, 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) const override;
#ifdef GTSAM_TANGENT_PREINTEGRATION
static PreintegratedImuMeasurements Merge(
const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12);
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#endif
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
}
};
// class ImuFactor
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
private:
typedef ImuFactor2 This;
typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_;
public:
ImuFactor2() {}
ImuFactor2(Key state_i, Key state_j, Key bias,
const PreintegratedImuMeasurements& preintegratedMeasurements);
~ImuFactor2() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override;
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
const PreintegratedImuMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
Vector evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
}
};
// class ImuFactor2
template <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {};
template <>
struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
}
debug.h
Global debugging flags.
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::equals
Template to create a binary predicate.
Definition Testable.h:111
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::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
gtsam::PreintegratedImuMeasurements::~PreintegratedImuMeasurements
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition ImuFactor.h:111
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition ImuFactor.h:94
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition ImuFactor.h:105
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition ImuFactor.h:85
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition ImuFactor.h:141
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition ImuFactor.h:79
gtsam::ImuFactor
ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
Definition ImuFactor.h:172
gtsam::ImuFactor::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:222
gtsam::ImuFactor::ImuFactor
ImuFactor()
Default constructor - only use for serialization.
Definition ImuFactor.h:191
gtsam::ImuFactor::shared_ptr
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition ImuFactor.h:187
gtsam::ImuFactor2
ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
Definition ImuFactor.h:263
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Default constructor - only use for serialization.
Definition ImuFactor.h:274
gtsam::ImuFactor2::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:301
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition PreintegrationBase.h:96
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition TangentPreintegration.h:28
gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469