From 8904fa4887ed7b3e6d110ad5a98efbc2df48595e Mon Sep 17 00:00:00 2001
From: Darius Rueckert <darius.rueckert@fau.de>
Date: Mon, 23 Mar 2020 14:59:26 +0100
Subject: [PATCH] Inline Jet initialization in Autodiff

Inlining the Jet initialzation is mandatory for good performance
in autodiff, because all the constants in the dual part can be
propagated into the cost functor.

This patch unrolls the initialization loop with templates and adds
EIGEN_ALWAYS_INLINE to the constructors.

Change-Id: Ic89d645984f3e1df6c63948236da823ba60d9620
---
 include/ceres/internal/autodiff.h | 27 +++++++++++++++++++--------
 1 file changed, 19 insertions(+), 8 deletions(-)

diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h
index 0bc41f262..cefcfb4c0 100644
--- a/include/ceres/internal/autodiff.h
+++ b/include/ceres/internal/autodiff.h
@@ -181,14 +181,24 @@ namespace internal {
 //
 // is what would get put in dst if N was 3, offset was 3, and the jet type JetT
 // was 8-dimensional.
-template <int Offset, int N, typename T, typename JetT>
-inline void Make1stOrderPerturbation(const T* src, JetT* dst) {
-  DCHECK(src);
-  DCHECK(dst);
-  for (int j = 0; j < N; ++j) {
-    dst[j] = JetT(src[j], Offset + j);
+template <int j, int N, int Offset, typename T, typename JetT>
+struct Make1stOrderPerturbation {
+ public:
+  static void Apply(const T* src, JetT* dst) {
+    if (j == 0) {
+      DCHECK(src);
+      DCHECK(dst);
+    }
+    dst[j] = JetT(src[j], j + Offset);
+    Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst);
   }
-}
+};
+
+template <int N, int Offset, typename T, typename JetT>
+struct Make1stOrderPerturbation<N, N, Offset, T, JetT> {
+ public:
+  static void Apply(const T* src, JetT* dst) {}
+};
 
 // Calls Make1stOrderPerturbation for every parameter block.
 //
@@ -208,7 +218,8 @@ struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>,
                                  Offset> {
   template <typename T, typename JetT>
   static void Apply(T const* const* parameters, JetT* x) {
-    Make1stOrderPerturbation<Offset, N>(parameters[ParameterIdx], x + Offset);
+    Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply(
+        parameters[ParameterIdx], x + Offset);
     Make1stOrderPerturbations<integer_sequence<int, Ns...>,
                               ParameterIdx + 1,
                               Offset + N>::Apply(parameters, x);