Loading...
Searching...
No Matches
TangentPreintegration.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/PreintegrationBase.h>
namespace gtsam {
class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
protected:
Vector9 preintegrated_;
Matrix93 preintegrated_H_biasAcc_;
Matrix93 preintegrated_H_biasOmega_;
TangentPreintegration() {
resetIntegration();
}
public:
TangentPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
~TangentPreintegration() override {
}
void resetIntegration() override;
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
NavState deltaXij() const override { return NavState().retract(preintegrated_); }
const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
bool equals(const TangentPreintegration& other, double tol) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, const double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
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;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
virtual boost::shared_ptr<TangentPreintegration> clone() const {
return boost::shared_ptr<TangentPreintegration>();
}
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(preintegrated_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
PreintegrationBase.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition PreintegrationBase.h:56
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition TangentPreintegration.h:28
gtsam::TangentPreintegration::preintegrated_H_biasOmega_
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition TangentPreintegration.h:37
gtsam::TangentPreintegration::~TangentPreintegration
~TangentPreintegration() override
Virtual destructor.
Definition TangentPreintegration.h:57
gtsam::TangentPreintegration::preintegrated_
Vector9 preintegrated_
Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
Definition TangentPreintegration.h:35
gtsam::TangentPreintegration::TangentPreintegration
TangentPreintegration()
Default constructor for serialization.
Definition TangentPreintegration.h:40
gtsam::TangentPreintegration::preintegrated_H_biasAcc_
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition TangentPreintegration.h:36
gtsam::TangentPreintegration::clone
virtual boost::shared_ptr< TangentPreintegration > clone() const
Dummy clone for MATLAB.
Definition TangentPreintegration.h:123