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