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

Shared data for triangulation unit tests

release/4.3a0
roderick-koehle 2021-07-13 22:00:21 +02:00 committed by GitHub
parent f53f5db4d1
commit 3402e46ad1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,6 +33,19 @@ class TestCal3Fisheye(GtsamTestCase):
u, v = np.arctan2(x, z), 0.0
cls.obj_point = np.array([x, y, z])
cls.img_point = np.array([u, v])
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)
pose1 = gtsam.Pose3(q1, p1)
pose2 = gtsam.Pose3(q2, p2)
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2])
cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras])
def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye()
@ -96,40 +109,17 @@ class TestCal3Fisheye(GtsamTestCase):
@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)
triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(measurements[0], self.img_point)
self.gtsamAssertEquals(triangulated, obj_point)
self.gtsamAssertEquals(triangulated, self.origin)
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)])
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
shared_cal = gtsam.Cal3_S2()
poses = gtsam.Pose3Vector([pose1, pose2])
triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(measurements[0], self.img_point)
self.gtsamAssertEquals(triangulated, obj_point)
self.gtsamAssertEquals(triangulated, self.origin)
def test_retract(self):
expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,