Loading...
Searching...
No Matches
PreintegratedRotation.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/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
struct GTSAM_EXPORT PreintegratedRotationParams {
Matrix3 gyroscopeCovariance;
boost::optional<Vector3> omegaCoriolis;
boost::optional<Pose3> body_P_sensor;
PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
boost::optional<Vector3> omega_coriolis)
: gyroscopeCovariance(gyroscope_covariance) {
if (omega_coriolis)
omegaCoriolis.reset(omega_coriolis.get());
}
virtual ~PreintegratedRotationParams() {}
virtual void print(const std::string& s) const;
virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
// Provide support for Eigen::Matrix in boost::optional
bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
if (omegaCoriolisFlag) {
ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
}
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
class GTSAM_EXPORT PreintegratedRotation {
public:
typedef PreintegratedRotationParams Params;
protected:
boost::shared_ptr<Params> p_;
double deltaTij_;
Rot3 deltaRij_;
Matrix3 delRdelBiasOmega_;
PreintegratedRotation() {}
public:
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
resetIntegration();
}
PreintegratedRotation(const boost::shared_ptr<Params>& p,
double deltaTij, const Rot3& deltaRij,
const Matrix3& delRdelBiasOmega)
: p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
void resetIntegration();
bool matchesParamsWith(const PreintegratedRotation& other) const {
return p_ == other.p_;
}
const boost::shared_ptr<Params>& params() const {
return p_;
}
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaRij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation& other, double tol) const;
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
OptionalJacobian<3, 3> F = boost::none);
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const;
Vector3 integrateCoriolis(const Rot3& rot_i) const;
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
ar& BOOST_SERIALIZATION_NVP(p_);
ar& BOOST_SERIALIZATION_NVP(deltaTij_);
ar& BOOST_SERIALIZATION_NVP(deltaRij_);
ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
}
Matrix.h
typedef and functions to augment Eigen's MatrixXd
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Pose3.h
3D Pose
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::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::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
gtsam::PreintegratedRotationParams::body_P_sensor
boost::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Definition PreintegratedRotation.h:36
gtsam::PreintegratedRotationParams::omegaCoriolis
boost::optional< Vector3 > omegaCoriolis
Coriolis constant.
Definition PreintegratedRotation.h:35
gtsam::PreintegratedRotationParams::gyroscopeCovariance
Matrix3 gyroscopeCovariance
Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
Definition PreintegratedRotation.h:34
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition PreintegratedRotation.h:89
gtsam::PreintegratedRotation::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition PreintegratedRotation.h:99
gtsam::PreintegratedRotation::p_
boost::shared_ptr< Params > p_
Parameters.
Definition PreintegratedRotation.h:95
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
Explicit initialization of all class members.
Definition PreintegratedRotation.h:114
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p)
Default constructor, resets integration to zero.
Definition PreintegratedRotation.h:109
gtsam::PreintegratedRotation::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegratedRotation.h:97
gtsam::PreintegratedRotation::matchesParamsWith
bool matchesParamsWith(const PreintegratedRotation &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegratedRotation.h:128
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation()
Default constructor for serialization.
Definition PreintegratedRotation.h:102
gtsam::PreintegratedRotation::deltaRij_
Rot3 deltaRij_
Preintegrated relative orientation (in frame i).
Definition PreintegratedRotation.h:98
gtsam::PreintegratedRotation::resetIntegration
void resetIntegration()
Re-initialize PreintegratedMeasurements.
Definition PreintegratedRotation.cpp:51