From f3c9b3967b572bbd1b117fce674e19257effe151 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 16 Apr 2022 13:51:18 -0400
Subject: [PATCH] Use implicit conversion

---
 gtsam/base/OptionalJacobian.h | 15 +++++++++++++++
 gtsam/geometry/PinholePose.h  | 13 -------------
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
index 07801df7a..8a8891446 100644
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@@ -20,6 +20,7 @@
 #pragma once
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
+#include <stdexcept>
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@@ -96,6 +97,20 @@ public:
     usurp(dynamic->data());
   }
 
+  /**
+   * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
+   * @note This is important so we don't overwrite someone else's memory!
+   */
+  OptionalJacobian(Eigen::Ref<Eigen::MatrixXd> dynamic_ref) :
+      map_(nullptr) {
+    if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
+      usurp(dynamic_ref.data());
+    } else {
+      // It's never a good idea to throw in the constructor
+      throw std::invalid_argument("OptionalJacobian called with wrong dimensions or storage order.\n");
+    }
+  }
+
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 4bfbc8b2d..7b92be5d5 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -121,19 +121,6 @@ public:
     return _project(pw, Dpose, Dpoint, Dcal);
   }
 
-  /// project, but for Python use
-  Point2 project(const Point3& pw, Eigen::Ref<Matrix> Dpose, Eigen::Ref<Matrix> Dpoint, Eigen::Ref<Matrix> Dcal) const {
-    Eigen::Matrix<double, 2, 6> Dpose_;
-    Eigen::Matrix<double, 2, 3> Dpoint_;
-    Eigen::Matrix<double, 2, DimK> Dcal_;
-
-    auto ret = _project(pw, Dpose_, Dpoint_, Dcal_);
-    Dpose = Dpose_;
-    Dpoint = Dpoint_;
-    Dcal = Dcal_;
-    return ret;
-  }
-
   /// project a 3D point from world coordinates into the image
   Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none,