def camera_to_sensor(self, camera_point):
# FTHETA: r = theta
# RECTILINEAR: r = tan(theta)
# EQUISOLID: r = 2 sin(theta / 2)
# ORTHOGRAPHIC: r = sin(theta)
# see https://wiki.panotools.org/Fisheye_Projection
if self.type == "FTHETA":
# r = theta <=>
# r = atan2(|xy|, -z)
xy = linalg.norm(camera_point[:2])
unit = normalize_vector(camera_point[:2])
r = math.atan2(xy, -camera_point[2])
return self.distort(r) * unit
elif self.type == "RECTILINEAR":
# r = tan(theta) <=>
# r = |xy| / -z <=>
# pre-distortion result is xy / -z
xy = linalg.norm(camera_point[:2])
unit = normalize_vector(camera_point[:2])
if -camera_point[2] <= 0: # outside FOV
r = math.tan(math.pi / 2)
else:
r = xy / -camera_point[2]
return self.distort(r) * unit
elif self.type == "EQUISOLID":
# r = 2 sin(theta / 2) <=>
# using sin(theta / 2) = sqrt((1 - cos(theta)) / 2)
# r = 2 sqrt((1 + z / |xyz|) / 2)
xy = linalg.norm(camera_point[:2])
unit = normalize_vector(camera_point[:2])
r = 2 * math.sqrt((1 + camera_point[2] / linalg.norm(camera_point)) / 2)
return self.distort(r) * unit
elif self.type == "ORTHOGRAPHIC":
# r = sin(theta) <=>
# r = |xy| / |xyz| <=>
# pre-distortion result is xy / |xyz|
xy = linalg.norm(camera_point[:2])
unit = normalize_vector(camera_point[:2])
if -camera_point[2] <= 0: # outside FOV
r = math.sin(math.pi / 2)
else:
r = linalg.norm(camera_point[:2] / linalg.norm(camera_point))
return self.distort(r) * unit
else:
log.fatal(f"unexpected type {self.type}")
def test_projection(self, vector, depth=1):
log.check(vector.shape == (3, ), "not a valid point in 3d")
camera = self.camera
expected_point = camera.position + depth * normalize_vector(vector)
if not camera.sees(expected_point)[0]:
# ignore cases where camera cannot see the point
return TestOutput(True, "point is outside the camera's view")
pixel = camera.world_to_pixel(expected_point)
actual_point = camera.pixel_to_world(pixel).point_at(depth)
expected = {"projected point": expected_point}
actual = {"projected point": actual_point}
return check_failed_attributes(expected, actual, 1e-3)
def __init__(self, origin, direction):
log.check_eq(len(origin), len(direction), "dimensions do not match")
direction = normalize_vector(direction)
self.origin = np.asarray(origin)
self.direction = np.asarray(direction)