From f0295c7c322520bdc5c9ff5b409cda8c47049038 Mon Sep 17 00:00:00 2001
From: Brett Downing <brett@hacodyne.com>
Date: Fri, 28 Feb 2025 10:50:54 +1100
Subject: [PATCH] adds test for expression wrappers

---
 gtsam/geometry/tests/testPose3.cpp | 34 ++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index f5ebf29af..b1df45004 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/testLie.h>
 #include <gtsam/base/lieProxies.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/slam/expressions.h>
 
 
 #include <CppUnitLite/TestHarness.h>
@@ -1301,6 +1302,39 @@ TEST(Pose3, interpolateRtJacobians) {
   }
 }
 
+TEST(Pose3, expressionWrappers) {
+  Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
+  Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
+  double t = 0.3;
+  Values vals;
+  vals.insert(0,X);
+  vals.insert(1,Y);
+  vals.insert(2,t);
+  
+  { // interpolate (templated wrapper applies to all classes)
+    Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
+    std::vector<Matrix> Hlist = {{},{},{}};
+    Pose3 expected = interpolate(X, Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
+    Pose3 actual = interpolate(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
+
+    EXPECT(assert_equal(expected,actual,1e-6));
+    EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
+    EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
+    EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
+  }
+  { // interpolateRt (Pose3 specialisation)
+    Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
+    std::vector<Matrix> Hlist = {{},{},{}};
+    Pose3 expected = X.interpolateRt(Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
+    Pose3 actual = interpolateRt(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
+
+    EXPECT(assert_equal(expected,actual,1e-6));
+    EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
+    EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
+    EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
+  }
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Create) {
   Matrix63 actualH1, actualH2;