From 1640f172e6ad56d107506daf4c46947a8908d7f5 Mon Sep 17 00:00:00 2001
From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com>
Date: Fri, 22 Oct 2021 19:34:27 +0200
Subject: [PATCH] Test jacobian of Cal3Unified for on-axis point

---
 python/gtsam/tests/test_Cal3Unified.py | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py
index dab1ae446..bafbacfa4 100644
--- a/python/gtsam/tests/test_Cal3Unified.py
+++ b/python/gtsam/tests/test_Cal3Unified.py
@@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase):
         score = graph.error(state)
         self.assertAlmostEqual(score, 0)
 
+    def test_jacobian(self):
+        """Evaluate jacobian at optical axis"""
+        obj_point_on_axis = np.array([0, 0, 1])
+        img_point = np.array([0.0, 0.0])
+        pose = gtsam.Pose3()
+        camera = gtsam.Cal3Unified()
+        state = gtsam.Values()
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
+        state.insert_cal3unified(camera_key, camera)
+        state.insert_point3(landmark_key, obj_point_on_axis)
+        state.insert_pose3(pose_key, pose)
+        g = gtsam.NonlinearFactorGraph()
+        noise_model = gtsam.noiseModel.Unit.Create(2)
+        factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key)
+        g.add(factor)
+        f = g.error(state)
+        gaussian_factor_graph = g.linearize(state)
+        H, z = gaussian_factor_graph.jacobian()
+        self.assertAlmostEqual(f, 0)
+        self.gtsamAssertEquals(z, np.zeros(2))
+        self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
+
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""