From 1756114986756f8571fee15a6039037369d9873e Mon Sep 17 00:00:00 2001
From: kartik arcot <kartik.arcot@verdantrobotics.com>
Date: Sat, 21 Jan 2023 11:20:01 -0800
Subject: [PATCH] optional jacobian with matrix references

---
 gtsam/base/OptionalJacobian.h             | 28 ++++++++++++++++++++++-
 gtsam/base/tests/testOptionalJacobian.cpp |  9 ++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index 3d5af97fd..22a17e131 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -19,8 +19,10 @@
 
 #pragma once
 #include <cstddef>
+#include <functional>
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
+#include <optional>
 #include <stdexcept>
 #include <string>
 
@@ -118,8 +120,21 @@ public:
                       "Expected: ") +
           "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
     }
+  } 
+
+  /// Constructor with std::nullopt just makes empty
+  OptionalJacobian(std::nullopt_t /*none*/) :
+      map_(nullptr) {
   }
 
+  /// Constructor compatible with old-style derivatives
+  OptionalJacobian(const std::optional<std::reference_wrapper<Eigen::MatrixXd>> optional) :
+      map_(nullptr) {
+    if (optional) {
+      optional->get().resize(Rows, Cols);
+      usurp(optional->get().data());
+    }
+  }
   /// Constructor that will usurp data of a block expression
   /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
   //  template <typename Derived, bool InnerPanel>
@@ -184,7 +199,7 @@ private:
 
 public:
 
-  /// Default constructor acts like 
+  /// Default constructor 
   OptionalJacobian() :
     pointer_(nullptr) {
   }
@@ -195,6 +210,17 @@ public:
   /// Construct from refrence to dynamic matrix
   OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
 
+  /// Constructor with std::nullopt just makes empty
+  OptionalJacobian(std::nullopt_t /*none*/) :
+    pointer_(nullptr) {
+  }
+
+  /// Constructor for optional matrix reference
+  OptionalJacobian(const std::optional<std::reference_wrapper<Eigen::MatrixXd>> optional) :
+      pointer_(nullptr) {
+    if (optional) pointer_ = &((*optional).get());
+  }
+
   /// Return true if allocated, false if default constructor was used
   operator bool() const {
     return pointer_!=nullptr;
diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp
index 7f1197584..4c775af75 100644
--- a/gtsam/base/tests/testOptionalJacobian.cpp
+++ b/gtsam/base/tests/testOptionalJacobian.cpp
@@ -20,6 +20,9 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <optional>
+#include <functional>
+
 using namespace std;
 using namespace gtsam;
 
@@ -32,6 +35,7 @@ using namespace gtsam;
 TEST( OptionalJacobian, Constructors ) {
   Matrix23 fixed;
   Matrix dynamic;
+  std::optional<std::reference_wrapper<Matrix>> optionalRef(std::ref(dynamic));
 
   OptionalJacobian<2, 3> H;
   EXPECT(!H);
@@ -40,12 +44,17 @@ TEST( OptionalJacobian, Constructors ) {
   TEST_CONSTRUCTOR(2, 3, &fixed, true);
   TEST_CONSTRUCTOR(2, 3, dynamic, true);
   TEST_CONSTRUCTOR(2, 3, &dynamic, true);
+  TEST_CONSTRUCTOR(2, 3, std::nullopt, false);
+  TEST_CONSTRUCTOR(2, 3, optionalRef, true);
 
   // Test dynamic
   OptionalJacobian<-1, -1> H7;
   EXPECT(!H7);
 
   TEST_CONSTRUCTOR(-1, -1, dynamic, true);
+  TEST_CONSTRUCTOR(-1, -1, std::nullopt, false);
+  TEST_CONSTRUCTOR(-1, -1, optionalRef, true);
+
 }
 
 //******************************************************************************