Go to the documentation of this file.
26 TangentPreintegration::TangentPreintegration(
const std::shared_ptr<Params>&
p,
27 const Bias& biasHat) :
49 other.preintegrated_H_biasOmega_,
tol);
55 const Vector3& w_body,
double dt,
const Vector9& preintegrated,
67 Matrix3 w_tangent_H_theta, invH;
69 local.applyInvDexp(w_body,
A ? &w_tangent_H_theta : 0,
C ? &invH : 0);
70 const Rot3 R(local.expmap());
72 const double dt22 = 0.5 *
dt *
dt;
74 Vector9 preintegratedPlus;
82 const Matrix3 a_nav_H_theta =
R.matrix() *
skewSymmetric(-a_body) * local.dexp();
85 A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta *
dt;
86 A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;
87 A->block<3, 3>(3, 6) = I_3x3 *
dt;
88 A->block<3, 3>(6, 0) = a_nav_H_theta *
dt;
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;
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;
101 return preintegratedPlus;
113 Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
116 acc,
omega, D_correctedAcc_acc, D_correctedAcc_omega,
117 D_correctedOmega_omega);
126 *
C *= D_correctedOmega_omega;
128 *
C += *
B * D_correctedAcc_omega;
129 *
B *= D_correctedAcc_acc;
156 return biasCorrected;
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)
175 const auto t01 = zeta01.segment<3>(0);
176 const auto p01 = zeta01.segment<3>(3);
177 const auto v01 = zeta01.segment<3>(6);
179 const auto t12 = zeta12.segment<3>(0);
180 const auto p12 = zeta12.segment<3>(3);
181 const auto v12 = zeta12.segment<3>(6);
183 Matrix3 R01_H_t01, R12_H_t12;
187 Matrix3 R02_H_R01, R02_H_R12;
192 const Matrix3
R = R01.
matrix();
194 p01 + v01 * deltaT12 +
R * p12,
199 D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
201 D_t_v(H1) = I_3x3 * deltaT12;
207 D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
219 throw std::domain_error(
220 "Cannot merge pre-integrated measurements with different params");
224 throw std::domain_error(
225 "Cannot merge pre-integrated measurements with sensor pose yet");
229 const double t12 = pim12.
deltaTij();
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
void mergeWith(const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
const Vector3 & gyroscope() const
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Point3_ position(const NavState_ &X)
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
Class compose(const Class &g) const
bool equals(const ConstantBias &expected, double tol=1e-5) const
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
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
Matrix3 skewSymmetric(double wx, double wy, double wz)
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
const Vector9 & preintegrated() const
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Some functions to compute numerical derivatives.
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
const Vector3 measuredOmega
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
const Vector3 & accelerometer() const
static Vector9 Compose(const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={})
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Params & p() const
const reference to params
void resetIntegration() override
bool equals(const TangentPreintegration &other, double tol) const
std::shared_ptr< Params > p_
Matrix< Scalar, Dynamic, Dynamic > C
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
double deltaTij_
Time interval from i to j.
const imuBias::ConstantBias & biasHat() const
Functor that implements Exponential map and its derivatives.
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
static const Vector3 measuredAcc
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
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={})
Rot2 R(Rot2::fromAngle(0.1))
Velocity3_ velocity(const NavState_ &X)
const std::shared_ptr< Params > & params() const
shared pointer to params
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:55