def roll_and_pitch_from_world_to_image_correspondence(camera, correspondence):
"""
Computes the instantaneous roll and pitch of an image row from one gcp.
Args:
camera: instance of the ps.PushbroomCamera class
correspondence: list [r, c, h, lon, lat] which defines a correpondence
between the pixel (r, c) at row r and column c and the 3-space point
whose geographic coordinates, in degrees, are (lon, lat) and height
above the Earth, in meters, is h.
Returns:
roll and pitch values, in radians
"""
r, c, h, lon, lat = correspondence
t = r * camera.instrument.dwell_time # time associated to the row
# a: coordinates of the '3d image point' located on the image plane
R = utils.elemental_rotation_3d('z', np.polyval(camera.poly_psi, t)) # yaw
a = np.dot(R, camera.instrument.sight_vector(c))
# b: coordinates of the ground control point
b = np.dot(camera.rotational_to_orbital(t).T,
utils.geo_to_cartesian(lon, lat, h))
b += np.array([0, 0, camera.orbit.radius])
b /= np.linalg.norm(b)
# compute the needed roll and pitch angles
return roll_and_pitch_mapping_a_to_b(a, b)
def roll_and_pitch_from_world_to_image_correspondence(camera, correspondence):
"""
Computes the instantaneous roll and pitch of an image row from one gcp.
Args:
camera: instance of the ps.PushbroomCamera class
correspondence: list [r, c, h, lon, lat] which defines a correpondence
between the pixel (r, c) at row r and column c and the 3-space point
whose geographic coordinates, in degrees, are (lon, lat) and height
above the Earth, in meters, is h.
Returns:
roll and pitch values, in radians
"""
r, c, h, lon, lat = correspondence
t = r * camera.instrument.dwell_time # time associated to the row
# a: coordinates of the '3d image point' located on the image plane
R = utils.elemental_rotation_3d('z', np.polyval(camera.poly_psi, t)) # yaw
a = np.dot(R, camera.instrument.sight_vector(c))
# b: coordinates of the ground control point
b = np.dot(
camera.rotational_to_orbital(t).T, utils.geo_to_cartesian(lon, lat, h))
b += np.array([0, 0, camera.orbit.radius])
b /= np.linalg.norm(b)
# compute the needed roll and pitch angles
return roll_and_pitch_mapping_a_to_b(a, b)
def roll_and_pitch_mapping_a_to_b(a, b):
"""
Computes the roll and pitch needed to map a point on another, on a sphere.
The two input points are given by their cartesian coordinates (x, y, z). The
computed angles phi and psi are such that the rotation obtained by composing
an elemental rotation of phi about the x-axis with an elemental rotation of
psi about the new y-axis maps a to b.
Args:
a, b: two array-like objects of length 3.
Returns:
phi, psi: the two roll and pitch angles such that R_x(phi)R_y(psi)a = b.
"""
# checks that the input 3d points are on the same sphere
np.testing.assert_allclose(np.linalg.norm(a), np.linalg.norm(b))
# compute the roll angle phi
x = utils.solve_acos_plus_bsin_plus_c(b[1], b[2], -a[1])
# compute the pitch angle psi
bb = np.dot(np.linalg.inv(utils.elemental_rotation_3d('x', x)), b)
y = np.arctan2(bb[0], bb[2]) - np.arctan2(a[0], a[2])
# check that the composition of R_x(phi) and R_y(psi) maps a to b
np.testing.assert_allclose(np.dot(utils.rotation_3d('xyz', x, y, 0), a), b)
return x, y
def roll_and_pitch_mapping_a_to_b(a, b):
"""
Computes the roll and pitch needed to map a point on another, on a sphere.
The two input points are given by their cartesian coordinates (x, y, z). The
computed angles phi and psi are such that the rotation obtained by composing
an elemental rotation of phi about the x-axis with an elemental rotation of
psi about the new y-axis maps a to b.
Args:
a, b: two array-like objects of length 3.
Returns:
phi, psi: the two roll and pitch angles such that R_x(phi)R_y(psi)a = b.
"""
# checks that the input 3d points are on the same sphere
np.testing.assert_allclose(np.linalg.norm(a), np.linalg.norm(b))
# compute the roll angle phi
x = utils.solve_acos_plus_bsin_plus_c(b[1], b[2], -a[1])
# compute the pitch angle psi
bb = np.dot(np.linalg.inv(utils.elemental_rotation_3d('x', x)), b)
y = np.arctan2(bb[0], bb[2]) - np.arctan2(a[0], a[2])
# check that the composition of R_x(phi) and R_y(psi) maps a to b
np.testing.assert_allclose(np.dot(utils.rotation_3d('xyz', x, y, 0), a), b)
return x, y
