From f5ec070f9fc87d52076846ff394b79b001f4bd13 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 7 Jul 2022 15:09:55 -0400
Subject: [PATCH 1/2] wrap BearingRange classes and factors

---
 gtsam/geometry/geometry.i | 6 ++++++
 gtsam/sam/sam.i           | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 92d9ec0a39..f3f8a5c793 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1279,5 +1279,11 @@ class BearingRange {
 
 typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double>
     BearingRange2D;
+typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double>
+    BearingRangePose2;
+typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Point3, gtsam::Unit3, double>
+    BearingRange3D;
+typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3, double>
+    BearingRangePose3;
 
 }  // namespace gtsam
diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i
index 90c319ede7..a46a6de9e4 100644
--- a/gtsam/sam/sam.i
+++ b/gtsam/sam/sam.i
@@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
                                   double>
     BearingRangeFactorPose2;
+typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3,
+                                  double>
+    BearingRangeFactor3D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3,
+                                  double>
+    BearingRangeFactorPose3;
 
 }  // namespace gtsam

From 2ab09a580fa059b7dd5cd0750365927f81f5a5e0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 7 Jul 2022 15:24:56 -0400
Subject: [PATCH 2/2] add Python unit tests

---
 gtsam/linear/linear.i              |  2 +-
 python/gtsam/tests/test_Factors.py | 22 ++++++++++++++++++++--
 python/gtsam/tests/test_sam.py     | 28 ++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index 943b661d88..fdf156ff99 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -279,7 +279,6 @@ virtual class GaussianFactor {
 virtual class JacobianFactor : gtsam::GaussianFactor {
   //Constructors
   JacobianFactor();
-  JacobianFactor(const gtsam::GaussianFactor& factor);
   JacobianFactor(Vector b_in);
   JacobianFactor(size_t i1, Matrix A1, Vector b,
       const gtsam::noiseModel::Diagonal* model);
@@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   JacobianFactor(const gtsam::GaussianFactorGraph& graph,
                  const gtsam::Ordering& ordering,
                  const gtsam::VariableSlots& p_variableSlots);
+  JacobianFactor(const gtsam::GaussianFactor& factor);
 
   //Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py
index 3ec8648a49..a5688eec86 100644
--- a/python/gtsam/tests/test_Factors.py
+++ b/python/gtsam/tests/test_Factors.py
@@ -11,9 +11,8 @@
 """
 import unittest
 
-import numpy as np
-
 import gtsam
+import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -21,6 +20,7 @@ class TestNonlinearEquality2Factor(GtsamTestCase):
     """
     Test various instantiations of NonlinearEquality2.
     """
+
     def test_point3(self):
         """Test for Point3 version."""
         factor = gtsam.NonlinearEquality2Point3(0, 1)
@@ -30,5 +30,23 @@ def test_point3(self):
         np.testing.assert_allclose(error, np.zeros(3))
 
 
+class TestJacobianFactor(GtsamTestCase):
+    """Test JacobianFactor"""
+
+    def test_gaussian_factor_graph(self):
+        """Test construction from GaussianFactorGraph."""
+        gfg = gtsam.GaussianFactorGraph()
+        jf = gtsam.JacobianFactor(gfg)
+        self.assertIsInstance(jf, gtsam.JacobianFactor)
+
+        nfg = gtsam.NonlinearFactorGraph()
+        nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))
+        values = gtsam.Values()
+        values.insert(1, 0.0)
+        gfg = nfg.linearize(values)
+        jf = gtsam.JacobianFactor(gfg)
+        self.assertIsInstance(jf, gtsam.JacobianFactor)
+
+
 if __name__ == "__main__":
     unittest.main()
diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py
index e01b9c1d1c..fd8da53080 100644
--- a/python/gtsam/tests/test_sam.py
+++ b/python/gtsam/tests/test_sam.py
@@ -50,6 +50,34 @@ def test_BearingRangeFactor2D(self):
         self.assertEqual(range_measurement, measurement.range())
         self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
 
+    def test_BearingRangeFactor3D(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactor3D.
+        """
+        bearing_measurement = gtsam.Unit3()
+        range_measurement = 10.0
+        factor = gtsam.BearingRangeFactor3D(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(3, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
+    def test_BearingRangeFactorPose3(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactorPose3.
+        """
+        range_measurement = 10.0
+        bearing_measurement = gtsam.Unit3()
+        factor = gtsam.BearingRangeFactorPose3(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(3, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
 
 if __name__ == "__main__":
     unittest.main()