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)