Fix CombinedImuFactor #882
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const { | |
| << endl; | ||
| cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]" | ||
| << endl; | ||
| cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]" | ||
|
|
||
| << endl; | ||
| } | ||
|
|
||
|
|
@@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth | |
| tol) && | ||
| equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance, | ||
| tol) && | ||
| equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol); | ||
| } | ||
|
|
||
| //------------------------------------------------------------------------------ | ||
|
|
@@ -110,17 +110,21 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( | |
| // and preintegrated measurements | ||
|
|
||
| // Single Jacobians to propagate covariance | ||
| Matrix3 theta_H_omega = C.topRows<3>(); | ||
| Matrix3 pos_H_acc = B.middleRows<3>(3); | ||
| Matrix3 vel_H_acc = B.bottomRows<3>(); | ||
|
|
||
| Matrix3 theta_H_biasOmegaInit = -theta_H_omega; | ||
| Matrix3 pos_H_biasAccInit = -pos_H_acc; | ||
| Matrix3 vel_H_biasAccInit = -vel_H_acc; | ||
|
|
||
| // overall Jacobian wrt preintegrated measurements (df/dx) | ||
| Eigen::Matrix<double, 15, 15> F; | ||
| F.setZero(); | ||
| F.block<9, 9>(0, 0) = A; | ||
| F.block<3, 3>(0, 12) = theta_H_omega; | ||
| F.block<3, 3>(3, 9) = pos_H_acc; | ||
| F.block<3, 3>(6, 9) = vel_H_acc; | ||
| F.block<6, 6>(9, 9) = I_6x6; | ||
|
|
||
| // Update the uncertainty on the state (matrix F in [4]). | ||
|
|
@@ -131,37 +135,51 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( | |
| const Matrix3& aCov = p().accelerometerCovariance; | ||
| const Matrix3& wCov = p().gyroscopeCovariance; | ||
| const Matrix3& iCov = p().integrationCovariance; | ||
| const Matrix6& bInitCov = p().biasAccOmegaInit; | ||
|
|
||
| // first order uncertainty propagation | ||
| // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() | ||
| Eigen::Matrix<double, 15, 15> G_measCov_Gt; | ||
| G_measCov_Gt.setZero(15, 15); | ||
|
|
||
| const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt; | ||
| const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt; | ||
| const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt; | ||
| const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt; | ||
|
|
||
| // BLOCK DIAGONAL TERMS | ||
| D_R_R(&G_measCov_Gt) = | ||
| (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) // | ||
| + | ||
| (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose()); | ||
|
|
||
| D_t_t(&G_measCov_Gt) = | ||
| (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) // | ||
| + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) // | ||
| + (dt * iCov); | ||
|
|
||
| D_v_v(&G_measCov_Gt) = | ||
|
There was a problem hiding this comment. Could you restore the order in which these terms are computed for easier review, and comment on what has changed and why? There was a problem hiding this comment. This order would be more aligned with the math and should make verifying it easier. There was a problem hiding this comment. I'm saying it's harder for review. It's also not arbitrary. Maybe re-arrange the math. The order of There was a problem hiding this comment. I'm a bit confused. It's already in attitude-position-velocity order (D_R_R for attitude, D_t_t for position and D_v_v fro velocity). The prior code had it position-velocity-attitude which isn't the expected order. There was a problem hiding this comment. @varunagrawal : besides the order of the pre-integrated measurements, can you point out what changed with respect to my (terrible) writeup? I think you added a second-order term in the position and changed how the covariances are discretized. were there other bugs you fixed with respect to my writeup? (i.e., was my writeup wrong, or only the implementation? or both? :-) ) also: is the order attitude-position-velocity now also used in the standard IMU factor? or only the combined? There was a problem hiding this comment. I believe just the implementation was incorrect in certain places. @lucacarlone's overall writeup was pretty great and I just expanded upon that with additional information I picked up from other books and papers. :) As for the attitude-position-velocity order, this was not really an issue for ImuFactor since the expression for the jacobians was a lot simpler ( There was a problem hiding this comment. I looked at the differences again: I also changed the noise variable order from $$\epsilon_{b^{\omega}{init}}, \epsilon{\Delta b^{a}}, \epsilon_{\Delta b^{\omega}}$$ to $$\epsilon_{int}, \epsilon_{b^{a}{init}}, \epsilon{b^{\omega}_{init}}$$ (Apologies for the rendering issues. Seems like Github's Math mode is a bit buggy.) |
||
| (vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) // | ||
| + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); | ||
|
|
||
| D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; | ||
| D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; | ||
|
|
||
| // OFF BLOCK DIAGONAL TERMS | ||
| D_R_t(&G_measCov_Gt) = | ||
|
There was a problem hiding this comment. Same. And why get rid of temp? There was a problem hiding this comment. I got rid of temp because the derivation is no longer |
||
| theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose(); | ||
| D_R_v(&G_measCov_Gt) = | ||
| theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose(); | ||
| D_t_R(&G_measCov_Gt) = | ||
| pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); | ||
| D_t_v(&G_measCov_Gt) = | ||
| (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) + | ||
| (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); | ||
| D_v_R(&G_measCov_Gt) = | ||
| vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); | ||
| D_v_t(&G_measCov_Gt) = | ||
| (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) + | ||
| (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()); | ||
|
|
||
| preintMeasCov_.noalias() += G_measCov_Gt; | ||
| } | ||
|
|
@@ -271,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { | |
| os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); | ||
| return os; | ||
| } | ||
|
|
||
| } // namespace gtsam | ||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
slightly confusing, since it is 15x15 in the combined factor. I would rephrase this sentence to keep it more general.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok, though Frank wanted this to be specific since we have different sections for ImuFactor and CombinedFactor. I'll add a corresponding paragraph in the
Combined IMU Factorsubsection.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
thanks!