diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 814b020b1..85a95c35a 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -64,15 +64,17 @@ void PreintegratedAhrsMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; - Matrix3 D_Rincr_integratedOmega; - const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_incrR_integratedOmega); // expensive !! // Update Jacobian - update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; // Update rotation and deltaTij. Matrix3 Fr; // Jacobian of the update - updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); + deltaRij_ = deltaRij_.compose(incrR, Fr); + deltaTij_ += deltaT; // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index c6bc8282a..ea68f724e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -55,33 +55,22 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, boost::optional F_test, boost::optional G_test) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). + const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion - Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); - - const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + // Update preintegrated measurements. + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr + Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT, + &D_incrR_integratedOmega, &F_9x9); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion - // Update preintegrated measurements. TODO Frank moved from end of this function !!! - Matrix9 F_9x9; - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance Matrix3 H_vel_biasacc = -dRij * deltaT; - Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; + Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6a720972c..6745a86fc 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -63,24 +63,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { - Vector3 correctedAcc, correctedOmega; - boost::tie(correctedAcc, correctedOmega) = - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); - - // rotation increment computed from the current rotation rate measurement - const Vector3 integratedOmega = correctedOmega * deltaT; - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr - // rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); - - // Update Jacobians - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, - deltaT); + const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test // Update preintegrated measurements (also get Jacobian) - const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT, + &D_incrR_integratedOmega, &F); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF @@ -93,8 +82,8 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_ = F * preintMeasCov_ * F.transpose(); D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT; D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT; - D_R_R(&preintMeasCov_) += D_Rincr_integratedOmega * p().gyroscopeCovariance - * D_Rincr_integratedOmega.transpose() * deltaT; + D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance + * D_incrR_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; @@ -113,7 +102,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // This in only for testing & documentation, while the actual computation is done block-wise // omegaNoise intNoise accNoise (*G_test) << - D_Rincr_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle + D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos Z_3x3, Z_3x3, dRij * deltaT; // vel } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index df521341f..df0953945 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -108,21 +108,34 @@ class PreintegratedRotation { /// @} - /// Update preintegrated measurements - void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - OptionalJacobian<3, 3> H = boost::none) { - deltaRij_ = deltaRij_.compose(incrR, H, boost::none); - deltaTij_ += deltaT; - } + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, + Matrix3* Fr) { - /** - * Update Jacobians to be used during preintegration - * TODO: explain arguments - */ - void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, - const Rot3& incrR, double deltaT) { + // First we compensate the measurements for the bias + Vector3 correctedOmega = measuredOmega - biasHat; + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (p_->body_P_sensor) { + Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix(); + // rotation rate vector in the body frame + correctedOmega = body_R_sensor * correctedOmega; + } + + // rotation vector describing rotation increment computed from the + // current rotation rate measurement + const Vector3 theta_incr = correctedOmega * deltaT; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_incrR_integratedOmega); // expensive !! + + // Update deltaTij and rotation + deltaTij_ += deltaT; + deltaRij_ = deltaRij_.compose(incrR, Fr); + + // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation, with optional Jacobian diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f6664e5dd..13baa973d 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -60,40 +60,46 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, /// Update preintegrated measurements void PreintegrationBase::updatePreintegratedMeasurements( - const Vector3& correctedAcc, const Rot3& incrR, const double deltaT, - OptionalJacobian<9, 9> F) { + const Vector3& measuredAcc, const Vector3& measuredOmega, + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) { + + Matrix3 D_Rij_incrR; + + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). + + Vector3 correctedAcc, correctedOmega; + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); + + // rotation vector describing rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // rotation increment computed from the current rotation rate measurement // Calculate acceleration in *current* i frame, i.e., before rotation update below Matrix3 D_acc_R; - const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F? &D_acc_R : 0); - - Matrix3 F_angles_angles; - updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); + const Matrix3 dRij = deltaRij_.matrix(); // expensive + const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F ? &D_acc_R : 0); double dt22 = 0.5 * deltaT * deltaT; + deltaTij_ += deltaT; + deltaRij_ = deltaRij_.compose(incrR, F ? &D_Rij_incrR : 0); deltaPij_ += dt22 * i_acc + deltaT * deltaVij_; deltaVij_ += deltaT * i_acc; - if (F) { - *F << // angle pos vel - F_angles_angles, Z_3x3, Z_3x3, // angle - dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos - deltaT * D_acc_R, Z_3x3, I_3x3; // vel - } -} + *F << // angle pos vel + D_Rij_incrR, Z_3x3, Z_3x3, // angle + dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos + deltaT * D_acc_R, Z_3x3, I_3x3; // vel -/// Update Jacobians to be used during preintegration -void PreintegrationBase::updatePreintegratedJacobians( - const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega, - const Rot3& incrR, double deltaT) { - const Matrix3 dRij = deltaRij_.matrix(); // expensive - const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT - * delRdelBiasOmega_; - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; - delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); + const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * delRdelBiasOmega_; + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij; + delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * temp; delVdelBiasAcc_ += -dRij * deltaT; - delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); + delVdelBiasOmega_ += temp * deltaT; + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * deltaT; } std::pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e41114b07..bcb38a8f9 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -170,12 +170,9 @@ public: bool equals(const PreintegrationBase& other, double tol) const; /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F); - - /// Update Jacobians to be used during preintegration - void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT); + void updatePreintegratedMeasurements(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double deltaT, + Matrix3* D_incrR_integratedOmega, Matrix9* F); std::pair correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,