Add ifdefs that restore the old IMU factor functionality
dellaert
parent
aea1f1e572
commit
fa15264e83
|
|
@ -70,7 +70,13 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// Update preintegrated measurements.
|
||||
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix93 B, C;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||
#else
|
||||
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
|
||||
PreintegrationBase::update(measuredAcc, measuredOmega, dt,
|
||||
&D_incrR_integratedOmega, &A, &B, &C);
|
||||
#endif
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first
|
||||
// order propagation that can be seen as a prediction phase in an EKF
|
||||
|
|
@ -79,7 +85,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
// and preintegrated measurements
|
||||
|
||||
// Single Jacobians to propagate covariance
|
||||
// TODO(frank): should we not also accout for bias on position?
|
||||
// TODO(frank): should we not also account for bias on position?
|
||||
Matrix3 theta_H_biasOmega = - C.topRows<3>();
|
||||
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
|||
// Update preintegrated measurements (also get Jacobian)
|
||||
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix93 B, C;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||
#else
|
||||
Matrix3 D_incrR_integratedOmega;
|
||||
PreintegrationBase::update(measuredAcc, measuredOmega, dt,
|
||||
&D_incrR_integratedOmega, &A, &B, &C);
|
||||
#endif
|
||||
|
||||
// first order covariance propagation:
|
||||
// as in [2] we consider a first order propagation that can be seen as a
|
||||
|
|
@ -91,13 +97,14 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
|
||||
Matrix9* H1, Matrix9* H2) {
|
||||
PreintegrationBase::mergeWith(pim12, H1, H2);
|
||||
preintMeasCov_ =
|
||||
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
|
||||
}
|
||||
|
||||
#endif
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
||||
|
|
@ -174,6 +181,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
|||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
PreintegratedImuMeasurements ImuFactor::Merge(
|
||||
const PreintegratedImuMeasurements& pim01,
|
||||
const PreintegratedImuMeasurements& pim12) {
|
||||
|
|
@ -216,6 +224,7 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
|
|||
f01->key5(), // B
|
||||
pim02);
|
||||
}
|
||||
#endif
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
|
|
|
|||
|
|
@ -124,8 +124,10 @@ public:
|
|||
/// Return pre-integrated measurement covariance
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
/// Merge in a different set of measurements and update bias derivatives accordingly
|
||||
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
|
||||
#endif
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// @deprecated constructor
|
||||
|
|
@ -230,6 +232,7 @@ public:
|
|||
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
|
||||
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
/// Merge two pre-integrated measurement classes
|
||||
static PreintegratedImuMeasurements Merge(
|
||||
const PreintegratedImuMeasurements& pim01,
|
||||
|
|
@ -237,6 +240,7 @@ public:
|
|||
|
||||
/// Merge two factors
|
||||
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
|
||||
#endif
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// @deprecated typename
|
||||
|
|
|
|||
|
|
@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
|
|||
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
|
||||
NavState 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 {
|
||||
|
|
|
|||
|
|
@ -203,7 +203,7 @@ public:
|
|||
/// @name Dynamics
|
||||
/// @{
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
|
||||
/// Integrate forward in time given angular velocity and acceleration in body frame
|
||||
/// Uses second order integration for position, returns derivatives except dt.
|
||||
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
|
||||
|
|
|
|||
|
|
@ -37,15 +37,28 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
|
|||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::resetIntegration() {
|
||||
deltaTij_ = 0.0;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
preintegrated_.setZero();
|
||||
preintegrated_H_biasAcc_.setZero();
|
||||
preintegrated_H_biasOmega_.setZero();
|
||||
#else
|
||||
deltaXij_ = NavState();
|
||||
delRdelBiasOmega_.setZero();
|
||||
delPdelBiasAcc_.setZero();
|
||||
delPdelBiasOmega_.setZero();
|
||||
delVdelBiasAcc_.setZero();
|
||||
delVdelBiasOmega_.setZero();
|
||||
#endif
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
|
||||
os << " deltaTij " << pim.deltaTij_ << endl;
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
os << " deltaRij " << Point3(pim.theta()) << endl;
|
||||
#else
|
||||
os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
|
||||
#endif
|
||||
os << " deltaPij " << Point3(pim.deltaPij()) << endl;
|
||||
os << " deltaVij " << Point3(pim.deltaVij()) << endl;
|
||||
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
|
||||
|
|
@ -64,9 +77,18 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
|
|||
return p_->equals(*other.p_, tol)
|
||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||
&& biasHat_.equals(other.biasHat_, tol)
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
|
||||
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
|
||||
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
|
||||
#else
|
||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
|
||||
#endif
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -105,7 +127,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
|
|||
// Update derivative: centrifugal causes the correlation between acc and omega!!!
|
||||
if (correctedAcc_H_unbiasedOmega) {
|
||||
double wdp = correctedOmega.dot(b_arm);
|
||||
*correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
|
||||
const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
|
||||
*correctedAcc_H_unbiasedOmega = -( diag_wdp
|
||||
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
|
||||
+ 2 * b_arm * unbiasedOmega.transpose();
|
||||
}
|
||||
|
|
@ -114,6 +137,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
|
|||
return make_pair(correctedAcc, correctedOmega);
|
||||
}
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
//------------------------------------------------------------------------------
|
||||
// See extensive discussion in ImuFactor.lyx
|
||||
Vector9 PreintegrationBase::UpdatePreintegrated(
|
||||
|
|
@ -224,29 +248,150 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
|
|||
}
|
||||
return biasCorrected;
|
||||
}
|
||||
#else
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
|
||||
|
||||
// Correct for bias in the sensor frame
|
||||
Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
return correctMeasurementsBySensorPose(unbiasedAcc, unbiasedOmega,
|
||||
D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega,
|
||||
D_correctedOmega_measuredOmega);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, const double dt,
|
||||
OptionalJacobian<9, 9> D_updated_current,
|
||||
OptionalJacobian<9, 3> D_updated_measuredAcc,
|
||||
OptionalJacobian<9, 3> D_updated_measuredOmega) const {
|
||||
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
Matrix3 D_correctedAcc_measuredAcc, //
|
||||
D_correctedAcc_measuredOmega, //
|
||||
D_correctedOmega_measuredOmega;
|
||||
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
|
||||
boost::tie(correctedAcc, correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
|
||||
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
|
||||
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
|
||||
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
|
||||
// Do update in one fell swoop
|
||||
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
|
||||
NavState updated = deltaXij_.update(correctedAcc, correctedOmega, dt, D_updated_current,
|
||||
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
|
||||
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
|
||||
if (needDerivs) {
|
||||
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
|
||||
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
|
||||
if (!p().body_P_sensor->translation().vector().isZero()) {
|
||||
*D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
|
||||
}
|
||||
}
|
||||
return updated;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::update(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, const double dt,
|
||||
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
|
||||
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
|
||||
|
||||
// Save current rotation for updating Jacobians
|
||||
const Rot3 oldRij = deltaXij_.attitude();
|
||||
|
||||
// Do update
|
||||
deltaTij_ += dt;
|
||||
deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt,
|
||||
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
|
||||
|
||||
// Update Jacobians
|
||||
// TODO(frank): we are repeating some computation here: accessible in F ?
|
||||
Vector3 correctedAcc, correctedOmega;
|
||||
boost::tie(correctedAcc, correctedOmega) =
|
||||
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
|
||||
|
||||
Matrix3 D_acc_R;
|
||||
oldRij.rotate(correctedAcc, D_acc_R);
|
||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
||||
|
||||
const Vector3 integratedOmega = correctedOmega * dt;
|
||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
||||
const Matrix3 incrRt = incrR.transpose();
|
||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||
- *D_incrR_integratedOmega * dt;
|
||||
|
||||
double dt22 = 0.5 * dt * dt;
|
||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
||||
delVdelBiasAcc_ += -dRij * dt;
|
||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::biasCorrectedDelta(
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||
Matrix3 D_correctedRij_bias;
|
||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
||||
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
|
||||
H ? &D_correctedRij_bias : 0);
|
||||
if (H)
|
||||
D_correctedRij_bias *= delRdelBiasOmega_;
|
||||
|
||||
Vector9 xi;
|
||||
Matrix3 D_dR_correctedRij;
|
||||
// TODO(frank): could line below be simplified? It is equivalent to
|
||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
||||
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
|
||||
+ delVdelBiasOmega_ * biasIncr.gyroscope();
|
||||
|
||||
if (H) {
|
||||
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
|
||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
||||
}
|
||||
return xi;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NavState PreintegrationBase::predict(const NavState& state_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 6> H2) const {
|
||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
|
||||
OptionalJacobian<9, 6> H2) const {
|
||||
// TODO(frank): make sure this stuff is still correct
|
||||
Matrix96 D_biasCorrected_bias;
|
||||
Vector9 biasCorrected =
|
||||
biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0);
|
||||
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
|
||||
H2 ? &D_biasCorrected_bias : 0);
|
||||
|
||||
// Correct for initial velocity and gravity
|
||||
Matrix9 D_delta_state, D_delta_biasCorrected;
|
||||
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
|
||||
p().omegaCoriolis, p().use2ndOrderCoriolis,
|
||||
H1 ? &D_delta_state : 0,
|
||||
H2 ? &D_delta_biasCorrected : 0);
|
||||
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
|
||||
H2 ? &D_delta_biasCorrected : 0);
|
||||
|
||||
// Use retract to get back to NavState manifold
|
||||
Matrix9 D_predict_state, D_predict_delta;
|
||||
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
|
||||
if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state;
|
||||
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
|
||||
if (H1)
|
||||
*H1 = D_predict_state + D_predict_delta * D_delta_state;
|
||||
if (H2)
|
||||
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
|
||||
return state_j;
|
||||
}
|
||||
|
||||
|
|
@ -306,6 +451,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
|||
return error;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
//------------------------------------------------------------------------------
|
||||
// sugar for derivative blocks
|
||||
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||
|
|
@ -393,6 +539,7 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
|
|||
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
|
||||
(*H2) * pim12.preintegrated_H_biasOmega_;
|
||||
}
|
||||
#endif
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
|
|
@ -408,6 +555,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
|||
p_ = q;
|
||||
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
|
||||
}
|
||||
|
||||
#endif
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
|
|
|
|||
|
|
@ -81,10 +81,18 @@ class PreintegrationBase {
|
|||
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
|
||||
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
|
||||
*/
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
Vector9 preintegrated_;
|
||||
|
||||
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
|
||||
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
|
||||
#else
|
||||
NavState deltaXij_;
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
#endif
|
||||
|
||||
/// Default constructor for serialization
|
||||
PreintegrationBase() {
|
||||
|
|
@ -140,17 +148,22 @@ public:
|
|||
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
|
||||
double deltaTij() const { return deltaTij_; }
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
const Vector9& preintegrated() const { return preintegrated_; }
|
||||
|
||||
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
|
||||
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
|
||||
|
||||
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
|
||||
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
|
||||
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
|
||||
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
|
||||
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
|
||||
|
||||
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
|
||||
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
|
||||
#else
|
||||
const NavState& deltaXij() const { return deltaXij_; }
|
||||
const Rot3& deltaRij() const { return deltaXij_.attitude(); }
|
||||
Vector3 deltaPij() const { return deltaXij_.position().vector(); }
|
||||
Vector3 deltaVij() const { return deltaXij_.velocity(); }
|
||||
#endif
|
||||
|
||||
// Exposed for MATLAB
|
||||
Vector6 biasHatVector() const { return biasHat_.vector(); }
|
||||
|
|
@ -175,6 +188,8 @@ public:
|
|||
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
|
||||
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
|
||||
// Update integrated vector on tangent manifold preintegrated with acceleration
|
||||
// Static, functional version.
|
||||
static Vector9 UpdatePreintegrated(const Vector3& a_body,
|
||||
|
|
@ -200,6 +215,38 @@ public:
|
|||
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const;
|
||||
|
||||
#else
|
||||
|
||||
/// Subtract estimate and correct for sensor pose
|
||||
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
|
||||
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
|
||||
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
|
||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
|
||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
|
||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
|
||||
|
||||
/// Calculate the updated preintegrated measurement, does not modify
|
||||
/// It takes measured quantities in the j frame
|
||||
NavState updatedDeltaXij(const Vector3& j_measuredAcc,
|
||||
const Vector3& j_measuredOmega, const double dt,
|
||||
OptionalJacobian<9, 9> D_updated_current = boost::none,
|
||||
OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
|
||||
OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
|
||||
|
||||
/// Update preintegrated measurements and get derivatives
|
||||
/// It takes measured quantities in the j frame
|
||||
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
||||
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
|
||||
Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
|
||||
|
||||
/// Given the estimate of the bias, return a NavState tangent vector
|
||||
/// summarizing the preintegrated IMU measurements so far
|
||||
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 6> H = boost::none) const;
|
||||
|
||||
#endif
|
||||
|
||||
/// Predict state at time j
|
||||
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
|
|
@ -219,6 +266,7 @@ public:
|
|||
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
||||
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
|
||||
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
|
||||
double deltaT12,
|
||||
|
|
@ -229,13 +277,13 @@ public:
|
|||
/// The derivatives apply to the preintegrated Vector9
|
||||
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
|
||||
/// @}
|
||||
#endif
|
||||
|
||||
/** Dummy clone for MATLAB */
|
||||
virtual boost::shared_ptr<PreintegrationBase> clone() const {
|
||||
return boost::shared_ptr<PreintegrationBase>();
|
||||
}
|
||||
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
|
|
@ -257,11 +305,22 @@ private:
|
|||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
namespace bs = ::boost::serialization;
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
|
||||
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
|
||||
#else
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
|
||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
|
||||
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
auto p = testing::Params();
|
||||
testing::SomeMeasurements measurements;
|
||||
|
|
@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
|
||||
pim.preintegrated_H_biasOmega(), 1e-3));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
|
||||
|
|
|
|||
|
|
@ -94,13 +94,13 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
|
||||
// Actual pre-integrated values
|
||||
PreintegratedImuMeasurements actual(testing::Params());
|
||||
EXPECT(assert_equal(kZero, actual.theta()));
|
||||
EXPECT(assert_equal(Rot3(), actual.deltaRij()));
|
||||
EXPECT(assert_equal(kZero, actual.deltaPij()));
|
||||
EXPECT(assert_equal(kZero, actual.deltaVij()));
|
||||
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
|
||||
|
||||
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(expectedDeltaR1, actual.theta()));
|
||||
EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
|
||||
EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
|
||||
EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
|
||||
DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
|
||||
|
|
@ -129,7 +129,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
|
|||
|
||||
// Actual pre-integrated values
|
||||
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
EXPECT(assert_equal(expectedDeltaR2, actual.theta()));
|
||||
EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
|
||||
EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
|
||||
EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
|
||||
DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
|
||||
|
|
@ -439,6 +439,7 @@ TEST(ImuFactor, fistOrderExponential) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||
testing::SomeMeasurements measurements;
|
||||
|
||||
|
|
@ -458,7 +459,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
|
||||
pim.preintegrated_H_biasOmega(), 1e-3));
|
||||
}
|
||||
|
||||
#endif
|
||||
/* ************************************************************************* */
|
||||
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega) {
|
||||
|
|
@ -789,6 +790,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
|
||||
|
||||
struct ImuFactorMergeTest {
|
||||
|
|
@ -883,6 +885,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
|
|||
mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
|
||||
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Same values as pre-integration test but now testing covariance
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
static const double kDt = 0.1;
|
||||
|
||||
#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
|
||||
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
|
||||
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
|
||||
}
|
||||
|
|
@ -140,6 +141,7 @@ TEST(PreintegrationBase, MergedBiasDerivatives) {
|
|||
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
|
||||
expected_pim02.preintegrated_H_biasOmega(), 1e-7));
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue