From 0ced22841390b44cc3ce093ea0fcc217f222cfcb Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 22 Jul 2015 00:36:32 -0400
Subject: [PATCH] Regression test for predict with arbitrary measurements

---
 gtsam/navigation/tests/testImuFactor.cpp | 53 ++++++++++++++++++++----
 1 file changed, 44 insertions(+), 9 deletions(-)

diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index 6b66be342..1f1815fba 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@@ -38,6 +38,7 @@ using symbol_shorthand::B;
 
 static const Vector3 kGravity(0, 0, 9.81);
 static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
+static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
 
 /* ************************************************************************* */
 namespace {
@@ -282,8 +283,6 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
       Point3(5.5, 1.0, -50.0));
 
   // Measurements
-  Vector3 nonZeroOmegaCoriolis;
-  nonZeroOmegaCoriolis << 0, 0.1, 0.1;
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
@@ -298,7 +297,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
 
   // Create factor
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
-      nonZeroOmegaCoriolis);
+      kNonZeroOmegaCoriolis);
 
   Values values;
   values.insert(X(1), x1);
@@ -322,8 +321,6 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
       Point3(5.5, 1.0, -50.0));
 
   // Measurements
-  Vector3 nonZeroOmegaCoriolis;
-  nonZeroOmegaCoriolis << 0, 0.1, 0.1;
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
@@ -340,7 +337,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   Pose3 bodyPsensor = Pose3();
   bool use2ndOrderCoriolis = true;
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
-      nonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
+      kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
 
   Values values;
   values.insert(X(1), x1);
@@ -749,8 +746,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   Vector3 v2(Vector3(0.5, 0.0, 0.0));
 
   // Measurements
-  Vector3 nonZeroOmegaCoriolis;
-  nonZeroOmegaCoriolis << 0, 0.1, 0.1;
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
@@ -769,7 +764,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 
   // Create factor
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
-      nonZeroOmegaCoriolis);
+      kNonZeroOmegaCoriolis);
 
   Values values;
   values.insert(X(1), x1);
@@ -860,6 +855,46 @@ TEST(ImuFactor, PredictRotation) {
   EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2)));
 }
 
+/* ************************************************************************* */
+TEST(ImuFactor, PredictArbitrary) {
+  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
+
+  // Measurements
+  Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10);
+  Vector3 measuredAcc(0.1, 0.2, -9.81);
+  double deltaT = 0.001;
+
+  ImuFactor::PreintegratedMeasurements pre_int_data(
+      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
+      kMeasuredAccCovariance, kMeasuredOmegaCovariance,
+      kIntegrationErrorCovariance, true);
+
+  for (int i = 0; i < 1000; ++i)
+    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+  // Create factor
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
+      kZeroOmegaCoriolis);
+
+  // Predict
+  Pose3 x1, x2;
+  Vector3 v1 = Vector3(0, 0.0, 0.0);
+  Vector3 v2;
+  ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(),
+      kGravity, kZeroOmegaCoriolis);
+
+  // Regression test for Imu Refactor
+  Rot3 expectedR( //
+      +0.903715275, -0.250741668, 0.347026393, //
+      +0.347026393, 0.903715275, -0.250741668, //
+      -0.250741668, 0.347026393, 0.903715275);
+  Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711);
+  Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540);
+  Pose3 expectedPose(expectedR, expectedT);
+  EXPECT(assert_equal(expectedPose, x2, 1e-7));
+  EXPECT(assert_equal(Vector(expectedV), v2, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;