Skip to content

Commit

Permalink
Testing CameraSet and triangulatePoint3
Browse files Browse the repository at this point in the history
Currently triangulatePoint3 returns wrong results for fisheye models. The template for PinholePose may be implemented for a fixed size of variable dimensions.
  • Loading branch information
roderick-koehle authored Jul 11, 2021
1 parent 0a73961 commit 941594c
Showing 1 changed file with 39 additions and 1 deletion.
40 changes: 39 additions & 1 deletion python/gtsam/tests/test_Cal3Fisheye.py
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ def setUpClass(cls):
image plane and theta the incident angle of the object point.
"""
x, y, z = 1.0, 0.0, 1.0
# x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails!
# x, y, z = 0.5, 0.0, 2.0
u, v = np.arctan2(x, z), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
Expand Down Expand Up @@ -93,6 +93,44 @@ def test_sfm_factor2(self):
score = graph.error(state)
self.assertAlmostEqual(score, 0)

@unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
def test_triangulation_skipped(self):
"""Estimate spatial point from image measurements"""
p1 = [-1.0, 0.0, -1.0]
p2 = [ 1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
obj_point = np.array([0.0, 0.0, 0.0])
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(measurements[0], self.img_point)
self.gtsamAssertEquals(triangulated, obj_point)

def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
p1 = [-1.0, 0.0, -1.0]
p2 = [ 1.0, 0.0, -1.0]
q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0)
obj_point = np.array([0.0, 0.0, 0.0])
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras])
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)])
shared_cal = gtsam.Cal3_S2()
poses = gtsam.Pose3Vector([pose1, pose2])
triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(measurements[0], self.img_point)
self.gtsamAssertEquals(triangulated, obj_point)

def test_retract(self):
expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10)
Expand Down

0 comments on commit 941594c

Please sign in to comment.