From fffb7ae69d2091b62e1cead527f656941bfc66d9 Mon Sep 17 00:00:00 2001
From: Brett Downing <brett@hacodyne.com>
Date: Thu, 27 Feb 2025 13:40:13 +1100
Subject: [PATCH] adds jacobian for t in Lie::Interpolate

---
 gtsam/base/Lie.h  |  6 ++++--
 tests/testLie.cpp | 13 +++++++++++--
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index 862ae6f4d..ce6541d21 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -326,8 +326,9 @@ T expm(const Vector& x, int K=7) {
 template <typename T>
 T interpolate(const T& X, const T& Y, double t,
               typename MakeOptionalJacobian<T, T>::type Hx = {},
-              typename MakeOptionalJacobian<T, T>::type Hy = {}) {
-  if (Hx || Hy) {
+              typename MakeOptionalJacobian<T, T>::type Hy = {},
+              typename MakeOptionalJacobian<T, double>::type Ht = {}) {
+  if (Hx || Hy || Ht) {
     typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
     const T between =
         traits<T>::Between(X, Y, between_H_x);  // between_H_y = identity
@@ -338,6 +339,7 @@ T interpolate(const T& X, const T& Y, double t,
 
     if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
     if (Hy) *Hy = t * exp_H * log_H;
+    if (Ht) *Ht = delta;
     return result;
   }
   return traits<T>::Compose(
diff --git a/tests/testLie.cpp b/tests/testLie.cpp
index ae934d66a..9272542fb 100644
--- a/tests/testLie.cpp
+++ b/tests/testLie.cpp
@@ -139,16 +139,19 @@ TEST(Lie, Interpolate) {
   Product y(Point2(6, 7), Pose2(8, 9, 0));
 
   double t;
-  Matrix actH1, numericH1, actH2, numericH2;
+  Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3;
 
   t = 0.0;
-  interpolate<Product>(x, y, t, actH1, actH2);
+  interpolate<Product>(x, y, t, actH1, actH2, actH3);
   numericH1 = numericalDerivative31<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH1, actH1, tol));
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 
   t = 0.5;
   interpolate<Product>(x, y, t, actH1, actH2);
@@ -158,6 +161,9 @@ TEST(Lie, Interpolate) {
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 
   t = 1.0;
   interpolate<Product>(x, y, t, actH1, actH2);
@@ -167,6 +173,9 @@ TEST(Lie, Interpolate) {
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 }
 
 //******************************************************************************