PreintegratedRotation.cpp
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 #include "PreintegratedRotation.h"
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 void PreintegratedRotationParams::print(const string& s) const {
29  cout << (s.empty() ? s : s + "\n") << endl;
30  cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
31  if (omegaCoriolis)
32  cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
33  if (body_P_sensor) body_P_sensor->print("body_P_sensor");
34 }
35 
36 bool PreintegratedRotationParams::equals(
37  const PreintegratedRotationParams& other, double tol) const {
38  if (body_P_sensor) {
39  if (!other.body_P_sensor
40  || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
41  return false;
42  }
43  if (omegaCoriolis) {
44  if (!other.omegaCoriolis
45  || !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
46  return false;
47  }
48  return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
49 }
50 
51 void PreintegratedRotation::resetIntegration() {
52  deltaTij_ = 0.0;
53  deltaRij_ = Rot3();
54  delRdelBiasOmega_ = Z_3x3;
55 }
56 
57 void PreintegratedRotation::print(const string& s) const {
58  cout << s;
59  cout << " deltaTij [" << deltaTij_ << "]" << endl;
60  cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
61 }
62 
63 bool PreintegratedRotation::equals(const PreintegratedRotation& other,
64  double tol) const {
65  return this->matchesParamsWith(other)
66  && deltaRij_.equals(other.deltaRij_, tol)
67  && std::abs(deltaTij_ - other.deltaTij_) < tol
68  && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
69 }
70 
71 Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
72  const Vector3& biasHat, double deltaT,
73  OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
74 
75  // First we compensate the measurements for the bias
76  Vector3 correctedOmega = measuredOmega - biasHat;
77 
78  // Then compensate for sensor-body displacement: we express the quantities
79  // (originally in the IMU frame) into the body frame
80  if (p_->body_P_sensor) {
81  Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
82  // rotation rate vector in the body frame
83  correctedOmega = body_R_sensor * correctedOmega;
84  }
85 
86  // rotation vector describing rotation increment computed from the
87  // current rotation rate measurement
88  const Vector3 integratedOmega = correctedOmega * deltaT;
89  return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
90 }
91 
92 void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
93  const Vector3& biasHat, double deltaT,
94  OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
96  Matrix3 D_incrR_integratedOmega;
97  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
98  D_incrR_integratedOmega);
99 
100  // If asked, pass first derivative as well
101  if (optional_D_incrR_integratedOmega) {
102  *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
103  }
104 
105  // Update deltaTij and rotation
106  deltaTij_ += deltaT;
107  deltaRij_ = deltaRij_.compose(incrR, F);
108 
109  // Update Jacobian
110  const Matrix3 incrRt = incrR.transpose();
111  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
112  - D_incrR_integratedOmega * deltaT;
113 }
114 
115 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
116  OptionalJacobian<3, 3> H) const {
117  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
118  const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,{}, H);
119  if (H)
120  (*H) *= delRdelBiasOmega_;
121  return deltaRij_biascorrected;
122 }
123 
124 Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const {
125  if (!p_->omegaCoriolis)
126  return Vector3::Zero();
127  return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
128 }
129 
130 } // namespace gtsam
Key F(std::uint64_t j)
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
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
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
static const double deltaT
RealScalar s
std::optional< Vector3 > omegaCoriolis
Coriolis constant.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
static const Vector3 measuredOmega(w, 0, 0)
Matrix3 matrix() const
Definition: Rot3M.cpp:218
std::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
Rot3 deltaRij_
Preintegrated relative orientation (in frame i)
const G double tol
Definition: Group.h:86
#define abs(x)
Definition: datatypes.h:17
Matrix3 transpose() const
Definition: Rot3M.cpp:143
double deltaTij_
Time interval from i to j.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15