From a8e55e549ab6d0d0712a19c0b2123056e64e5988 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 2 May 2023 15:47:30 -0400
Subject: [PATCH 1/2] wrap Unit3 methods with Jacobians

---
 gtsam/geometry/geometry.i | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index e9929227a..ebc2d4d74 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -563,8 +563,19 @@ class Unit3 {
 
   // Other functionality
   Matrix basis() const;
+  Matrix basis(Eigen::Ref<Eigen::MatrixXd> H) const;
   Matrix skew() const;
   gtsam::Point3 point3() const;
+  gtsam::Point3 point3(Eigen::Ref<Eigen::MatrixXd> H) const;
+
+  Vector3 unitVector() const;
+  Vector3 unitVector(Eigen::Ref<Eigen::MatrixXd> H) const;
+  double dot(const gtsam::Unit3& q) const;
+  double dot(const gtsam::Unit3& q, Eigen::Ref<Eigen::MatrixXd> H1,
+             Eigen::Ref<Eigen::MatrixXd> H2) const;
+  Vector2 errorVector(const gtsam::Unit3& q) const;
+  Vector2 errorVector(const gtsam::Unit3& q, Eigen::Ref<Eigen::MatrixXd> H_p,
+                      Eigen::Ref<Eigen::MatrixXd> H_q) const;
 
   // Manifold
   static size_t Dim();

From 90eac3565cdbb133d85b7be8f4fe4ef77b0b4469 Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Tue, 2 May 2023 23:00:53 -0400
Subject: [PATCH 2/2] Add more wrapped functions

---
 gtsam/geometry/geometry.i | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index ebc2d4d74..630f6d252 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -125,6 +125,10 @@ class Point3 {
 
   // enabling serialization functionality
   void serialize() const;
+
+  // Other methods
+  gtsam::Point3 normalize(const gtsam::Point3 &p) const;
+  gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref<Eigen::MatrixXd> H) const;
 };
 
 class Point3Pairs {
@@ -342,6 +346,9 @@ class Rot3 {
 
   // Group action on Unit3
   gtsam::Unit3 rotate(const gtsam::Unit3& p) const;
+  gtsam::Unit3 rotate(const gtsam::Unit3& p,
+                      Eigen::Ref<Eigen::MatrixXd> HR,
+                      Eigen::Ref<Eigen::MatrixXd> Hp) const;
   gtsam::Unit3 unrotate(const gtsam::Unit3& p) const;
   
   // Standard Interface
@@ -582,6 +589,8 @@ class Unit3 {
   size_t dim() const;
   gtsam::Unit3 retract(Vector v) const;
   Vector localCoordinates(const gtsam::Unit3& s) const;
+  gtsam::Unit3 FromPoint3(const gtsam::Point3& point) const;
+  gtsam::Unit3 FromPoint3(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> H) const;
 
   // enabling serialization functionality
   void serialize() const;