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")
def test_relative_orientation(self):
omega = np.deg2rad(1.5)
phi = np.deg2rad(2.3)
kappa = np.deg2rad(3.4)
camera = PinholeCamera()
camera.init_pinhole_from_coeffs(0, 0, 0, omega, phi, kappa, 100)
omega_1, phi_1, kappa_1 = photogram_utils.solve_for_omega_phi_kappa(camera.M)
assert np.isclose(omega, omega_1, atol=1.e-15)
assert np.isclose(phi, phi_1, atol=1.e-15)
assert np.isclose(kappa, kappa_1, atol=1.e-15)
print("solved values for omega, phi and kappa are within 1.e-15")
def test_rotation_matrix(self):
omega = np.deg2rad(1.5)
phi = np.deg2rad(2.3)
kappa = np.deg2rad(3.4)
camera = PinholeCamera()
camera.init_pinhole_from_coeffs(0, 0, 0, omega, phi, kappa, 100)
m_omega = np.zeros((3, 3))
m_phi = np.zeros((3, 3))
m_kappa = np.zeros((3, 3))
m_omega[0, 0] = 1.0
m_omega[1, 1] = np.cos(omega)
m_omega[1, 2] = np.sin(omega)
m_omega[2, 1] = -1*np.sin(omega)
m_omega[2, 2] = np.cos(omega)
m_phi[0, 0] = np.cos(phi)
m_phi[0, 2] = -1.0*np.sin(phi)
m_phi[1, 1] = 1.0
m_phi[2, 0] = np.sin(phi)
m_phi[2, 2] = np.cos(phi)
m_kappa[0, 0] = np.cos(kappa)
m_kappa[0, 1] = np.sin(kappa)
m_kappa[1, 0] = -1.0*np.sin(kappa)
m_kappa[1, 1] = np.cos(kappa)
m_kappa[2, 2] = 1.0
m_1 = np.matmul(m_kappa, np.matmul(m_phi, m_omega))
m_2 = m_kappa @ m_phi @ m_omega
assert np.isclose(m_1, m_2).all()
assert np.isclose(m_2, camera.M).all()
print("confirming rotation order for standard photogrammetric operations:")
print("the correct order of operations for:")
print("(first rotation) omega, then (second rotation) phi, then (third rotation) kapa is:")
print("m_kappa @ m_phi @ m_omega")
print("or")
print("np.matmul(m_kappa, np.matmul(m_phi, m_omega))")
def pinhole_timings():
point_calc = PinholeCamera()
point_calc.init_pinhole_from_coeffs(0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 50.0)
lon_center = 0
lat_center = 0
d_lon = 500
d_lat = 500
nx = 2000
ny = 2000
ground_grid = photogrammetry_utils.create_ground_grid(
lon_center - d_lon, lon_center + d_lon, lat_center - d_lat,
lat_center + d_lat, nx, ny)
lons = image_utils.flatten_image_band(ground_grid[0])
lats = image_utils.flatten_image_band(ground_grid[1])
alts = np.zeros_like(lats)
n_loops = 4
tic = time.time()
for n in range(n_loops):
point_calc.world_to_image_plane(lons, lats, alts)
toc = time.time()
print("calculated " + str(n_loops * nx * ny) + " pixels in " +
str(toc - tic) + " seconds.")
print(
str(n_loops * nx * ny / (toc - tic) / 1e6) + " Megapixels per second")
def init_from_wgs84_params(
cls,
sensor_lon_decimal_degrees, # type: float
sensor_lat_decimal_degrees, # type: float
sensor_altitude, # type: float
omega, # type: float
phi, # type: float
kappa, # type: float
npix_x, # type: int
npix_y, # type: int
pixel_pitch_x, # type: float
pixel_pitch_y, # type: float
focal_length, # type: float
alt_units='meters', # type: str
omega_units='radians', # type: str
phi_units='radians', # type: str
kappa_units='radians', # type: str
pixel_pitch_x_units='micrometer', # type: str
pixel_pitch_y_units='micrometer', # type: str
focal_length_units='mm', # type: str
flip_x=False, # type: bool
flip_y=False, # type: bool
): # type: (...) -> IdealPinholeFpaLocalUtmPointCalc
"""
:param sensor_lon_decimal_degrees:
:param sensor_lat_decimal_degrees:
:param sensor_altitude:
:param omega:
:param phi:
:param kappa:
:param npix_x:
:param npix_y:
:param pixel_pitch_x:
:param pixel_pitch_y:
:param focal_length:
:param alt_units:
:param omega_units:
:param phi_units:
:param kappa_units:
:param pixel_pitch_x_units:
:param pixel_pitch_y_units:
:param focal_length_units:
:param flip_x:
:param flip_y:
:return:
For now the altitude must be relative to the DEM used for orthorectification.
For example, if the DEM is relative to the geoid, then sensor_altitude must also be relative to the geoid too.
"""
native_proj = proj_utils.decimal_degrees_to_local_utm_proj(
sensor_lon_decimal_degrees, sensor_lat_decimal_degrees)
proj_4326 = crs_defs.PROJ_4326
sensor_x_local, sensor_y_local = transform(proj_4326, native_proj,
sensor_lon_decimal_degrees,
sensor_lat_decimal_degrees)
utm_point_calc = IdealPinholeFpaLocalUtmPointCalc()
pinhole_camera = PinholeCamera()
pinhole_camera.init_pinhole_from_coeffs(
sensor_x_local,
sensor_y_local,
sensor_altitude,
omega,
phi,
kappa,
focal_length,
x_units='meters',
y_units='meters',
z_units=alt_units,
omega_units=omega_units,
phi_units=phi_units,
kappa_units=kappa_units,
focal_length_units=focal_length_units)
utm_point_calc.set_projection(native_proj)
pp_x_meters = pixel_pitch_x * ureg.parse_expression(
pixel_pitch_x_units)
pp_y_meters = pixel_pitch_y * ureg.parse_expression(
pixel_pitch_y_units)
pp_x_meters = pp_x_meters.to('meters').magnitude
pp_y_meters = pp_y_meters.to('meters').magnitude
utm_point_calc._pixel_pitch_x_meters = pp_x_meters
utm_point_calc._pixel_pitch_y_meters = pp_y_meters
utm_point_calc.set_approximate_lon_lat_center(sensor_x_local,
sensor_y_local)
utm_point_calc._npix_x = npix_x
utm_point_calc._npix_y = npix_y
utm_point_calc._flip_x = flip_x
utm_point_calc._flip_y = flip_y
utm_point_calc._pinhole_camera = pinhole_camera
return utm_point_calc
def init_from_local_params(
cls,
sensor_lon, # type: float
sensor_lat, # type: float
sensor_altitude, # type: float
native_proj, # type: Proj
omega, # type: float
phi, # type: float
kappa, # type: float
npix_x, # type: int
npix_y, # type: int
pixel_pitch_x, # type: float
pixel_pitch_y, # type: float
focal_length, # type: float
alt_units='meters', # type: str
omega_units='radians', # type: str
phi_units='radians', # type: str
kappa_units='radians', # type: str
pixel_pitch_x_units='micrometer', # type: str
pixel_pitch_y_units='micrometer', # type: str
focal_length_units='mm', # type: str
flip_x=False, # type: bool
flip_y=False, # type: bool
): # type: (...) -> IdealPinholeFpaLocalUtmPointCalc
utm_point_calc = IdealPinholeFpaLocalUtmPointCalc()
pinhole_camera = PinholeCamera()
pinhole_camera.init_pinhole_from_coeffs(
sensor_lon,
sensor_lat,
sensor_altitude,
omega,
phi,
kappa,
focal_length,
x_units='meters',
y_units='meters',
z_units=alt_units,
omega_units=omega_units,
phi_units=phi_units,
kappa_units=kappa_units,
focal_length_units=focal_length_units)
utm_point_calc.set_projection(native_proj)
pp_x_meters = pixel_pitch_x * ureg.parse_expression(
pixel_pitch_x_units)
pp_y_meters = pixel_pitch_y * ureg.parse_expression(
pixel_pitch_y_units)
pp_x_meters = pp_x_meters.to('meters').magnitude
pp_y_meters = pp_y_meters.to('meters').magnitude
utm_point_calc._pixel_pitch_x_meters = pp_x_meters
utm_point_calc._pixel_pitch_y_meters = pp_y_meters
utm_point_calc.set_approximate_lon_lat_center(sensor_lon, sensor_lat)
utm_point_calc._npix_x = npix_x
utm_point_calc._npix_y = npix_y
utm_point_calc._flip_x = flip_x
utm_point_calc._flip_y = flip_y
utm_point_calc._pinhole_camera = pinhole_camera
return utm_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_projection(self):
camera = PinholeCamera()
camera.init_pinhole_from_coeffs(0, 0, 100, 0, 0, 0, 100)
x, y = camera.world_to_image_plane(100, 0, 0)
assert x == camera.f
stop = 1