TangentPreintegration.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 
18 #include "TangentPreintegration.h"
20 
21 using namespace std;
22 
23 namespace gtsam {
24 
25 //------------------------------------------------------------------------------
26 TangentPreintegration::TangentPreintegration(const std::shared_ptr<Params>& p,
27  const Bias& biasHat) :
28  PreintegrationBase(p, biasHat) {
30 }
31 
32 //------------------------------------------------------------------------------
34  deltaTij_ = 0.0;
35  preintegrated_.setZero();
38 }
39 
40 //------------------------------------------------------------------------------
42  double tol) const {
43  return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol
44  && biasHat_.equals(other.biasHat_, tol)
45  && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
47  other.preintegrated_H_biasAcc_, tol)
49  other.preintegrated_H_biasOmega_, tol);
50 }
51 
52 //------------------------------------------------------------------------------
53 // See extensive discussion in ImuFactor.lyx
55  const Vector3& w_body, double dt, const Vector9& preintegrated,
58  const auto theta = preintegrated.segment<3>(0);
59  const auto position = preintegrated.segment<3>(3);
60  const auto velocity = preintegrated.segment<3>(6);
61 
62  // This functor allows for saving computation when exponential map and its
63  // derivatives are needed at the same location in so<3>
64  so3::DexpFunctor local(theta);
65 
66  // Calculate exact mean propagation
67  Matrix3 w_tangent_H_theta, invH;
68  const Vector3 w_tangent = // angular velocity mapped back to tangent space
69  local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
70  const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
71  const Vector3 a_nav = R * a_body;
72  const double dt22 = 0.5 * dt * dt;
73 
74  Vector9 preintegratedPlus;
75  preintegratedPlus << // new preintegrated vector:
76  theta + w_tangent * dt, // theta
77  position + velocity * dt + a_nav * dt22, // position
78  velocity + a_nav * dt; // velocity
79 
80  if (A) {
81  // Exact derivative of R*a with respect to theta:
82  const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
83 
84  A->setIdentity();
85  A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
86  A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
87  A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
88  A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
89  }
90  if (B) {
91  B->block<3, 3>(0, 0) = Z_3x3;
92  B->block<3, 3>(3, 0) = R.matrix() * dt22;
93  B->block<3, 3>(6, 0) = R.matrix() * dt;
94  }
95  if (C) {
96  C->block<3, 3>(0, 0) = invH * dt;
97  C->block<3, 3>(3, 0) = Z_3x3;
98  C->block<3, 3>(6, 0) = Z_3x3;
99  }
100 
101  return preintegratedPlus;
102 }
103 
104 //------------------------------------------------------------------------------
106  const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
107  Matrix93* C) {
108  // Correct for bias in the sensor frame
111 
112  // Possibly correct for sensor pose by converting to body frame
113  Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
114  if (p().body_P_sensor) {
115  std::tie(acc, omega) = correctMeasurementsBySensorPose(
116  acc, omega, D_correctedAcc_acc, D_correctedAcc_omega,
117  D_correctedOmega_omega);
118  }
119 
120  // Do update
121  deltaTij_ += dt;
123 
124  if (p().body_P_sensor) {
125  // More complicated derivatives in case of non-trivial sensor pose
126  *C *= D_correctedOmega_omega;
127  if (!p().body_P_sensor->translation().isZero())
128  *C += *B * D_correctedAcc_omega;
129  *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
130  }
131 
132  // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
133  // where acc_H_biasAcc = -I_3x3, hence
134  // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
136 
137  // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
138  // where omega_H_biasOmega = -I_3x3, hence
139  // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
141 }
142 
143 //------------------------------------------------------------------------------
145  const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
146  // We correct for a change between bias_i and the biasHat_ used to integrate
147  // This is a simple linear correction with obvious derivatives
148  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
149  const Vector9 biasCorrected = preintegrated()
151  + preintegrated_H_biasOmega_ * biasIncr.gyroscope();
152 
153  if (H) {
155  }
156  return biasCorrected;
157 }
158 
159 //------------------------------------------------------------------------------
160 // sugar for derivative blocks
161 #define D_R_R(H) (H)->block<3,3>(0,0)
162 #define D_R_t(H) (H)->block<3,3>(0,3)
163 #define D_R_v(H) (H)->block<3,3>(0,6)
164 #define D_t_R(H) (H)->block<3,3>(3,0)
165 #define D_t_t(H) (H)->block<3,3>(3,3)
166 #define D_t_v(H) (H)->block<3,3>(3,6)
167 #define D_v_R(H) (H)->block<3,3>(6,0)
168 #define D_v_t(H) (H)->block<3,3>(6,3)
169 #define D_v_v(H) (H)->block<3,3>(6,6)
170 
171 //------------------------------------------------------------------------------
172 Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
173  const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
175  const auto t01 = zeta01.segment<3>(0);
176  const auto p01 = zeta01.segment<3>(3);
177  const auto v01 = zeta01.segment<3>(6);
178 
179  const auto t12 = zeta12.segment<3>(0);
180  const auto p12 = zeta12.segment<3>(3);
181  const auto v12 = zeta12.segment<3>(6);
182 
183  Matrix3 R01_H_t01, R12_H_t12;
184  const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
185  const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
186 
187  Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
188  const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
189 
190  Matrix3 t02_H_R02;
191  Vector9 zeta02;
192  const Matrix3 R = R01.matrix();
193  zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
194  p01 + v01 * deltaT12 + R * p12, // position
195  v01 + R * v12; // velocity
196 
197  if (H1) {
198  H1->setIdentity();
199  D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
200  D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
201  D_t_v(H1) = I_3x3 * deltaT12;
202  D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
203  }
204 
205  if (H2) {
206  H2->setZero();
207  D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
208  D_t_t(H2) = R;
209  D_v_v(H2) = R;
210  }
211 
212  return zeta02;
213 }
214 
215 //------------------------------------------------------------------------------
217  Matrix9* H1, Matrix9* H2) {
218  if (!matchesParamsWith(pim12)) {
219  throw std::domain_error(
220  "Cannot merge pre-integrated measurements with different params");
221  }
222 
223  if (params()->body_P_sensor) {
224  throw std::domain_error(
225  "Cannot merge pre-integrated measurements with sensor pose yet");
226  }
227 
228  const double t01 = deltaTij();
229  const double t12 = pim12.deltaTij();
230  deltaTij_ = t01 + t12;
231 
232  const Vector9 zeta01 = preintegrated();
233  Vector9 zeta12 = pim12.preintegrated(); // will be modified.
234 
235  const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
236  zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
237  + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
238 
239  preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
240 
242  + (*H2) * pim12.preintegrated_H_biasAcc_;
243 
245  + (*H2) * pim12.preintegrated_H_biasOmega_;
246 }
247 
248 //------------------------------------------------------------------------------
249 
250 }// 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::TangentPreintegration::mergeWith
void mergeWith(const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
Definition: TangentPreintegration.cpp:216
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
Definition: ImuBias.h:71
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::TangentPreintegration::preintegrated_H_biasOmega_
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition: TangentPreintegration.h:37
D_v_v
#define D_v_v(H)
Definition: TangentPreintegration.cpp:169
gtsam::position
Point3_ position(const NavState_ &X)
Definition: navigation/expressions.h:37
gtsam::TangentPreintegration::biasCorrectedDelta
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
Definition: TangentPreintegration.cpp:144
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
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::TangentPreintegration::theta
Vector3 theta() const
Definition: TangentPreintegration.h:77
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
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::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
D_t_t
#define D_t_t(H)
Definition: TangentPreintegration.cpp:165
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
gtsam::TangentPreintegration::preintegrated
const Vector9 & preintegrated() const
Definition: TangentPreintegration.h:76
gtsam::PreintegrationBase::matchesParamsWith
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition: PreintegrationBase.h:86
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::TangentPreintegration::update
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
Definition: TangentPreintegration.cpp:105
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
TangentPreintegration.h
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::TangentPreintegration::Compose
static Vector9 Compose(const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={})
Definition: TangentPreintegration.cpp:172
gtsam::TangentPreintegration::preintegrated_H_biasAcc_
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition: TangentPreintegration.h:36
D_v_R
#define D_v_R(H)
Definition: TangentPreintegration.cpp:167
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition: PreintegrationBase.h:96
D_t_R
#define D_t_R(H)
Definition: TangentPreintegration.cpp:164
gtsam::TangentPreintegration::preintegrated_
Vector9 preintegrated_
Definition: TangentPreintegration.h:35
gtsam::TangentPreintegration::resetIntegration
void resetIntegration() override
Definition: TangentPreintegration.cpp:33
gtsam::TangentPreintegration::equals
bool equals(const TangentPreintegration &other, double tol) const
Definition: TangentPreintegration.cpp:41
gtsam::PreintegrationBase::p_
std::shared_ptr< Params > p_
Definition: PreintegrationBase.h:47
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::TangentPreintegration
Definition: TangentPreintegration.h:28
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
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::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::PreintegrationBase::biasHat
const imuBias::ConstantBias & biasHat() const
Definition: PreintegrationBase.h:104
gtsam::so3::DexpFunctor
Functor that implements Exponential map and its derivatives.
Definition: SO3.h:157
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
D_R_R
#define D_R_R(H)
Definition: TangentPreintegration.cpp:161
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:85
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:562
D_t_v
#define D_t_v(H)
Definition: TangentPreintegration.cpp:166
gtsam::TangentPreintegration::UpdatePreintegrated
static Vector9 UpdatePreintegrated(const Vector3 &a_body, const Vector3 &w_body, const double dt, const Vector9 &preintegrated, OptionalJacobian< 9, 9 > A={}, OptionalJacobian< 9, 3 > B={}, OptionalJacobian< 9, 3 > C={})
Definition: TangentPreintegration.cpp:54
gtsam::PreintegrationBase::deltaTij
double deltaTij() const
Definition: PreintegrationBase.h:105
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::velocity
Velocity3_ velocity(const NavState_ &X)
Definition: navigation/expressions.h:40
gtsam::PreintegrationBase::params
const std::shared_ptr< Params > & params() const
shared pointer to params
Definition: PreintegrationBase.h:91


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:55