diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 5260597fa..57e86d3b6 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -72,6 +72,17 @@ bool PreintegratedCombinedMeasurements::equals( //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { + // Set the initial bias covariance to + // the estimated covariance from the last step. + p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); + PreintegrationType::resetIntegration(); + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void PreintegratedCombinedMeasurements::resetIntegration( + const gtsam::Matrix6& Q_init) { + p().biasAccOmegaInt = Q_init; PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 05e4b481e..ff247009e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -126,6 +126,15 @@ public: /// Re-initialize PreintegratedCombinedMeasurements void resetIntegration() override; + /** + * @brief Re-initialize PreintegratedCombinedMeasurements with initial bias + * covariance estimate. + * + * @param Q_init The initial bias covariance estimates as 6x6 matrix. If not + * provided, it uses the last values from the preintMeasCov. + */ + void resetIntegration(const gtsam::Matrix6& Q_init); + /// const reference to params, shadows definition in base class Params& p() const { return *boost::static_pointer_cast(this->p_); } /// @} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aacfff0f0..250a0dc42 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -250,6 +250,7 @@ TEST(CombinedImuFactor, CheckCovariance) { EXPECT(assert_equal(expected, actual.preintMeasCov())); } +/* ************************************************************************* */ // Test that the covariance values for the ImuFactor and the CombinedImuFactor // (top-left 9x9) are the same TEST(CombinedImuFactor, SameCovariance) { @@ -316,6 +317,38 @@ TEST(CombinedImuFactor, Accelerating) { EXPECT(assert_equal(estimatedCov, expected, 0.1)); } +/* ************************************************************************* */ +TEST(CombinedImuFactor, ResetIntegration) { + const double a = 0.2, v = 50; + + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(v, 0, 0); + + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); + + const double T = 3.0; // seconds + + CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); + + PreintegratedCombinedMeasurements pim = runner.integrate(T); + // Make copy for testing different conditions + PreintegratedCombinedMeasurements pim2 = pim; + + // Test default method + pim.resetIntegration(); + Matrix6 expected = Z_6x6; + EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt , 1e-9)); + + // Test method where Q_init is provided + Matrix6 expected_Q_init = I_6x6 * 0.001; + pim2.resetIntegration(expected_Q_init); + EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr;