12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Test of jacobian of Cal3Fisheye for on-axis point

release/4.3a0
roderick-koehle 2021-10-22 19:33:06 +02:00 committed by GitHub
parent 9440ff6cd8
commit 0a1fead551
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
python/gtsam/tests/test_Cal3Fisheye.py
View File

@ -105,6 +105,28 @@ class TestCal3Fisheye(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.Cal3Fisheye()
state = gtsam.Values()
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
state.insert_point3(landmark_key, obj_point_on_axis)
state.insert_pose3(pose_key, pose)
state.insert_cal3fisheye(camera_key, camera)
g = gtsam.NonlinearFactorGraph()
noise_model = gtsam.noiseModel.Unit.Create(2)
factor = gtsam.GeneralSFMFactor2Cal3Fisheye(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_skipped(self):
"""Estimate spatial point from image measurements"""