Loading...
Searching...
No Matches
ManifoldPreintegration.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
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
protected:
NavState deltaXij_;
Matrix3 delRdelBiasOmega_;
Matrix3 delPdelBiasAcc_;
Matrix3 delPdelBiasOmega_;
Matrix3 delVdelBiasAcc_;
Matrix3 delVdelBiasOmega_;
ManifoldPreintegration() {
resetIntegration();
}
public:
ManifoldPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
void resetIntegration() override;
NavState deltaXij() const override { return deltaXij_; }
Rot3 deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); }
Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
bool equals(const ManifoldPreintegration& other, double tol) const;
void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override;
virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
return boost::shared_ptr<ManifoldPreintegration>();
}
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(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
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_);
}
};
}
NavState.h
Navigation state composing of attitude, position, and velocity.
PreintegrationBase.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
gtsam::ManifoldPreintegration::ManifoldPreintegration
ManifoldPreintegration()
Default constructor for serialization.
Definition ManifoldPreintegration.h:49
gtsam::ManifoldPreintegration::delVdelBiasAcc_
Matrix3 delVdelBiasAcc_
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:45
gtsam::ManifoldPreintegration::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:42
gtsam::ManifoldPreintegration::delPdelBiasAcc_
Matrix3 delPdelBiasAcc_
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:43
gtsam::ManifoldPreintegration::deltaXij_
NavState deltaXij_
Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
Definition ManifoldPreintegration.h:41
gtsam::ManifoldPreintegration::delPdelBiasOmega_
Matrix3 delPdelBiasOmega_
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:44
gtsam::ManifoldPreintegration::clone
virtual boost::shared_ptr< ManifoldPreintegration > clone() const
Dummy clone for MATLAB.
Definition ManifoldPreintegration.h:109
gtsam::ManifoldPreintegration::delVdelBiasOmega_
Matrix3 delVdelBiasOmega_
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:46
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition PreintegrationBase.h:56