diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h
index 020a5691c..b5068ad95 100644
--- a/gtsam/base/TestableAssertions.h
+++ b/gtsam/base/TestableAssertions.h
@@ -20,6 +20,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/global_includes.h>
 
+#include <functional>
 #include <optional>
 #include <map>
 #include <iostream>
@@ -71,6 +72,15 @@ bool assert_equal(const V& expected, const std::optional<V>& actual, double tol
   return assert_equal(expected, *actual, tol);
 }
 
+template<class V>
+bool assert_equal(const V& expected, 
+    const std::optional<std::reference_wrapper<const V>>& actual, double tol = 1e-9) {
+  if (!actual) {
+    std::cout << "actual is std::nullopt" << std::endl;
+    return false;
+  }
+  return assert_equal(expected, *actual.get(), tol);
+}
 
 /**
  * Function for comparing maps of testable->testable
diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index 7802f27e1..fb3bd948c 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/gtsam/base/tests/testMatrix.cpp
@@ -23,6 +23,8 @@
 #include <boost/tuple/tuple.hpp>
 #include <iostream>
 #include <sstream>
+#include <optional>
+#include <functional>
 
 using namespace std;
 using namespace gtsam;
@@ -1176,6 +1178,17 @@ TEST(Matrix, AbsoluteError) {
   EXPECT(isEqual);
 }
 
+// A test to check if a matrix and an optional reference_wrapper to
+// a matrix are equal.
+TEST(Matrix, MatrixRef) {
+  Matrix A = Matrix::Random(3, 3);
+  Matrix B = Matrix::Random(3, 3);
+
+  EXPECT(assert_equal(A, A));
+  EXPECT(assert_equal(A, std::cref(A)));
+  EXPECT(!assert_equal(A, std::cref(B)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h
index 5c3cec63f..fff047851 100644
--- a/gtsam/nonlinear/ISAM2UpdateParams.h
+++ b/gtsam/nonlinear/ISAM2UpdateParams.h
@@ -37,16 +37,16 @@ struct ISAM2UpdateParams {
 
   /** An optional map of keys to group labels, such that a variable can be
    * constrained to a particular grouping in the BayesTree */
-  std::optional<FastMap<Key, int>> constrainedKeys{std::nullopt};
+  std::optional<FastMap<Key, int>> constrainedKeys;
 
   /** An optional set of nonlinear keys that iSAM2 will hold at a constant
    * linearization point, regardless of the size of the linear delta */
-  std::optional<FastList<Key>> noRelinKeys{std::nullopt};
+  std::optional<FastList<Key>> noRelinKeys;
 
   /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless
    * of the size of the linear delta. This allows the provided keys to be
    * reordered. */
-  std::optional<FastList<Key>> extraReelimKeys{std::nullopt};
+  std::optional<FastList<Key>> extraReelimKeys;
 
   /** Relinearize any variables whose delta magnitude is sufficiently large
    * (Params::relinearizeThreshold), regardless of the relinearization
@@ -63,7 +63,7 @@ struct ISAM2UpdateParams {
    *    depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include
    *    its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`.
    */
-  std::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys{std::nullopt};
+  std::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys;
 
   /** By default, iSAM2 uses a wildfire update scheme that stops updating when
    * the deltas become too small down in the tree. This flagg forces a full
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 0618f4eb1..cf0f5aa7f 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -21,7 +21,6 @@
 
 #pragma once
 
-#include <cstddef>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/GraphvizFormatting.h>
diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h
index 45240ecfc..7184b3dfc 100644
--- a/gtsam/nonlinear/NonlinearOptimizerParams.h
+++ b/gtsam/nonlinear/NonlinearOptimizerParams.h
@@ -23,7 +23,9 @@
 
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphSolver.h>
+
 #include <string>
+#include <optional>
 
 namespace gtsam {