def _update_target(self):
"""
Change camera orientation so that target is on the camera bore-sight defined by target_axis vector.
Additional constraint is needed for unique final rotation, this can be provided by target_up vector.
"""
boresight = np.array(self.target_axis)
loc = self.target.loc - self.loc
axis = np.cross(boresight, loc)
angle = tools.angle_between_v(boresight, loc)
q = tools.angleaxis_to_q((angle,) + tuple(axis))
# if up target given, use it
if self.target_up is not None:
current_up = tools.q_times_v(q, np.array(self.target_axis_up))
target_up = np.array(self.target_up)
# project target_up on camera bore-sight, then remove the projection from target_up to get
# it projected on a plane perpendicular to the bore-sight
target_up_proj = target_up - np.dot(target_up, loc) * loc / np.dot(loc, loc)
if np.linalg.norm(target_up_proj) > 0:
axis = np.cross(target_up_proj, current_up)
angle = tools.angle_between_v(current_up, target_up_proj)
q = tools.angleaxis_to_q((angle,) + tuple(axis)) * q
self.q = q
def north_to_up(sc_q):
# assume -y axis is towards north, camera borehole is along +z axis, and cam up is towards -y axis
north_v, cam_axis, cam_up = np.array([0, -1, 0]), np.array([0, 0, 1]), np.array([0, -1, 0])
# north vector in image frame
sc_north = tools.q_times_v(sc_q, north_v)
# project to image plane
img_north = tools.vector_rejection(sc_north, cam_axis)
# calculate angle between projected north vector and image up
angle = tools.angle_between_v(cam_up, img_north, direction=cam_axis)
return angle
def closest_scene(self, sc_ast_q=None, light_v=None):
""" in opengl frame """
if sc_ast_q is None:
sc_ast_q, _ = self.system_model.gl_sc_asteroid_rel_q()
if light_v is None:
light_v, _ = self.system_model.gl_light_rel_dir()
d_sc_ast_q, i1 = tools.discretize_q(sc_ast_q,
points=self._fdb_sc_ast_perms)
err_q = sc_ast_q * d_sc_ast_q.conj()
c_light_v = tools.q_times_v(err_q.conj(), light_v)
d_light_v, i2 = tools.discretize_v(c_light_v,
points=self._fdb_light_perms)
err_angle = tools.angle_between_v(light_v, d_light_v)
return i1, i2, d_sc_ast_q, d_light_v, err_q, err_angle
def process(self, orig_sce_img, outfile, rotate_sc=False, **kwargs):
# maybe load torch model
if self.model is None:
self.load_model()
if outfile is not None:
self.debug_filebase = outfile + ('n' if isinstance(
orig_sce_img, str) else '')
# maybe load scene image
if isinstance(orig_sce_img, str):
orig_sce_img = self.load_target_image(orig_sce_img)
self.timer = Stopwatch()
self.timer.start()
if self.DEF_ESTIMATE_THRESHOLD:
threshold = ImageProc.optimal_threshold(None, orig_sce_img)
else:
threshold = self.DEF_LUMINOSITY_THRESHOLD
# detect target, get bounds
x, y, w, h = ImageProc.single_object_bounds(
orig_sce_img,
threshold=threshold,
crop_marg=self.DEF_CROP_MARGIN,
min_px=self.DEF_MIN_PIXELS,
debug=DEBUG)
if x is None:
raise PositioningException('asteroid not detected in image')
# crop image
img_bw = ImageProc.crop_and_zoom_image(orig_sce_img, x, y, w, h, None,
(224, 224))
# save cropped image in log archive
if BATCH_MODE and self.debug_filebase:
self.timer.stop()
cv2.imwrite(self.debug_filebase + 'a.png', img_bw)
self.timer.start()
# massage input
input = cv2.cvtColor(img_bw, cv2.COLOR_GRAY2BGR)
input = Image.fromarray(input)
input = PoseIllumiDataset.eval_transform(input)[None, :, :, :].to(
self.device, non_blocking=True)
# run model
with torch.no_grad():
output = self.model(input)
# massage output
output = output[0] if isinstance(output, (list, tuple)) else output
output = output.detach().cpu().numpy()
# check if estimated illumination direction is close or not
ill_est = self.model.illumination(output)[0]
r_ini, q_ini, ill_ini = self.system_model.get_cropped_system_scf(
x, y, w, h)
if tools.angle_between_v(
ill_est, ill_ini) > 10: # max 10 degree discrepancy accepted
print(
'bad illumination direction estimated, initial=%s, estimated=%s'
% (ill_ini, ill_est))
# apply result
r_est = self.model.position(output)[0]
q_est = np.quaternion(*self.model.rotation(output)[0])
self.system_model.set_cropped_system_scf(x,
y,
w,
h,
r_est,
q_est,
rotate_sc=rotate_sc)
self.timer.stop()
if False:
r_est2, q_est2, ill_est2 = self.system_model.get_cropped_system_scf(
x, y, w, h)
self.system_model.swap_values_with_real_vals()
r_real, q_real, ill_real = self.system_model.get_cropped_system_scf(
x, y, w, h)
self.system_model.swap_values_with_real_vals()
print('compare q_est vs q_est2, q_real vs q_est, q_real vs q_est2')
# save result image
if BATCH_MODE and self.debug_filebase:
# save result in log archive
res_img = self.render(textures=False)
sce_img = cv2.resize(orig_sce_img, tuple(np.flipud(res_img.shape)))
cv2.imwrite(self.debug_filebase + 'b.png',
np.concatenate((sce_img, res_img), axis=1))
if DEBUG:
cv2.imshow('compare', np.concatenate((sce_img, res_img),
axis=1))
cv2.waitKey()
def load_image_meta(src, sm):
# params given in equatorial J2000 coordinates, details:
# https://pds.nasa.gov/ds-view/pds/viewProfile.jsp
# ?dsid=RO-C-NAVCAM-2-ESC3-MTP021-V1.0
with open(src, 'r') as f:
config_data = f.read()
config_data = '[meta]\n' + config_data
config_data = re.sub(r'^/\*', '#', config_data, flags=re.M)
config_data = re.sub(r'^\^', '', config_data, flags=re.M)
config_data = re.sub(r'^(\w+):(\w+)', r'\1__\2', config_data, flags=re.M)
config_data = re.sub(r'^END\s*$', '', config_data, flags=re.M)
config_data = re.sub(r'^NOTE\s*=\s*"[^"]*"', '', config_data, flags=re.M)
config_data = re.sub(r'^OBJECT\s*=\s*.*?END_OBJECT\s*=\s*\w+',
'',
config_data,
flags=re.M | re.S)
config_data = re.sub(r' <(DEGREE|SECOND|KILOMETER)>', '', config_data)
config = ConfigParser(converters={'tuple': literal_eval})
config.read_string(config_data)
image_time = config.get('meta', 'START_TIME')
# spacecraft orientation, equatorial J2000
sc_rot_ra = config.getfloat('meta', 'RIGHT_ASCENSION')
sc_rot_dec = config.getfloat('meta', 'DECLINATION')
sc_rot_cnca = config.getfloat('meta', 'CELESTIAL_NORTH_CLOCK_ANGLE')
sc_igrf_q = tools.ypr_to_q(
rads(sc_rot_dec), rads(sc_rot_ra),
-rads(sc_rot_cnca)) # same with rosetta lbls also
# from asteroid to spacecraft, asteroid body fixed coordinates
# TODO: figure out why FOR SOME REASON distance is given ~30x too close
ast_sc_dist = config.getfloat('meta', 'TARGET_CENTER_DISTANCE') * 30
ast_sc_lat = config.getfloat('meta', 'SUB_SPACECRAFT_LATITUDE')
ast_sc_lon = config.getfloat('meta', 'SUB_SPACECRAFT_LONGITUDE')
ast_sc_bff_r = tools.spherical2cartesian(rads(ast_sc_lat),
rads(ast_sc_lon), ast_sc_dist)
ast_axis_img_clk_ang = config.getfloat('meta', 'BODY_POLE_CLOCK_ANGLE')
ast_axis_img_plane_ang = config.getfloat(
'meta', 'HAY__BODY_POLE_ASPECT_ANGLE') # what is the use?
# from sun to spacecraft, equatorial J2000
ast_sun_dist = config.getfloat('meta', 'TARGET_HELIOCENTRIC_DISTANCE')
ast_sun_lat = config.getfloat('meta', 'SUB_SOLAR_LATITUDE')
ast_sun_lon = config.getfloat('meta', 'SUB_SOLAR_LONGITUDE')
sun_ast_bff_r = -tools.spherical2cartesian(rads(ast_sun_lat),
rads(ast_sun_lon), ast_sun_dist)
sun_sc_bff_r = sun_ast_bff_r + ast_sc_bff_r
ast_axis_sun_ang = config.getfloat('meta', 'HAY__BODY_POLE_SUN_ANGLE')
a = config.getfloat('meta', 'SUB_SOLAR_AZIMUTH') # what is this!?
# TODO: continue here
ast_axis_scf_q = tools.ypr_to_q(-rads(ast_sc_lat), -rads(ast_sc_lon), 0)
# TODO: figure out: how to get roll as some ast_axis_img_clk_ang come from ast_sc_lat?
ast_rot_scf_q = tools.ypr_to_q(0, 0, -rads(ast_axis_img_clk_ang))
ast_scf_q = ast_axis_scf_q #* ast_rot_scf_q
dec = 90 - ast_sc_lat
ra = -ast_sc_lon
if dec > 90:
dec = 90 + ast_sc_lat
ra = tools.wrap_degs(ra + 180)
print('ra: %f, dec: %f, zlra: %f' % (ra, dec, ast_axis_img_clk_ang))
ast_igrf_q = ast_scf_q * sc_igrf_q
sun_ast_igrf_r = tools.q_times_v(ast_igrf_q, sun_ast_bff_r)
ast_sc_igrf_r = tools.q_times_v(ast_igrf_q, ast_sc_bff_r)
sun_sc_igrf_r = tools.q_times_v(ast_igrf_q, sun_sc_bff_r)
z_axis = np.array([0, 0, 1])
x_axis = np.array([1, 0, 0])
ast_axis_u = tools.q_times_v(ast_igrf_q, z_axis)
ast_zlon_u = tools.q_times_v(ast_igrf_q, x_axis)
ast_axis_dec, ast_axis_ra, _ = tools.cartesian2spherical(*ast_axis_u)
ast_zlon_proj = tools.vector_rejection(ast_zlon_u, z_axis)
ast_zlon_ra = tools.angle_between_v(ast_zlon_proj, x_axis)
ast_zlon_ra *= 1 if np.cross(x_axis, ast_zlon_proj).dot(z_axis) > 0 else -1
# frame where ast zero lat and lon point towards the sun?
# ast_axis_ra = -ast_sun_lon
# ast_axis_dec = 90 - ast_sun_lat
# ast_axis_zero_lon_ra = 0
arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr])
print('sun_ast_bff_r: %s' % arr2str(sun_ast_bff_r * 1e3))
print('sun_sc_bff_r: %s' % arr2str(sun_sc_bff_r * 1e3))
print('ast_sc_bff_r: %s' % arr2str(ast_sc_bff_r * 1e3))
# TODO: even the light is wrong, should be right based on the sun_ast and sun_sc vectors!!
print('sun_ast_igrf_r: %s' % arr2str(sun_ast_igrf_r * 1e3))
print('sun_sc_igrf_r: %s' % arr2str(sun_sc_igrf_r * 1e3))
print('ast_sc_igrf_r: %s' % arr2str(ast_sc_igrf_r * 1e3))
print('ast_axis_ra: %f' % degs(ast_axis_ra))
print('ast_axis_dec: %f' % degs(ast_axis_dec))
print('ast_zlon_ra: %f' % degs(ast_zlon_ra))
aa = quaternion.as_rotation_vector(sc_igrf_q)
angle = np.linalg.norm(aa)
sc_angleaxis = [angle] + list(aa / angle)
print('sc_angleaxis [rad]: %s' % arr2str(sc_angleaxis))