26 TangentPreintegration::TangentPreintegration(
const std::shared_ptr<Params>&
p,
27 const Bias& biasHat) :
58 const auto theta = preintegrated.segment<3>(0);
59 const auto position = preintegrated.segment<3>(3);
60 const auto velocity = preintegrated.segment<3>(6);
67 Matrix3 w_tangent_H_theta, invH;
69 local.
applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
72 const double dt22 = 0.5 * dt *
dt;
74 Vector9 preintegratedPlus;
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;
188 const Rot3 R02 = R01.
compose(R12, 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();
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
bool equals(const ConstantBias &expected, double tol=1e-5) const
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
bool equals(const TangentPreintegration &other, double tol) const
Rot2 R(Rot2::fromAngle(0.1))
Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
Some functions to compute numerical derivatives.
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
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Params & p() const
const reference to params
const Vector3 & accelerometer() const
const Matrix3 & dexp() const
const std::shared_ptr< Params > & params() const
shared pointer to params
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
const imuBias::ConstantBias & biasHat() const
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
static const Vector3 measuredAcc
double deltaTij_
Time interval from i to j.
Velocity3_ velocity(const NavState_ &X)
Class compose(const Class &g) const
void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Matrix< Scalar, Dynamic, Dynamic > C
void mergeWith(const TangentPreintegration &pim, Matrix9 *H1, Matrix9 *H2)
Matrix3 skewSymmetric(double wx, double wy, double wz)
static const Vector3 measuredOmega(w, 0, 0)
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Multiplies with dexp().inverse(), with optional derivatives.
Functor that implements Exponential map and its derivatives.
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={})
Point3_ position(const NavState_ &X)
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
const Vector3 & gyroscope() const
void resetIntegration() override
static Vector9 Compose(const Vector9 &zeta01, const Vector9 &zeta12, double deltaT12, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={})
std::shared_ptr< Params > p_
SO3 expmap() const
Rodrigues formula.
Bias biasHat_
Acceleration and gyro bias used for preintegration.
const Vector9 & preintegrated() const