From bf1aff2f0eb5733a5dc9a497d5cf67cff25292e5 Mon Sep 17 00:00:00 2001
From: Julian Kent <jkflying@gmail.com>
Date: Wed, 4 Mar 2020 10:50:24 +0100
Subject: [PATCH] Fix 3+ nested Jet constructor

In Jet types, there is an attempt to set the derivative to 0.
However, if the derivative is not directly a scalar but a Jet<Jet>,
this cannot be constructed directly from a 0 literal. This is solved
by using the default constructor for the scalar type instead of 0.

The typical use case for this is adding constraints to the second
derivative of a curve in an Autodiff cost function.

Change-Id: Id480096632a731f312be12e294b3d6e244211529
---
 include/ceres/jet.h        |  6 +++---
 internal/ceres/jet_test.cc | 23 +++++++++++++++++++++++
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index 1879ea18c..1f81743e4 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -179,18 +179,18 @@ struct Jet {
   // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
   // the C++ standard mandates that e.g. default constructed doubles are
   // initialized to 0.0; see sections 8.5 of the C++03 standard.
-  Jet() : a() { v.setZero(); }
+  Jet() : a() { v.setConstant(Scalar()); }
 
   // Constructor from scalar: a + 0.
   explicit Jet(const T& value) {
     a = value;
-    v.setZero();
+    v.setConstant(Scalar());
   }
 
   // Constructor from scalar plus variable: a + t_i.
   Jet(const T& value, int k) {
     a = value;
-    v.setZero();
+    v.setConstant(Scalar());
     v[k] = T(1.0);
   }
 
diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc
index dff660bf2..a6cad626e 100644
--- a/internal/ceres/jet_test.cc
+++ b/internal/ceres/jet_test.cc
@@ -899,5 +899,28 @@ TEST(JetTraitsTest, ArrayScalarBinaryOps) {
   ExpectJetsClose(r4(1), r4(1));
 }
 
+TEST(Jet, nested3x) {
+  typedef Jet<J,2> JJ;
+  typedef Jet<JJ,2> JJJ;
+
+  JJJ x;
+  x.a = JJ(J(1, 0), 0);
+  x.v[0] = JJ(J(1));
+
+  JJJ y = x * x * x;
+
+  ExpectClose(y.a.a.a, 1, kTolerance);
+  ExpectClose(y.v[0].a.a, 3., kTolerance);
+  ExpectClose(y.v[0].v[0].a, 6., kTolerance);
+  ExpectClose(y.v[0].v[0].v[0], 6., kTolerance);
+
+  JJJ e = exp(x);
+
+  ExpectClose(e.a.a.a, kE, kTolerance);
+  ExpectClose(e.v[0].a.a, kE, kTolerance);
+  ExpectClose(e.v[0].v[0].a, kE, kTolerance);
+  ExpectClose(e.v[0].v[0].v[0], kE, kTolerance);
+}
+
 }  // namespace internal
 }  // namespace ceres