def test_fixture_yaw_boresight(self):
fixtured_cam = FixturedCamera()
fixtured_cam.set_relative_camera_xyz(0, 0, 0)
fixtured_cam.set_fixture_orientation_by_roll_pitch_yaw(
0, 0, 5, yaw_units='degrees')
fixtured_cam.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, -5, yaw_units='degrees')
camera_M = fixtured_cam.get_camera_absolute_M_matrix()
roll, pitch, yaw = photogram_utils.solve_for_omega_phi_kappa(camera_M)
assert np.isclose(roll, 0)
assert np.isclose(pitch, 0)
assert np.isclose(yaw, 0)
print(
"setting fixture yaw to 5 degrees and camera boresight yaw to -5 degrees results in a zero camera roation"
)
print("pitch boresight test passed.")
def test_fixture_pitch_boresight(self):
fixtured_cam = FixturedCamera()
fixtured_cam.set_relative_camera_xyz(0, 0, 0)
fixtured_cam.set_fixture_orientation_by_roll_pitch_yaw(
0, 5, 0, roll_units='degrees')
fixtured_cam.set_boresight_matrix_from_camera_relative_rpy_params(
0, -5, 0, roll_units='degrees')
camera_M = fixtured_cam.get_camera_absolute_M_matrix()
roll, pitch, yaw = photogram_utils.solve_for_omega_phi_kappa(camera_M)
# TODO: use higher precision floats if we want better absolute accuracy here
assert np.isclose(roll, 0, atol=1.e-7)
assert np.isclose(pitch, 0, atol=1.e-7)
assert np.isclose(yaw, 0, atol=1.e-7)
print(
"setting fixture pitch to 5 degrees and camera boresight pitch to -5 degrees results in a zero camera roation"
)
print("pitch boresight test passed.")
def __init__(self):
super(OpenCVPointCalc, self).__init__()
# intrinsic params
self._fx_pixels = 0
self._fy_pixels = 0
self._cx_pixels = 0
self._cy_pixels = 0
self._k1 = 0.0
self._k2 = 0.0
self._k3 = 0.0
self._p1 = 0.0
self._p2 = 0.0
self._pixel_pitch_microns = 0.0
self._camera_matrix = None
self._distortion_coeffs = None
# orientation params
self._x_meters = 0.0
self._y_meters = 0.0
self._z_meters = 0.0
self._omega_radians = 0.0
self._phi_radians = 0.0
self._kappa_radians = 0.0
# offsets
self._x_offset_meters = 0.0
self._y_offset_meters = 0.0
self._z_offset_meters = 0.0
self._omega_offset_radians = 0.0
self._phi_offset_radians = 0.0
self._kappa_offset_radians = 0.0
# fixture
self._fixture = FixturedCamera()
def _generate_camera_point_calc(
self,
lon,
lat,
alt,
roll,
pitch,
yaw,
):
fixtured_camera = FixturedCamera()
fixtured_camera.set_boresight_matrix_from_camera_relative_rpy_params(
self._boresight_roll,
self._boresight_pitch,
self._boresight_yaw,
roll_units=self._boresight_units,
pitch_units=self._boresight_units,
yaw_units=self._boresight_units,
order=self._boresight_rpy_order)
fixtured_camera.set_fixture_orientation_by_roll_pitch_yaw(
roll,
pitch,
yaw,
roll_units=self._boresight_units,
pitch_units=self._boresight_units,
yaw_units=self._boresight_units)
camera_1_m_matrix = fixtured_camera.get_camera_absolute_M_matrix()
omega, phi, kappa = photogrammetry_utils.solve_for_omega_phi_kappa(
camera_1_m_matrix)
point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_local_params(
lon,
lat,
alt,
self.world_projection,
omega,
phi,
kappa,
self._npix_x,
self._npix_y,
self._pixel_pitch_x,
self._pixel_pitch_y,
self._focal_length,
alt_units=self._external_orientation_spatial_units,
pixel_pitch_x_units=self._pixel_pitch_units,
pixel_pitch_y_units=self._pixel_pitch_units,
focal_length_units=self._focal_length_units,
flip_y=True)
return point_calc
def test_fixture_yaw_boresight2(self):
fixtured_cam_1 = FixturedCamera()
fixtured_cam_1.set_relative_camera_xyz(0, 0, 0)
fixtured_cam_1.set_fixture_orientation_by_roll_pitch_yaw(
0, 0, 0, roll_units='degrees')
fixtured_cam_1.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, 0, roll_units='degrees')
fixtured_cam_1.set_fixture_xyz(0, 0, 100)
cam1_x, cam1_y, cam1_z = fixtured_cam_1.get_camera_absolute_xyz()
cam1_M = fixtured_cam_1.get_camera_absolute_M_matrix()
omega1, phi1, kappa1 = photogram_utils.solve_for_omega_phi_kappa(
cam1_M)
pinhole_cam1 = PinholeCamera()
pinhole_cam1.init_pinhole_from_coeffs(cam1_x,
cam1_y,
cam1_z,
omega1,
phi1,
kappa1,
focal_length=50,
focal_length_units='millimeters')
fpa_x1, fpa_y1 = pinhole_cam1.world_to_image_plane(50, 0, 0)
fixtured_cam_1.set_fixture_orientation_by_roll_pitch_yaw(
0, 0, 5, yaw_units='degrees')
cam2_x, cam2_y, cam2_z = fixtured_cam_1.get_camera_absolute_xyz()
cam2_M = fixtured_cam_1.get_camera_absolute_M_matrix()
omega2, phi2, kappa2 = photogram_utils.solve_for_omega_phi_kappa(
cam2_M)
pinhole_cam2 = PinholeCamera()
pinhole_cam2.init_pinhole_from_coeffs(cam2_x,
cam2_y,
cam2_z,
omega2,
phi2,
kappa2,
focal_length=50,
focal_length_units='millimeters')
fpa_x2, fpa_y2 = pinhole_cam2.world_to_image_plane(50, 0, 0)
fixtured_cam_1.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, -5, yaw_units='degrees')
cam3_x, cam3_y, cam3_z = fixtured_cam_1.get_camera_absolute_xyz()
cam3_M = fixtured_cam_1.get_camera_absolute_M_matrix()
omega3, phi3, kappa3 = photogram_utils.solve_for_omega_phi_kappa(
cam3_M)
pinhole_cam2 = PinholeCamera()
pinhole_cam2.init_pinhole_from_coeffs(cam3_x,
cam3_y,
cam3_z,
omega3,
phi3,
kappa3,
focal_length=50,
focal_length_units='millimeters')
fpa_x3, fpa_y3 = pinhole_cam2.world_to_image_plane(50, 0, 0)
assert np.isclose(fpa_y1, fpa_y3)
assert np.isclose(fpa_x1, fpa_x3)
assert not np.isclose(fpa_x1, fpa_x2)
assert not np.isclose(fpa_y1, fpa_y2)
print(
"setting fixture to 5 degrees and camera boresight yaw to -5 degrees results in a zero camera rotation"
)
print("yaw boresight test passed.")
def test_x_lever_arm_offset_yaw(self):
lat = 43.085898
lon = -77.677624
sensor_alt = 100
temp_point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_wgs84_params(
lon, lat, sensor_alt, 0, 0, 0, 1000, 1000, 5, 5, 1)
fixture_local_lon, fixture_local_lat = temp_point_calc.get_approximate_lon_lat_center(
)
fixtured_cam = FixturedCamera()
fixtured_cam.set_fixture_xyz(fixture_local_lon, fixture_local_lat,
sensor_alt)
fixtured_cam.set_relative_camera_xyz(1, 0, 0)
fixtured_cam.set_fixture_orientation_by_roll_pitch_yaw(
0,
0,
10,
roll_units='degrees',
pitch_units='degrees',
yaw_units='degrees')
fixtured_cam.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, 0)
camera_M = fixtured_cam.get_camera_absolute_M_matrix()
camera_x, camera_y, camera_z = fixtured_cam.get_camera_absolute_xyz()
omega, phi, kappa = photogram_utils.solve_for_omega_phi_kappa(camera_M)
pinhole_cam = PinholeCamera()
pinhole_cam.init_pinhole_from_coeffs(camera_x,
camera_y,
camera_z,
omega,
phi,
kappa,
1,
focal_length_units='meters')
fpa_point_calc1 = IdealPinholeFpaLocalUtmPointCalc.init_from_local_params(
camera_x, camera_y, camera_z, temp_point_calc.get_projection(),
omega, phi, 0, 1000, 1000, 5, 5, 1)
fpa_point_calc2 = IdealPinholeFpaLocalUtmPointCalc.init_from_local_params(
camera_x, camera_y, camera_z, temp_point_calc.get_projection(),
omega, phi, kappa, 1000, 1000, 5, 5, 1)
fpa_lon_1, fpa_lat_1 = fpa_point_calc1.pixel_x_y_alt_to_lon_lat(
1000, 500, 0)
fpa_lon_2, fpa_lat_2 = fpa_point_calc2.pixel_x_y_alt_to_lon_lat(
1000, 500, 0)
fixture_x, fixture_y, fixture_z = fixtured_cam.get_fixture_xyz()
assert np.isclose(fpa_lat_1, camera_y)
assert fpa_lon_1 > camera_x
print(
"last pixel in the array is in the positive longitude direction at roll=0, pitch=0, yaw=0"
)
assert fpa_lon_2 < fpa_lon_1
assert fpa_lat_2 > fpa_lat_1
print(
"a positive yaw rotation results seem correct from the perspective of pixel shift"
)
assert camera_x < (fixture_x + 1)
assert camera_y > fixture_y
print(
"a positive yaw moves a camera originally located at (1, 0, 0) relative to the fixture to a greater y location and lower x location."
)
print("yaw rotation tests passed.")
def test_x_lever_arm_offset(self):
lat = 43.085898
lon = -77.677624
sensor_alt = 100
temp_point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_wgs84_params(
lon, lat, sensor_alt, 0, 0, 0, 1000, 1000, 5, 5, 1)
fixture_local_lon, fixture_local_lat = temp_point_calc.get_approximate_lon_lat_center(
)
fixtured_cam = FixturedCamera()
fixtured_cam.set_fixture_xyz(fixture_local_lon, fixture_local_lat,
sensor_alt)
fixtured_cam.set_relative_camera_xyz(1, 0, 0)
fixtured_cam.set_fixture_orientation_by_roll_pitch_yaw(
0,
10,
0,
roll_units='degrees',
pitch_units='degrees',
yaw_units='degrees')
fixtured_cam.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, 0)
camera_M = fixtured_cam.get_camera_absolute_M_matrix()
camera_x, camera_y, camera_z = fixtured_cam.get_camera_absolute_xyz()
omega, phi, kappa = photogram_utils.solve_for_omega_phi_kappa(camera_M)
pinhole_cam = PinholeCamera()
pinhole_cam.init_pinhole_from_coeffs(camera_x,
camera_y,
camera_z,
omega,
phi,
kappa,
1,
focal_length_units='meters')
fpa_point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_local_params(
camera_x, camera_y, camera_z, temp_point_calc.get_projection(),
omega, phi, kappa, 1000, 1000, 5, 5, 1)
fpa_center_lon, fpa_center_lat = fpa_point_calc.pixel_x_y_alt_to_lon_lat(
500, 500, 0)
fixture_x, fixture_y, fixture_z = fixtured_cam.get_fixture_xyz()
assert np.isclose(fixture_y, camera_y)
assert camera_z < fixture_z
assert camera_x < (fixture_x + 1)
assert np.isclose(
np.square(camera_x - fixture_x) + np.square(camera_z - fixture_z),
1.0)
assert np.isclose(fpa_center_lat, camera_y)
assert fpa_center_lon < camera_x
print(
"positive pitch results in camera center pointing to a higher latitude"
)
print("makes sense by right-hand-rule conventions.")
print(
"a positive y lever arm offset with a positive roll results in the camera moving higher in altitude and lower in latitude."
)
print("test passed.")
def test_y_lever_arm_offset(self):
lat = 43.085898
lon = -77.677624
sensor_alt = 100
temp_point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_wgs84_params(
lon, lat, sensor_alt, 0, 0, 0, 1000, 1000, 5, 5, 1)
fixture_local_lon, fixture_local_lat = temp_point_calc.get_approximate_lon_lat_center(
)
fixtured_cam = FixturedCamera()
fixtured_cam.set_fixture_xyz(fixture_local_lon, fixture_local_lat,
sensor_alt)
fixtured_cam.set_relative_camera_xyz(0, 1, 0)
fixtured_cam.set_fixture_orientation_by_roll_pitch_yaw(
0,
0,
0,
roll_units='degrees',
pitch_units='degrees',
yaw_units='degrees')
fixtured_cam.set_boresight_matrix_from_camera_relative_rpy_params(
0, 0, 0)
camera_M = fixtured_cam.get_camera_absolute_M_matrix()
camera_x, camera_y, camera_z = fixtured_cam.get_camera_absolute_xyz()
omega, phi, kappa = photogram_utils.solve_for_omega_phi_kappa(camera_M)
pinhole_cam = PinholeCamera()
pinhole_cam.init_pinhole_from_coeffs(camera_x,
camera_y,
camera_z,
omega,
phi,
kappa,
1,
focal_length_units='meters')
fpa_point_calc = IdealPinholeFpaLocalUtmPointCalc.init_from_local_params(
camera_x, camera_y, camera_z, temp_point_calc.get_projection(),
omega, phi, kappa, 1000, 1000, 5, 5, 1)
fpa_center_lon, fpa_center_lat = fpa_point_calc.pixel_x_y_alt_to_lon_lat(
500, 500, 0)
fixture_x, fixture_y, fixture_z = fixtured_cam.get_fixture_xyz()
assert np.isclose(fixture_x, fpa_center_lon)
assert np.isclose(fixture_y, fpa_center_lat)
assert np.isclose(camera_x, fixture_x)
assert np.isclose(camera_y - fixture_y, 1)
assert np.isclose(camera_z, fixture_z)
print("y lever arm is correctly returned by the fixtured camera")
class OpenCVPointCalc(AbstractEarthOverheadPointCalc):
def __init__(self):
super(OpenCVPointCalc, self).__init__()
# intrinsic params
self._fx_pixels = 0
self._fy_pixels = 0
self._cx_pixels = 0
self._cy_pixels = 0
self._k1 = 0.0
self._k2 = 0.0
self._k3 = 0.0
self._p1 = 0.0
self._p2 = 0.0
self._pixel_pitch_microns = 0.0
self._camera_matrix = None
self._distortion_coeffs = None
# orientation params
self._x_meters = 0.0
self._y_meters = 0.0
self._z_meters = 0.0
self._omega_radians = 0.0
self._phi_radians = 0.0
self._kappa_radians = 0.0
# offsets
self._x_offset_meters = 0.0
self._y_offset_meters = 0.0
self._z_offset_meters = 0.0
self._omega_offset_radians = 0.0
self._phi_offset_radians = 0.0
self._kappa_offset_radians = 0.0
# fixture
self._fixture = FixturedCamera()
@classmethod
def init_from_params(
cls,
params, # type: dict
): # type: (...) -> OpenCVPointCalc
point_calc = cls()
point_calc.set_projection(
pyproj.Proj(proj='utm',
zone=18,
ellps='WGS84',
datum='WGS84',
preserve_units=True))
point_calc.init_intrinsic(params['fx_pixels'], params['fy_pixels'],
params['cx_pixels'], params['cy_pixels'],
params['k1'], params['k2'], params['k3'],
params['p1'], params['p2'],
params['pixel_pitch_microns'])
point_calc.init_offsets(params['x_offset_meters'],
params['y_offset_meters'],
params['z_offset_meters'],
params['omega_offset_radians'],
params['phi_offset_radians'],
params['kappa_offset_radians'])
return point_calc
def init_intrinsic(
self,
fx_pixels, # type: float
fy_pixels, # type: float
cx_pixels, # type: float
cy_pixels, # type: float
k1, # type: float
k2, # type: float
k3, # type: float
p1, # type: float
p2, # type: float
pixel_pitch_microns # type: float
): # type: (...) -> None
self._fx_pixels = -fx_pixels
self._fy_pixels = -fy_pixels
self._cx_pixels = cx_pixels
self._cy_pixels = cy_pixels
self._k1 = k1
self._k2 = k2
self._k3 = k3
self._p1 = p1
self._p2 = p2
self._pixel_pitch_microns = pixel_pitch_microns
self._camera_matrix = np.array(
[[self._fx_pixels, 0, self._cx_pixels],
[0, self._fy_pixels, self._cy_pixels], [0, 0, 1]],
dtype=np.float64)
self._distortion_coeffs = np.array(
[self._k1, self._k2, self._p1, self._p2, self._k3],
dtype=np.float64)
def init_extrinsic(
self,
x_meters, # type: float
y_meters, # type: float
z_meters, # type: float
omega_radians, # type: float
phi_radians, # type: float
kappa_radians # type: float
): # type: (...) -> None
self._x_meters = x_meters
self._y_meters = y_meters
self._z_meters = z_meters
self._omega_radians = omega_radians
self._phi_radians = phi_radians
self._kappa_radians = kappa_radians
self._fixture.set_fixture_xyz(x_meters, y_meters, z_meters)
self._fixture.set_fixture_orientation_by_roll_pitch_yaw(
omega_radians, phi_radians, kappa_radians)
def init_offsets(
self,
x_offset_meters, # type: float
y_offset_meters, # type: float
z_offset_meters, # type: float
omega_offset_radians, # type: float
phi_offset_radians, # type: float
kappa_offset_radians # type: float
): # type: (...) -> None
self._x_offset_meters = x_offset_meters
self._y_offset_meters = y_offset_meters
self._z_offset_meters = z_offset_meters
self._omega_offset_radians = omega_offset_radians
self._phi_offset_radians = phi_offset_radians
self._kappa_offset_radians = kappa_offset_radians
self._fixture.set_relative_camera_xyz(x_offset_meters, y_offset_meters,
z_offset_meters)
self._fixture.set_boresight_matrix_from_camera_relative_rpy_params(
omega_offset_radians, phi_offset_radians, kappa_offset_radians)
def _lon_lat_alt_to_pixel_x_y_native(
self,
lons, # type: np.ndarray
lats, # type: np.ndarray
alts=None, # type: np.ndarray
band=None # type: int
): # type: (...) -> (np.ndarray, np.ndarray)
# https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
camera_rot = self._fixture.get_camera_absolute_M_matrix()
camera_xyz = self._fixture.get_camera_absolute_xyz()
trans_xyz = np.ones((3, len(lons)))
trans_xyz[0, :] = lons - camera_xyz[0]
trans_xyz[1, :] = lats - camera_xyz[1]
trans_xyz[2, :] = alts - camera_xyz[2]
cam_coords = np.matmul(camera_rot, trans_xyz)
x_prime = cam_coords[0, :] / cam_coords[2, :]
y_prime = cam_coords[1, :] / cam_coords[2, :]
r_squared = (x_prime * x_prime) + (y_prime * y_prime)
r_fourth = r_squared * r_squared
r_sixth = r_squared * r_squared * r_squared
radial_distortion = 1.0 + (self._k1 * r_squared) + (
self._k2 * r_fourth) + (self._k3 * r_sixth)
x_double_prime = (x_prime * radial_distortion) + (2.0 * self._p1 * x_prime * y_prime) + \
(self._p2 * (r_squared + 2.0 * x_prime * x_prime))
y_double_prime = (y_prime * radial_distortion) + (self._p1 * (r_squared + 2.0 * y_prime * y_prime)) + \
(2.0 * self._p2 * x_prime * y_prime)
u = -self._fx_pixels * x_double_prime + self._cx_pixels
v = self._fy_pixels * y_double_prime + self._cy_pixels
return u, v
def _pixel_x_y_alt_to_lon_lat_native(
self,
pixel_xs, # type: np.ndarray
pixel_ys, # type: np.ndarray
alts=None, # type: np.ndarray
band=None # type: np.ndarray
): # type: (...) -> (np.ndarray, np.ndarray)
pass