From 1bc9f3ac06c8524383508b78c6166059ee179cc7 Mon Sep 17 00:00:00 2001
From: krunalchande <krunalchande@gmail.com>
Date: Fri, 21 Nov 2014 19:39:46 -0500
Subject: [PATCH] Added unit tests

---
 .../tests/testCombinedImuFactor.cpp           | 60 +++++++++++++++++++
 1 file changed, 60 insertions(+)

diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp
index eb6fa7b4a..a87be6c50 100644
--- a/gtsam/navigation/tests/testCombinedImuFactor.cpp
+++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp
@@ -262,6 +262,66 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
+TEST(CombinedImuFactor, PredictPositionAndVelocity){
+  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
+
+  // Measurements
+  Vector3 gravity; gravity << 0, 0, 9.81;
+  Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
+  Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
+  Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
+  double deltaT = 0.001;
+  double tol = 1e-6;
+
+  Matrix I6x6(6,6);
+  I6x6 = Matrix::Identity(6,6);
+
+  CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
+      bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(),  I6x6, true );
+
+  for (int i = 0; i<1000; ++i)   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+    // Create factor
+    noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov());
+    CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
+
+    // Predict
+    Pose3 x1;
+    Vector3 v1(0, 0.0, 0.0);
+    PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis);
+    Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
+    Vector3 expectedVelocity; expectedVelocity<<0,1,0;
+    EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
+    EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
+
+
+}
+
+TEST(CombinedImuFactor, PredictRotation) {
+  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
+  Matrix I6x6(6,6);
+  CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
+      bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(),  I6x6, true );
+  Vector3 measuredAcc; measuredAcc << 0, 0, -9.81;
+  Vector3 gravity; gravity<<0,0,9.81;
+  Vector3 omegaCoriolis; omegaCoriolis << 0,0,0;
+  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0;
+  double deltaT = 0.001;
+  double tol = 1e-4;
+  for (int i = 0; i < 1000; ++i)
+    Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
+        deltaT);
+  CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
+      Combined_pre_int_data, gravity, omegaCoriolis);
+
+  // Predict
+  Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
+  Vector3 v(0,0,0);
+  PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis);
+  Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
+  EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
+}
+
 #include <gtsam/linear/GaussianFactorGraph.h>