Loading...
Searching...
No Matches
AHRSFactor.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/navigation/PreintegratedRotation.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
protected:
Vector3 biasHat_;
Matrix3 preintMeasCov_;
friend class AHRSFactor;
public:
PreintegratedAhrsMeasurements() {}
PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
const Vector3& biasHat) :
PreintegratedRotation(p), biasHat_(biasHat) {
resetIntegration();
}
PreintegratedAhrsMeasurements(
const boost::shared_ptr<Params>& p,
const Vector3& bias_hat,
double deltaTij,
const Rot3& deltaRij,
const Matrix3& delRdelBiasOmega,
const Matrix3& preint_meas_cov) :
PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
biasHat_(bias_hat),
preintMeasCov_(preint_meas_cov) {}
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
const Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; }
void print(const std::string& s = "Preintegrated Measurements: ") const;
bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
void resetIntegration();
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
PreintegratedAhrsMeasurements(const Vector3& biasHat,
const Matrix3& measuredOmegaCovariance)
: PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
p_->gyroscopeCovariance = measuredOmegaCovariance;
resetIntegration();
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
}
};
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
typedef AHRSFactor This;
typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
PreintegratedAhrsMeasurements _PIM_;
AHRSFactor() {}
public:
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
#else
typedef boost::shared_ptr<AHRSFactor> shared_ptr;
#endif
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& preintegratedMeasurements);
~AHRSFactor() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override;
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const override;
static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim);
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
static Rot3 predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
#endif
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_);
}
};
// AHRSFactor
} //namespace gtsam
Pose3.h
3D Pose
PreintegratedRotation.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::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::PreintegratedAhrsMeasurements
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition AHRSFactor.h:34
gtsam::PreintegratedAhrsMeasurements::biasHat_
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition AHRSFactor.h:38
gtsam::PreintegratedAhrsMeasurements::resetIntegration
void resetIntegration()
Reset inetgrated quantities to zero.
Definition AHRSFactor.cpp:44
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition AHRSFactor.h:112
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition AHRSFactor.h:52
gtsam::PreintegratedAhrsMeasurements::preintMeasCov_
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition AHRSFactor.h:39
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
Non-Default constructor, initialize with measurements.
Definition AHRSFactor.h:67
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition AHRSFactor.h:46
gtsam::AHRSFactor
Definition AHRSFactor.h:131
gtsam::AHRSFactor::preintegratedMeasurements
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition AHRSFactor.h:174
gtsam::AHRSFactor::shared_ptr
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition AHRSFactor.h:147
gtsam::PreintegratedRotation::p_
boost::shared_ptr< Params > p_
Parameters.
Definition PreintegratedRotation.h:95
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation()
Default constructor for serialization.
Definition PreintegratedRotation.h:102
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Rot3, Rot3, Vector3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469