added functionality to change bias and reset integration

gtsam/navigation/PreintegrationBase.cpp

@@ -49,6 +49,12 @@ void PreintegrationBase::print(const string& s) const {
   cout << s << *this << endl;
 }
 
+//------------------------------------------------------------------------------
+void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
+	biasHat_ = biasHat;
+	resetIntegration();
+}
+
 //------------------------------------------------------------------------------
 pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
     const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,

gtsam/navigation/PreintegrationBase.h

@@ -97,6 +97,11 @@ public:
   /// Re-initialize PreintegratedMeasurements
   virtual void resetIntegration()=0;
 
+  /// @name Basic utilities
+  /// @{
+  /// Re-initialize PreintegratedMeasurements and set new bias
+  void resetIntegrationAndSetBias(const Bias& biasHat);
+
   /// check parameters equality: checks whether shared pointer points to same Params object.
   bool matchesParamsWith(const PreintegrationBase& other) const {
     return p_.get() == other.p_.get();

gtsam/navigation/tests/testImuFactor.cpp

@@ -67,6 +67,31 @@ TEST(ImuFactor, PreintegratedMeasurementsConstruction) {
   DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
 }
 
+/* ************************************************************************* */
+TEST(ImuFactor, PreintegratedMeasurementsReset) {
+
+	auto p = testing::Params();
+	// Create a preintegrated measurement struct and integrate
+	PreintegratedImuMeasurements pimActual(p);
+	Vector3 measuredAcc(0.5, 1.0, 0.5);
+	Vector3 measuredOmega(0.1, 0.3, 0.1);
+	double deltaT = 1.0;
+	pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+	// reset and make sure that it is the same as a fresh one
+	pimActual.resetIntegration();
+	CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p)));
+
+	// Now create one with a different bias ..
+	Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
+	PreintegratedImuMeasurements pimExpected(p, nonZeroBias);
+
+	// integrate again, then reset to a new bias
+	pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+	pimActual.resetIntegrationAndSetBias(nonZeroBias);
+	CHECK(assert_equal(pimActual, pimExpected));
+}
+
 /* ************************************************************************* */
 TEST(ImuFactor, Accelerating) {
   const double a = 0.2, v = 50;