Loading...
Searching...
No Matches
PreintegrationBase.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/PreintegrationParams.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/linear/NoiseModel.h>
#include <iosfwd>
#include <string>
#include <utility>
namespace gtsam {
class GTSAM_EXPORT PreintegrationBase {
public:
typedef imuBias::ConstantBias Bias;
typedef PreintegrationParams Params;
protected:
boost::shared_ptr<Params> p_;
Bias biasHat_;
double deltaTij_;
PreintegrationBase() {}
virtual ~PreintegrationBase() {}
public:
PreintegrationBase(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
virtual void resetIntegration() = 0;
void resetIntegrationAndSetBias(const Bias& biasHat);
bool matchesParamsWith(const PreintegrationBase& other) const {
return p_.get() == other.p_.get();
}
const boost::shared_ptr<Params>& params() const {
return p_;
}
Params& p() const {
return *p_;
}
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
double deltaTij() const { return deltaTij_; }
virtual Vector3 deltaPij() const = 0;
virtual Vector3 deltaVij() const = 0;
virtual Rot3 deltaRij() const = 0;
virtual NavState deltaXij() const = 0;
// Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); }
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
virtual void print(const std::string& s="") const;
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
virtual void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt);
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const = 0;
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 6> H2 = boost::none) const;
Vector9 computeError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
OptionalJacobian<9, 6> H3) const;
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
boost::none, OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
private:
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
};
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
NoiseModel.h
PreintegrationParams.h
NavState.h
Navigation state composing of attitude, position, and velocity.
ImuBias.h
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::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegrationBase.h:53
gtsam::PreintegrationBase::computeErrorAndJacobians
Vector9 computeErrorAndJacobians(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H1=boost::none, OptionalJacobian< 9, 3 > H2=boost::none, OptionalJacobian< 9, 6 > H3=boost::none, OptionalJacobian< 9, 3 > H4=boost::none, OptionalJacobian< 9, 6 > H5=boost::none) const
Compute errors w.r.t.
Definition PreintegrationBase.cpp:168
gtsam::PreintegrationBase::computeError
Vector9 computeError(const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1, OptionalJacobian< 9, 9 > H2, OptionalJacobian< 9, 6 > H3) const
Calculate error given navStates.
Definition PreintegrationBase.cpp:142
gtsam::PreintegrationBase::~PreintegrationBase
virtual ~PreintegrationBase()
Virtual destructor for serialization.
Definition PreintegrationBase.h:59
gtsam::PreintegrationBase::matchesParamsWith
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegrationBase.h:86
gtsam::PreintegrationBase::biasCorrectedDelta
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const =0
Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU me...
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition PreintegrationBase.h:96
gtsam::PreintegrationBase::predict
NavState predict(const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 6 > H2=boost::none) const
Predict state at time j.
Definition PreintegrationBase.cpp:116
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition PreintegrationBase.h:50
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition PreintegrationBase.h:56
gtsam::PreintegrationBase::params
const boost::shared_ptr< Params > & params() const
shared pointer to params
Definition PreintegrationBase.h:91
gtsam::PreintegrationBase::update
virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0
Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Mod...
gtsam::PreintegrationBase::integrateMeasurement
virtual void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt)
Version without derivatives.
Definition PreintegrationBase.cpp:106
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26