From 6302a795330a00b44c5a61aab142e17238958252 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 4 Jun 2016 23:00:48 -0400
Subject: [PATCH] added functionality to change bias and reset integration

---
 gtsam/navigation/PreintegrationBase.cpp  |  6 ++++++
 gtsam/navigation/PreintegrationBase.h    |  5 +++++
 gtsam/navigation/tests/testImuFactor.cpp | 25 ++++++++++++++++++++++++
 3 files changed, 36 insertions(+)

diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index d99c86952..0e8cf67c9 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/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,
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 08dcd1381..06be4642d 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/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();
diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index 9db7aac72..d1dc6316d 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/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;