12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small refactor for clarity

release/4.3a0
Frank Dellaert 2016-01-30 10:10:43 -08:00
parent 3af7e80f97
commit 3a2c2e23be
1 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -66,14 +66,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement( void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion
// Update preintegrated measurements. // Update preintegrated measurements.
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C; Matrix93 B, C;
PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, PreintegrationBase::update(measuredAcc, measuredOmega, dt,
&D_incrR_integratedOmega, &A, &B, &C); &D_incrR_integratedOmega, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first // Update preintegrated measurements covariance: as in [2] we consider a first
@ -83,8 +83,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements // and preintegrated measurements
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = -dRij * deltaT; Matrix3 H_vel_biasacc = -dRij * dt;
Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * dt;
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double, 15, 15> F; Eigen::Matrix<double, 15, 15> F;
@ -94,24 +94,27 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
F.block<3, 3>(6, 9) = H_vel_biasacc; F.block<3, 3>(6, 9) = H_vel_biasacc;
F.block<6, 6>(9, 9) = I_6x6; F.block<6, 6>(9, 9) = I_6x6;
// propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
const Matrix3& aCov = p().accelerometerCovariance;
const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance;
// first order uncertainty propagation // first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * // Optimized matrix multiplication (1/dt) * G * measurementCovariance * G.transpose()
// G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt; Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = D_v_v(&G_measCov_Gt) = (1 / dt) * H_vel_biasacc *
(1 / deltaT) * (H_vel_biasacc) * (aCov + p().biasAccOmegaInit.block<3, 3>(0, 0)) *
(p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) *
(H_vel_biasacc.transpose()); (H_vel_biasacc.transpose());
D_R_R(&G_measCov_Gt) = D_R_R(&G_measCov_Gt) = (1 / dt) * H_angles_biasomega *
(1 / deltaT) * (H_angles_biasomega) * (wCov + p().biasAccOmegaInit.block<3, 3>(3, 3)) *
(p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) *
(H_angles_biasomega.transpose()); (H_angles_biasomega.transpose());
D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) * Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) *