53 const Vector3& biasHat) :
68 const boost::shared_ptr<Params>& p,
69 const Vector3& bias_hat,
72 const Matrix3& delRdelBiasOmega,
73 const Matrix3& preint_meas_cov) :
76 preintMeasCov_(preint_meas_cov) {}
78 Params& p()
const {
return *boost::static_pointer_cast<Params>(p_);}
79 const Vector3& biasHat()
const {
return biasHat_; }
80 const Matrix3& preintMeasCov()
const {
return preintMeasCov_; }
83 void print(
const std::string& s =
"Preintegrated Measurements: ")
const;
86 bool equals(
const PreintegratedAhrsMeasurements&,
double tol = 1e-9)
const;
89 void resetIntegration();
100 void integrateMeasurement(
const Vector3& measuredOmega,
double deltaT);
104 Vector3 predict(
const Vector3& bias, OptionalJacobian<3,3> H = boost::none)
const;
108 static Vector DeltaAngles(
const Vector& msr_gyro_t,
const double msr_dt,
109 const Vector3& delta_angles);
113 const Matrix3& measuredOmegaCovariance)
115 p_->gyroscopeCovariance = measuredOmegaCovariance;
122 friend class boost::serialization::access;
123 template<
class ARCHIVE>
124 void serialize(ARCHIVE & ar,
const unsigned int ) {
126 ar & BOOST_SERIALIZATION_NVP(p_);
127 ar & BOOST_SERIALIZATION_NVP(biasHat_);
144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
145 typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
164 gtsam::NonlinearFactor::shared_ptr clone()
const override;
168 DefaultKeyFormatter)
const override;
181 Vector evaluateError(
const Rot3& rot_i,
const Rot3& rot_j,
182 const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
183 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
184 boost::none)
const override;
188 static Rot3 Predict(
const Rot3& rot_i,
const Vector3& bias,
194 const Vector3& omegaCoriolis,
195 const boost::optional<Pose3>& body_P_sensor = boost::none);
199 const Rot3& rot_i,
const Vector3& bias,
201 const boost::optional<Pose3>& body_P_sensor = boost::none);
203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
212 friend class boost::serialization::access;
213 template<
class ARCHIVE>
214 void serialize(ARCHIVE & ar,
const unsigned int ) {
217 & boost::serialization::make_nvp(
"NoiseModelFactor3",
218 boost::serialization::base_object<Base>(*
this));
219 ar & BOOST_SERIALIZATION_NVP(_PIM_);
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
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
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
Template to create a binary predicate.
Definition: Testable.h:111
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition: AHRSFactor.h:34
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition: AHRSFactor.h:38
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition: AHRSFactor.h:112
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition: AHRSFactor.h:52
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition: AHRSFactor.h:39
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
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition: AHRSFactor.h:46
Definition: AHRSFactor.h:131
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: AHRSFactor.h:174
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: AHRSFactor.h:147
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegratedRotation.h:31
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition: PreintegratedRotation.h:89
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400