ManifoldPreintegration.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 "ManifoldPreintegration.h"
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 //------------------------------------------------------------------------------
29 ManifoldPreintegration::ManifoldPreintegration(
30  const std::shared_ptr<Params>& p, const Bias& biasHat) :
31  PreintegrationBase(p, biasHat) {
33 }
34 
35 //------------------------------------------------------------------------------
37  deltaTij_ = 0.0;
38  deltaXij_ = NavState();
39  delRdelBiasOmega_.setZero();
40  delPdelBiasAcc_.setZero();
41  delPdelBiasOmega_.setZero();
42  delVdelBiasAcc_.setZero();
43  delVdelBiasOmega_.setZero();
44 }
45 
46 //------------------------------------------------------------------------------
48  double tol) const {
49  return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol
50  && biasHat_.equals(other.biasHat_, tol)
51  && deltaXij_.equals(other.deltaXij_, tol)
52  && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
53  && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
54  && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
55  && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
56  && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
57 }
58 
59 //------------------------------------------------------------------------------
61  const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
62  Matrix93* C) {
63 
64  // Correct for bias in the sensor frame
67 
68  // Possibly correct for sensor pose
69  Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
70  if (p().body_P_sensor) {
71  std::tie(acc, omega) = correctMeasurementsBySensorPose(
72  acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
73  D_correctedOmega_omega);
74  }
75 
76  // Save current rotation for updating Jacobians
77  const Rot3 oldRij = deltaXij_.attitude();
78 
79  // Do update
80  deltaTij_ += dt;
81  deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
82 
83  if (p().body_P_sensor) {
84  // More complicated derivatives in case of non-trivial sensor pose
85  *C *= D_correctedOmega_omega;
86  if (!p().body_P_sensor->translation().isZero())
87  *C += *B * D_correctedAcc_omega;
88  *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
89  }
90 
91  // Update Jacobians
92  // TODO(frank): Try same simplification as in new approach
93  Matrix3 D_acc_R;
94  oldRij.rotate(acc, D_acc_R);
95  const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
96 
97  const Vector3 integratedOmega = omega * dt;
98  Matrix3 D_incrR_integratedOmega;
99  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
100  const Matrix3 incrRt = incrR.transpose();
101  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
102 
103  double dt22 = 0.5 * dt * dt;
104  const Matrix3 dRij = oldRij.matrix(); // expensive
105  delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
106  delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
107  delVdelBiasAcc_ += -dRij * dt;
108  delVdelBiasOmega_ += D_acc_biasOmega * dt;
109 }
110 
111 //------------------------------------------------------------------------------
113  const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
114  // Correct deltaRij, derivative is delRdelBiasOmega_
115  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
116  Matrix3 D_correctedRij_bias;
117  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
118  const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {},
119  H ? &D_correctedRij_bias : 0);
120  if (H)
121  D_correctedRij_bias *= delRdelBiasOmega_;
122 
123  Vector9 xi;
124  Matrix3 D_dR_correctedRij;
125  // TODO(frank): could line below be simplified? It is equivalent to
126  // LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
127  NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
129  + delPdelBiasOmega_ * biasIncr.gyroscope();
131  + delVdelBiasOmega_ * biasIncr.gyroscope();
132 
133  if (H) {
134  Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
135  D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
136  D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
137  D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
138  (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
139  }
140  return xi;
141 }
142 
143 //------------------------------------------------------------------------------
144 
145 }// namespace gtsam
gtsam::ManifoldPreintegration::update
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
Definition: ManifoldPreintegration.cpp:60
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::NavState::dR
static Eigen::Block< Vector9, 3, 1 > dR(Vector9 &v)
Definition: NavState.h:138
gtsam::ManifoldPreintegration::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:42
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
Definition: ImuBias.h:71
gtsam::NavState::update
NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
Definition: NavState.cpp:170
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
ManifoldPreintegration.h
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::NavState::dP
static Eigen::Block< Vector9, 3, 1 > dP(Vector9 &v)
Definition: NavState.h:141
gtsam::ManifoldPreintegration::deltaRij
Rot3 deltaRij() const override
Definition: ManifoldPreintegration.h:77
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::PreintegrationBase
Definition: PreintegrationBase.h:41
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
Definition: ImuBias.h:104
gtsam::PreintegrationBase::correctMeasurementsBySensorPose
std::pair< Vector3, Vector3 > correctMeasurementsBySensorPose(const Vector3 &unbiasedAcc, const Vector3 &unbiasedOmega, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedAcc={}, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega={}, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega={}) const
Definition: PreintegrationBase.cpp:59
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::NavState
Definition: NavState.h:34
gtsam::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:76
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::ManifoldPreintegration::delPdelBiasOmega_
Matrix3 delPdelBiasOmega_
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:44
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::ManifoldPreintegration::deltaXij_
NavState deltaXij_
Definition: ManifoldPreintegration.h:41
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition: PreintegrationBase.h:50
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::NavState::attitude
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Definition: NavState.cpp:48
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
Definition: ImuBias.h:66
gtsam::ManifoldPreintegration::deltaVij
Vector3 deltaVij() const override
Definition: ManifoldPreintegration.h:79
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition: PreintegrationBase.h:96
gtsam::PreintegrationBase::p_
std::shared_ptr< Params > p_
Definition: PreintegrationBase.h:47
gtsam::NavState::dV
static Eigen::Block< Vector9, 3, 1 > dV(Vector9 &v)
Definition: NavState.h:144
gtsam::ManifoldPreintegration
Definition: ManifoldPreintegration.h:33
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam::ManifoldPreintegration::resetIntegration
void resetIntegration() override
Definition: ManifoldPreintegration.cpp:36
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::NavState::equals
bool equals(const NavState &other, double tol=1e-8) const
equals
Definition: NavState.cpp:101
gtsam::ManifoldPreintegration::delPdelBiasAcc_
Matrix3 delPdelBiasAcc_
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition: ManifoldPreintegration.h:43
std
Definition: BFloat16.h:88
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition: PreintegrationBase.h:53
gtsam::ManifoldPreintegration::delVdelBiasOmega_
Matrix3 delVdelBiasOmega_
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition: ManifoldPreintegration.h:46
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Rot3::Logmap
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 9, 9 >
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::ManifoldPreintegration::deltaPij
Vector3 deltaPij() const override
Definition: ManifoldPreintegration.h:78
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:85
gtsam::ManifoldPreintegration::delVdelBiasAcc_
Matrix3 delVdelBiasAcc_
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition: ManifoldPreintegration.h:45
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
Definition: Lie.h:78
gtsam::ManifoldPreintegration::biasCorrectedDelta
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
Definition: ManifoldPreintegration.cpp:112
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::ManifoldPreintegration::equals
bool equals(const ManifoldPreintegration &other, double tol) const
Definition: ManifoldPreintegration.cpp:47


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:10