From d72f31fbc629dd60e5b61b9a2149d6f232867e1a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 9 Jun 2024 00:25:33 -0700
Subject: [PATCH] Actually use displacement in test

---
 gtsam/navigation/tests/testAHRSFactor.cpp | 32 ++++++++++++++++++++---
 1 file changed, 28 insertions(+), 4 deletions(-)

diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp
index 5f192d9c7..9d512d595 100644
--- a/gtsam/navigation/tests/testAHRSFactor.cpp
+++ b/gtsam/navigation/tests/testAHRSFactor.cpp
@@ -30,6 +30,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/factorTesting.h>
 
+#include <cmath>
 #include <list>
 
 using namespace std::placeholders;
@@ -300,6 +301,29 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
   // 1e-3 needs to be added only when using quaternions for rotations
 }
 
+//******************************************************************************
+// TEST(AHRSFactor, PimWithBodyDisplacement) {
+//   Vector3 bias(0, 0, 0.3);
+//   Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
+//   double deltaT = 1.0;
+
+//   auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
+//   p->gyroscopeCovariance = kMeasuredOmegaCovariance;
+//   p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0));
+//   PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
+
+//   pim.integrateMeasurement(measuredOmega, deltaT);
+
+//   Vector3 biasOmegaIncr(0.01, 0.0, 0.0);
+//   Matrix3 actualH;
+//   pim.biascorrectedDeltaRij(biasOmegaIncr, actualH);
+
+//   // Numerical derivative using a lambda function:
+//   auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); };
+//   Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(f, bias);
+//   EXPECT(assert_equal(expectedH, actualH));
+// }
+
 //******************************************************************************
 TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   Vector3 bias(0, 0, 0.3);
@@ -312,10 +336,10 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
   double deltaT = 1.0;
 
-  const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
-                            Point3(1, 0, 0));
-
-  PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance);
+  auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
+  p->gyroscopeCovariance = kMeasuredOmegaCovariance;
+  p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0));
+  PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
 
   pim.integrateMeasurement(measuredOmega, deltaT);