AHRSFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
22 /* GTSAM includes */
25 #include <gtsam/geometry/Pose3.h>
26 
27 #include <optional>
28 
29 namespace gtsam {
30 
37 
38  protected:
39 
41  Matrix3 preintMeasCov_;
42 
43  friend class AHRSFactor;
44 
45  public:
46 
49 
54  PreintegratedAhrsMeasurements(const std::shared_ptr<Params>& p,
55  const Vector3& biasHat) :
56  PreintegratedRotation(p), biasHat_(biasHat) {
57  resetIntegration();
58  }
59 
70  const std::shared_ptr<Params>& p,
71  const Vector3& bias_hat,
72  double deltaTij,
73  const Rot3& deltaRij,
74  const Matrix3& delRdelBiasOmega,
75  const Matrix3& preint_meas_cov) :
76  PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
77  biasHat_(bias_hat),
78  preintMeasCov_(preint_meas_cov) {}
79 
80  Params& p() const { return *std::static_pointer_cast<Params>(p_);}
81  const Vector3& biasHat() const { return biasHat_; }
82  const Matrix3& preintMeasCov() const { return preintMeasCov_; }
83 
85  void print(const std::string& s = "Preintegrated Measurements: ") const;
86 
88  bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
89 
91  void resetIntegration();
92 
102  void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
103 
106  Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = {}) const;
107 
108  // This function is only used for test purposes
109  // (compare numerical derivatives wrt analytic ones)
110  static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
111  const Vector3& delta_angles);
112 
115  const Matrix3& measuredOmegaCovariance)
116  : PreintegratedRotation(std::make_shared<Params>()), biasHat_(biasHat) {
117  p_->gyroscopeCovariance = measuredOmegaCovariance;
118  resetIntegration();
119  }
120 
121 private:
122 
123 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
124 
125  friend class boost::serialization::access;
126  template<class ARCHIVE>
127  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
128  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
129  ar & BOOST_SERIALIZATION_NVP(p_);
130  ar & BOOST_SERIALIZATION_NVP(biasHat_);
131  }
132 #endif
133 };
134 
135 class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
136 
137  typedef AHRSFactor This;
139 
141 
142 public:
143 
144  // Provide access to the Matrix& version of evaluateError:
145  using Base::evaluateError;
146 
148 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
149  typedef typename std::shared_ptr<AHRSFactor> shared_ptr;
150 #else
151  typedef std::shared_ptr<AHRSFactor> shared_ptr;
152 #endif
153 
156 
164  AHRSFactor(Key rot_i, Key rot_j, Key bias,
165  const PreintegratedAhrsMeasurements& preintegratedMeasurements);
166 
167  ~AHRSFactor() override {
168  }
169 
171  gtsam::NonlinearFactor::shared_ptr clone() const override;
172 
174  void print(const std::string& s, const KeyFormatter& keyFormatter =
175  DefaultKeyFormatter) const override;
176 
178  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
179 
182  return _PIM_;
183  }
184 
187  Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
189  const Vector3& bias, OptionalMatrixType H1,
190  OptionalMatrixType H2, OptionalMatrixType H3) const override;
191 
194  static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
195  const PreintegratedAhrsMeasurements& pim);
196 
198  AHRSFactor(Key rot_i, Key rot_j, Key bias,
200  const Vector3& omegaCoriolis,
201  const std::optional<Pose3>& body_P_sensor = {});
202 
204  static Rot3 predict(
205  const Rot3& rot_i, const Vector3& bias,
206  const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
207  const std::optional<Pose3>& body_P_sensor = {});
208 
209 private:
210 
211 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
212 
213  friend class boost::serialization::access;
214  template<class ARCHIVE>
215  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
216  // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
217  ar
218  & boost::serialization::make_nvp("NoiseModelFactor3",
219  boost::serialization::base_object<Base>(*this));
220  ar & BOOST_SERIALIZATION_NVP(_PIM_);
221  }
222 #endif
223 
224 };
225 // AHRSFactor
226 
227 } //namespace gtsam
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const std::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
Definition: AHRSFactor.h:69
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::AHRSFactor::AHRSFactor
AHRSFactor()
Definition: AHRSFactor.h:155
gtsam::AHRSFactor
Definition: AHRSFactor.h:135
gtsam::PreintegratedRotationParams
Definition: PreintegratedRotation.h:57
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::AHRSFactor::preintegratedMeasurements
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: AHRSFactor.h:181
gtsam::PreintegratedAhrsMeasurements::biasHat
const Vector3 & biasHat() const
Definition: AHRSFactor.h:81
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
common::deltaT
static const double deltaT
Definition: testImuFactor.cpp:183
gtsam::PreintegratedAhrsMeasurements::biasHat_
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition: AHRSFactor.h:40
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
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
Definition: AHRSFactor.h:36
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::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
gtsam::PreintegratedAhrsMeasurements::preintMeasCov_
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition: AHRSFactor.h:41
gtsam::AHRSFactor::_PIM_
PreintegratedAhrsMeasurements _PIM_
Definition: AHRSFactor.h:140
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const std::shared_ptr< Params > &p, const Vector3 &biasHat)
Definition: AHRSFactor.h:54
PreintegratedRotation.h
gtsam::AHRSFactor::Base
NoiseModelFactorN< Rot3, Rot3, Vector3 > Base
Definition: AHRSFactor.h:138
gtsam::AHRSFactor::This
AHRSFactor This
Definition: AHRSFactor.h:137
gtsam::equals
Definition: Testable.h:112
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PreintegratedAhrsMeasurements::preintMeasCov
const Matrix3 & preintMeasCov() const
Definition: AHRSFactor.h:82
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition: AHRSFactor.h:48
gtsam
traits
Definition: SFMdata.h:40
gtsam::AHRSFactor::shared_ptr
std::shared_ptr< AHRSFactor > shared_ptr
Definition: AHRSFactor.h:151
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::PreintegratedAhrsMeasurements::p
Params & p() const
Definition: AHRSFactor.h:80
std
Definition: BFloat16.h:88
gtsam::PreintegratedRotation
Definition: PreintegratedRotation.h:117
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition: AHRSFactor.h:114
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::AHRSFactor::~AHRSFactor
~AHRSFactor() override
Definition: AHRSFactor.h:167
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:05