Merge pull request #913 from borglab/feature/expression_double_multiply

enable expression double multiply

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:

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,