From 9b393c3fc92ac6785fc607395efcbe4f269f1651 Mon Sep 17 00:00:00 2001
From: yetongumich <yetong@umich.edu>
Date: Tue, 2 Nov 2021 08:44:41 -0400
Subject: [PATCH] enable expression double multiply

---
 gtsam/nonlinear/Expression-inl.h         | 12 ++++++++++++
 gtsam/nonlinear/tests/testExpression.cpp | 13 +++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index cf2462dfc..b2ef6f055 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -246,6 +246,18 @@ struct apply_compose {
     return x.compose(y, H1, H2);
   }
 };
+
+template <>
+struct apply_compose<double> {
+  double operator()(const double& x, const double& y,
+                    OptionalJacobian<1, 1> H1 = boost::none,
+                    OptionalJacobian<1, 1> H2 = boost::none) const {
+    if (H1) H1->setConstant(y);
+    if (H2) H2->setConstant(x);
+    return x * y;
+  }
+};
+
 }
 
 // Global methods:
diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 80262ae3f..92f4998a2 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -293,6 +293,19 @@ TEST(Expression, compose3) {
   EXPECT(expected == R3.keys());
 }
 
+/* ************************************************************************* */
+// Test compose with double type (should be multiplication).
+TEST(Expression, compose4) {
+  // Create expression
+  gtsam::Key key = 1;
+  Double_ R1(key), R2(key);
+  Double_ R3 = R1 * R2;
+
+  // Check keys
+  set<Key> expected = list_of(1);
+  EXPECT(expected == R3.keys());
+}
+
 /* ************************************************************************* */
 // Test with ternary function.
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,