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