PreintegratedRotation.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 
22 #pragma once
23 
24 #include <gtsam/base/Matrix.h>
26 #include <gtsam/geometry/Pose3.h>
27 #include "gtsam/dllexport.h"
28 
29 namespace gtsam {
30 
31 namespace internal {
38 struct GTSAM_EXPORT IncrementalRotation {
40  const double deltaT;
41  const std::optional<Pose3>& body_P_sensor;
42 
49  Rot3 operator()(const Vector3& bias,
50  OptionalJacobian<3, 3> H_bias = {}) const;
51 };
52 
53 } // namespace internal
54 
57 struct GTSAM_EXPORT PreintegratedRotationParams {
61  std::optional<Vector3> omegaCoriolis;
62  std::optional<Pose3> body_P_sensor;
63 
64  PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
65 
66  PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
67  std::optional<Vector3> omega_coriolis)
68  : gyroscopeCovariance(gyroscope_covariance) {
69  if (omega_coriolis) {
70  omegaCoriolis = *omega_coriolis;
71  }
72  }
73 
75 
76  virtual void print(const std::string& s) const;
77  virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
78 
79  void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
80  void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis = omega; }
82 
83  const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
84  std::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
85  std::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
86 
87  private:
88 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
89 
90  friend class boost::serialization::access;
91  template<class ARCHIVE>
92  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
93  ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
94  ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
95 
96  // Provide support for Eigen::Matrix in std::optional
97  bool omegaCoriolisFlag = omegaCoriolis.has_value();
98  ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
99  if (omegaCoriolisFlag) {
100  ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
101  }
102  }
103 #endif
104 
105 #ifdef GTSAM_USE_QUATERNIONS
106  // Align if we are using Quaternions
107 public:
109 #endif
110 };
111 
117 class GTSAM_EXPORT PreintegratedRotation {
118  public:
120 
121  protected:
123  std::shared_ptr<Params> p_;
124 
125  double deltaTij_;
128 
131 
132  public:
135 
137  explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
138  resetIntegration();
139  }
140 
142  PreintegratedRotation(const std::shared_ptr<Params>& p,
143  double deltaTij, const Rot3& deltaRij,
144  const Matrix3& delRdelBiasOmega)
145  : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
146 
148 
151 
153  void resetIntegration();
154 
157  return p_ == other.p_;
158  }
160 
163  const std::shared_ptr<Params>& params() const { return p_; }
164  const double& deltaTij() const { return deltaTij_; }
165  const Rot3& deltaRij() const { return deltaRij_; }
166  const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
168 
171  void print(const std::string& s) const;
172  bool equals(const PreintegratedRotation& other, double tol) const;
174 
177 
186  void integrateGyroMeasurement(const Vector3& measuredOmega,
187  const Vector3& biasHat, double deltaT,
189 
196  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
197  OptionalJacobian<3, 3> H = {}) const;
198 
200  Vector3 integrateCoriolis(const Rot3& rot_i) const;
201 
203 
206 
207 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
208  inline Rot3 GTSAM_DEPRECATED incrementalRotation(
210  const Vector3& measuredOmega, const Vector3& bias, double deltaT,
211  OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
212  internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
213  Rot3 incrR = f(bias, D_incrR_integratedOmega);
214  // Backwards compatible "weird" Jacobian, no longer used.
215  if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
216  return incrR;
217  }
218 
221  void GTSAM_DEPRECATED integrateMeasurement(
222  const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
223  OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
224 
225 #endif
226 
228 
229  private:
230 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
231 
232  friend class boost::serialization::access;
233  template <class ARCHIVE>
234  void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
235  ar& BOOST_SERIALIZATION_NVP(p_);
236  ar& BOOST_SERIALIZATION_NVP(deltaTij_);
237  ar& BOOST_SERIALIZATION_NVP(deltaRij_);
238  ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
239  }
240 #endif
241 
242 #ifdef GTSAM_USE_QUATERNIONS
243  // Align if we are using Quaternions
244  public:
246 #endif
247 };
248 
249 template <>
250 struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
251 
252 }
gtsam::internal::IncrementalRotation::measuredOmega
const Vector3 & measuredOmega
Definition: PreintegratedRotation.h:39
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::internal::IncrementalRotation::deltaT
const double deltaT
Definition: PreintegratedRotation.h:40
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::PreintegratedRotationParams
Definition: PreintegratedRotation.h:57
gtsam::PreintegratedRotation::delRdelBiasOmega
const Matrix3 & delRdelBiasOmega() const
Definition: PreintegratedRotation.h:166
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::PreintegratedRotation::deltaRij
const Rot3 & deltaRij() const
Definition: PreintegratedRotation.h:165
gtsam::PreintegratedRotation::matchesParamsWith
bool matchesParamsWith(const PreintegratedRotation &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition: PreintegratedRotation.h:156
gtsam::PreintegratedRotationParams::PreintegratedRotationParams
PreintegratedRotationParams(const Matrix3 &gyroscope_covariance, std::optional< Vector3 > omega_coriolis)
Definition: PreintegratedRotation.h:66
gtsam::PreintegratedRotationParams::getBodyPSensor
std::optional< Pose3 > getBodyPSensor() const
Definition: PreintegratedRotation.h:85
gtsam::PreintegratedRotation::deltaTij_
double deltaTij_
Time interval from i to j.
Definition: PreintegratedRotation.h:125
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::PreintegratedRotation::Params
PreintegratedRotationParams Params
Definition: PreintegratedRotation.h:119
gtsam::internal::IncrementalRotation::body_P_sensor
const std::optional< Pose3 > & body_P_sensor
Definition: PreintegratedRotation.h:41
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
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::PreintegratedRotation::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition: PreintegratedRotation.h:127
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation()
Default constructor for serialization.
Definition: PreintegratedRotation.h:130
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const std::shared_ptr< Params > &p)
Default constructor, resets integration to zero.
Definition: PreintegratedRotation.h:137
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
gtsam::PreintegratedRotationParams::PreintegratedRotationParams
PreintegratedRotationParams()
Definition: PreintegratedRotation.h:64
common::deltaT
static const double deltaT
Definition: testImuFactor.cpp:183
gtsam::PreintegratedRotationParams::getOmegaCoriolis
std::optional< Vector3 > getOmegaCoriolis() const
Definition: PreintegratedRotation.h:84
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::PreintegratedRotationParams::getGyroscopeCovariance
const Matrix3 & getGyroscopeCovariance() const
Definition: PreintegratedRotation.h:83
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::PreintegratedRotationParams::gyroscopeCovariance
Matrix3 gyroscopeCovariance
Definition: PreintegratedRotation.h:60
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
operator()
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
Definition: IndexedViewMethods.h:73
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PreintegratedRotationParams::setGyroscopeCovariance
void setGyroscopeCovariance(const Matrix3 &cov)
Definition: PreintegratedRotation.h:79
gtsam::PreintegratedRotationParams::body_P_sensor
std::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Definition: PreintegratedRotation.h:62
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::PreintegratedRotationParams::setOmegaCoriolis
void setOmegaCoriolis(const Vector3 &omega)
Definition: PreintegratedRotation.h:80
gtsam::PreintegratedRotationParams::~PreintegratedRotationParams
virtual ~PreintegratedRotationParams()
Definition: PreintegratedRotation.h:74
gtsam::equals
Definition: Testable.h:112
std_optional_serialization.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::PreintegratedRotation::deltaTij
const double & deltaTij() const
Definition: PreintegratedRotation.h:164
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const std::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
Explicit initialization of all class members.
Definition: PreintegratedRotation.h:142
GTSAM_DEPRECATED
#define GTSAM_DEPRECATED
Definition: types.h:39
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::PreintegratedRotation
Definition: PreintegratedRotation.h:117
gtsam::PreintegratedRotation::params
const std::shared_ptr< Params > & params() const
Definition: PreintegratedRotation.h:163
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::PreintegratedRotationParams::omegaCoriolis
std::optional< Vector3 > omegaCoriolis
Coriolis constant.
Definition: PreintegratedRotation.h:61
internal
Definition: BandTriangularSolver.h:13
gtsam::PreintegratedRotationParams::setBodyPSensor
void setBodyPSensor(const Pose3 &pose)
Definition: PreintegratedRotation.h:81
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
gtsam::internal::IncrementalRotation
Function object for incremental rotation.
Definition: PreintegratedRotation.h:38
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
Pose3.h
3D Pose
gtsam::PreintegratedRotation::deltaRij_
Rot3 deltaRij_
Preintegrated relative orientation (in frame i)
Definition: PreintegratedRotation.h:126
gtsam::PreintegratedRotation::p_
std::shared_ptr< Params > p_
Parameters.
Definition: PreintegratedRotation.h:123


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:25