def scene_features(self, feat, maxmem, i1, i2):
try:
ref_img, depth = self.render_scene(i1, i2)
except InvalidSceneException:
return None
# get keypoints and descriptors
ref_kp, ref_desc, self._latest_detector = KeypointAlgo.detect_features(ref_img, feat, maxmem=maxmem,
max_feats=self.MAX_FEATURES, for_ref=True)
# save only 2d image coordinates, scrap scale, orientation etc
ref_kp_2d = np.array([p.pt for p in ref_kp], dtype='float32')
# get 3d coordinates
ref_kp_3d = KeypointAlgo.inverse_project(self.system_model, ref_kp_2d, depth, self.render_z, self._ref_img_sc)
if False:
mm_dist = self.system_model.min_med_distance
if False:
pos = (0, 0, -mm_dist)
qfin = tools.ypr_to_q(sc_ast_lat, 0, sc_ast_lon)
light_v = tools.spherical2cartesian(light_lat, light_lon, 1)
reimg = self.render_engine.render(self.obj_idx, pos, qfin, light_v)
reimg = cv2.cvtColor(reimg, cv2.COLOR_RGB2GRAY)
img = np.concatenate((cv2.resize(ref_img, (self.system_model.view_width, self.system_model.view_height)), reimg), axis=1)
else:
ref_kp = [cv2.KeyPoint(*self._cam.calc_img_xy(x, -y, -z-mm_dist), 1) for x, y, z in ref_kp_3d]
img = cv2.drawKeypoints(ref_img, ref_kp, ref_img.copy(), (0, 0, 255), flags=cv2.DRAW_MATCHES_FLAGS_DEFAULT)
cv2.imshow('res', img)
cv2.waitKey()
return np.array(ref_desc), ref_kp_2d, ref_kp_3d
def rotation_q(self, timestamp):
theta = self.rotation_theta(timestamp)
# TODO: use precession info
# orient z axis correctly, rotate around it
return tools.ypr_to_q(self.axis_latitude, self.axis_longitude, theta) \
* self.ast2sc_q
def _render_params(self, discretize_tol=False, center_model=False):
# called at self.render, override based on hidden field values
#qfin = tools.fdb_relrot_to_render_q(self._sc_ast_lat, self._sc_ast_lon)
qfin = tools.ypr_to_q(self._sc_ast_lat, 0, self._sc_ast_lon)
#light_v = tools.fdb_light_to_render_light(self._light_lat, self._light_lon)
light_v = tools.spherical2cartesian(self._light_lat, self._light_lon, 1)
# if qfin & light_v not reasonable, e.g. because solar elong < 45 deg:
# seems that not needed, left here in case later notice that useful
# raise InvalidSceneException()
return (0, 0, self.render_z), qfin, light_v
def calc_err(self, rvec, tvec, i1, j1, warn=False):
q_res = tools.angleaxis_to_q(rvec)
lat1, roll1 = self._fdb_sc_ast_perms[i1]
lat2, roll2 = self._fdb_sc_ast_perms[j1]
q_src = tools.ypr_to_q(lat1, 0, roll1)
q_trg = tools.ypr_to_q(lat2, 0, roll2)
q_rel = q_trg * q_src.conj()
# q_res.x = -q_res.x
# np.quaternion(0.707106781186547, 0, -0.707106781186547, 0)
m = self.system_model
q_frame = m.frm_conv_q(m.OPENGL_FRAME, m.OPENCV_FRAME)
q_res = q_frame * q_res.conj() * q_frame.conj()
err1 = math.degrees(tools.wrap_rads(tools.angle_between_q(q_res, q_rel)))
err2 = np.linalg.norm(tvec - np.array((0, 0, -self.render_z)).reshape((3, 1)))
ok = not (abs(err1) > 10 or abs(err2) > 0.10 * abs(self.render_z))
if not ok and warn:
print('at (%s, %s), err1: %.1fdeg, err2: %.1fkm\n\tq_real: %s\n\tq_est: %s' % (
i1, j1, err1, err2, q_rel, q_res))
return ok, err1, err2
def _set_sc_from_ast_rot_and_trans(self,
rvec,
tvec,
discretization_err_q,
rotate_sc=False):
sm = self.system_model
# rotate to gl frame from opencv camera frame
gl2cv_q = sm.frm_conv_q(sm.OPENGL_FRAME, sm.OPENCV_FRAME)
new_sc_pos = tools.q_times_v(gl2cv_q, tvec)
# camera rotation in opencv frame
cv_cam_delta_q = tools.angleaxis_to_q(rvec)
# solvePnPRansac has some bug that apparently randomly gives 180deg wrong answer
if new_sc_pos[2] > 0:
tpos = -new_sc_pos
tdelta_q = cv_cam_delta_q * tools.ypr_to_q(0, math.pi, 0)
# print('Bug with solvePnPRansac, correcting:\n\t%s => %s\n\t%s => %s'%(
# new_sc_pos, tpos, tools.q_to_ypr(cv_cam_delta_q), tools.q_to_ypr(tdelta_q)))
new_sc_pos = tpos
cv_cam_delta_q = tdelta_q
sm.spacecraft_pos = new_sc_pos
err_q = discretization_err_q or np.quaternion(1, 0, 0, 0)
if rotate_sc:
sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME)
sc_delta_q = err_q * sc2cv_q * cv_cam_delta_q.conj(
) * sc2cv_q.conj()
sm.rotate_spacecraft(sc_delta_q)
else:
sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME)
sc_q = sm.spacecraft_q
frame_q = sc_q * err_q * sc2cv_q
ast_delta_q = frame_q * cv_cam_delta_q * frame_q.conj()
err_corr_q = sc_q * err_q.conj() * sc_q.conj()
ast_q0 = sm.asteroid_q
sm.rotate_asteroid(err_corr_q * ast_delta_q)
if self.est_real_ast_orient:
# so that can track rotation of 67P
ast_q = sm.asteroid_q
err_deg = math.degrees(tools.angle_between_q(ast_q0, ast_q))
self.extra_values = list(quaternion.as_float_array(ast_q)) + [
sm.time.value, err_deg
]
def costfun(x, debug=0, verbose=True):
set_params(x)
err = 0
for phase_angle in np.radians(np.linspace(0, 150, img_n)):
light = tools.q_times_v(tools.ypr_to_q(phase_angle, 0, 0), np.array([0, 0, -1]))
synth1 = re.render(obj_idx, pos, np.quaternion(1,0,0,0), tools.normalize_v(light), get_depth=False, reflection=m_hapke)
synth2 = re.render(obj_idx, pos, np.quaternion(1,0,0,0), tools.normalize_v(light), get_depth=False, reflection=m_ll)
synth1 = cv2.cvtColor(synth1, cv2.COLOR_RGB2GRAY)
synth2 = cv2.cvtColor(synth2, cv2.COLOR_RGB2GRAY)
err_img = (synth1.astype('float') - synth2.astype('float'))**2
err += np.mean(err_img)
if debug:
if debug%2:
cv2.imshow('hapke vs ll', np.concatenate((synth1.astype('uint8'), 255*np.ones((synth2.shape[0], 1), dtype='uint8'), synth2), axis=1))
if debug>1:
err_img = err_img**0.2
cv2.imshow('err', err_img/np.max(err_img))
cv2.waitKey()
err /= img_n
if verbose:
print('%s => %f' % (', '.join(['%.4e' % i for i in np.array(x)*scales]), err))
return err
# break
#synth[0] = ImageProc.equalize_brightness(synth[0], real, percentile=99.999, image_gamma=1.8)
#synth[0] = ImageProc.adjust_gamma(synth[0], 1/4)
#real = ImageProc.adjust_gamma(real, 1/4)
real = cv2.resize(real, size)
cv2.imshow(
'real vs synthetic',
np.concatenate(
[real, 255 * np.ones(
(real.shape[0], 1), dtype='uint8')] + synth,
axis=1))
cv2.waitKey()
else:
# try different light directions a fixed angle (d) away from default
d = 10
for a in np.linspace(0, 360 * 2, 36 * 2, endpoint=False):
lat, lon, r = tools.cartesian2spherical(*sm.asteroid.real_position)
qa = tools.ypr_to_q(lat, lon, 0)
qd = tools.ypr_to_q(0, 0, np.radians(a)) * tools.ypr_to_q(
np.radians(d), 0, 0)
q = qa * qd * qa.conj()
sm.asteroid.real_position = tools.q_times_v(
q, sm.asteroid.real_position)
img = cv2.resize(
ab.render(shadows=True, reflection=RenderEngine.REFLMOD_HAPKE),
(1024, 1024))
cv2.imshow('s', img)
cv2.waitKey()
quit()
def generate_system_state(self, sm, i):
# reset asteroid axis to true values
sm.asteroid.reset_to_defaults()
sm.asteroid_rotation_from_model()
if self._state_list is not None:
lblloader.load_image_meta(
os.path.join(self._state_db_path, self._state_list[i]+'.LBL'), sm)
return
for i in range(100):
## sample params from suitable distributions
##
# datetime dist: uniform, based on rotation period
time = np.random.uniform(*sm.time.range)
# spacecraft position relative to asteroid in ecliptic coords:
sc_lat = np.random.uniform(-math.pi/2, math.pi/2)
sc_lon = np.random.uniform(-math.pi, math.pi)
# s/c distance as inverse uniform distribution
if TestLoop.UNIFORM_DISTANCE_GENERATION:
sc_r = np.random.uniform(self.min_r, self.max_r)
else:
sc_r = 1/np.random.uniform(1/self.max_r, 1/self.min_r)
# same in cartesian coord
sc_ex_u, sc_ey_u, sc_ez_u = spherical_to_cartesian(sc_r, sc_lat, sc_lon)
sc_ex, sc_ey, sc_ez = sc_ex_u.value, sc_ey_u.value, sc_ez_u.value
# s/c to asteroid vector
sc_ast_v = -np.array([sc_ex, sc_ey, sc_ez])
# sc orientation: uniform, center of asteroid at edge of screen
if self._opzone_only:
# always get at least 50% of astroid in view, 5% of the time maximum offset angle
max_angle = rad(min(sm.cam.x_fov, sm.cam.y_fov)/2)
da = min(max_angle, np.abs(np.random.normal(0, max_angle/2)))
dd = np.random.uniform(0, 2*math.pi)
sco_lat = wrap_rads(-sc_lat + da*math.sin(dd))
sco_lon = wrap_rads(math.pi + sc_lon + da*math.cos(dd))
sco_rot = np.random.uniform(-math.pi, math.pi) # rotation around camera axis
else:
# follows the screen edges so that get more partial views, always at least 25% in view
# TODO: add/subtract some margin
sco_lat = wrap_rads(-sc_lat)
sco_lon = wrap_rads(math.pi + sc_lon)
sco_rot = np.random.uniform(-math.pi, math.pi) # rotation around camera axis
sco_q = ypr_to_q(sco_lat, sco_lon, sco_rot)
ast_ang_r = math.atan(sm.asteroid.mean_radius/1000/sc_r) # if asteroid close, allow s/c to look at limb
dx = max(rad(sm.cam.x_fov/2), ast_ang_r)
dy = max(rad(sm.cam.y_fov/2), ast_ang_r)
disturbance_q = ypr_to_q(np.random.uniform(-dy, dy), np.random.uniform(-dx, dx), 0)
sco_lat, sco_lon, sco_rot = q_to_ypr(sco_q * disturbance_q)
sco_q = ypr_to_q(sco_lat, sco_lon, sco_rot)
# sc_ast_p ecliptic => sc_ast_p open gl -z aligned view
sc_pos = q_times_v((sco_q * sm.sc2gl_q).conj(), sc_ast_v)
# get asteroid position so that know where sun is
# *actually barycenter, not sun
as_v = sm.asteroid.position(time)
elong, direc = solar_elongation(as_v, sco_q)
# limit elongation to always be more than set elong
if elong > rad(sm.min_elong):
break
if elong <= rad(sm.min_elong):
assert False, 'probable infinite loop'
# put real values to model
sm.time.value = time
sm.spacecraft_pos = sc_pos
sm.spacecraft_rot = (deg(sco_lat), deg(sco_lon), deg(sco_rot))
# save real values so that can compare later
sm.time.real_value = sm.time.value
sm.real_spacecraft_pos = sm.spacecraft_pos
sm.real_spacecraft_rot = sm.spacecraft_rot
sm.real_asteroid_axis = sm.asteroid_axis
# get real relative position of asteroid model vertices
sm.asteroid.real_sc_ast_vertices = sm.sc_asteroid_vertices()
def real_spacecraft_q(self):
return tools.ypr_to_q(*list(
map(math.radians, (self.x_rot.real_value, self.y_rot.real_value,
self.z_rot.real_value))))
if False:
for i in range(36):
image = re.render(obj_idx, [0, 0, -sm.min_med_distance*3], q**i, np.array([1, 0, 0])/math.sqrt(1), get_depth=False)
cv2.imshow('image', image)
cv2.waitKey()
elif True:
RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_HAPKE] = DidymosPrimary.HAPKE_PARAMS
RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_LUNAR_LAMBERT] = DidymosPrimary.LUNAR_LAMBERT_PARAMS
imgs = ()
i = 1
th = math.radians(100)
#for i in range(4, 7):
for th in np.linspace(math.radians(90), 0, 4):
imgs_j = ()
for j, hapke in enumerate((True, False)):
model = RenderEngine.REFLMOD_HAPKE if hapke else RenderEngine.REFLMOD_LUNAR_LAMBERT
if hapke and j == 0:
RenderEngine.REFLMOD_PARAMS[model][9] = 0
if hapke and j == 1:
RenderEngine.REFLMOD_PARAMS[model][9] = 1
light = tools.q_times_v(tools.ypr_to_q(th, 0, 0), np.array([0, 0, -1]))
image = re.render(obj_idx, pos, q**i, tools.normalize_v(light), get_depth=False, reflection=model)
image = ImageProc.adjust_gamma(image, 1.8)
imgs_j += (image,)
imgs += (np.vstack(imgs_j),)
#cv2.imshow('depth', np.clip((sm.min_med_distance+sm.asteroid.mean_radius - depth)/5, 0, 1))
cv2.imshow('images', np.hstack(imgs))
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' <(deg|km)>', '', config_data)
config = ConfigParser(converters={'tuple': literal_eval})
config.read_string(config_data)
image_time = config.get('meta', 'IMAGE_TIME')
# from sun to spacecraft, equatorial J2000
sun_sc_eq_x, sun_sc_eq_y, sun_sc_eq_z = \
-np.array(config.gettuple('meta', 'SC_SUN_POSITION_VECTOR'))
if USE_ICRS:
sun_sc_ec_p = np.array([sun_sc_eq_x, sun_sc_eq_y, sun_sc_eq_z])
else:
sc = SkyCoord(x=sun_sc_eq_x, y=sun_sc_eq_y, z=sun_sc_eq_z, unit='km',
frame='icrs', representation_type='cartesian', obstime='J2000')\
.transform_to('heliocentrictrueecliptic')\
.represent_as('cartesian')
sun_sc_ec_p = np.array([sc.x.value, sc.y.value, sc.z.value])
sun_sc_dist = np.sqrt(np.sum(sun_sc_ec_p**2))
# from spacecraft to asteroid, equatorial J2000
sc_ast_x, sc_ast_y, sc_ast_z = \
config.gettuple('meta', 'SC_TARGET_POSITION_VECTOR')
# from asteroid to spacecraft, asteroid fixed body coordinates
ast_sc_r = config.getfloat('meta', 'TARGET_CENTER_DISTANCE')
ast_sc_lat = config.getfloat('meta', 'SUB_SPACECRAFT_LATITUDE')
ast_sc_lon = config.getfloat('meta', 'SUB_SPACECRAFT_LONGITUDE')
# 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')
solar_elongation = config.getfloat('meta', 'SOLAR_ELONGATION')
## set time
##
half_range = sm.asteroid.rotation_period / 2
timestamp = Time(image_time, scale='utc', format='isot').unix
sm.time.range = (timestamp - half_range, timestamp + half_range)
sm.time.value = timestamp
sm.time.real_value = timestamp
## set spacecraft orientation
##
xc, yc, zc = 0, 0, 0
#xc, yc, zc = -0.283, -0.127, 0 # ROS_CAM1_20150720T113057
#xc, yc, zc = 0.2699, -0.09, 0 # ROS_CAM1_20150720T165249
#xc, yc, zc = 0.09, -0.02, 0 # ROS_CAM1_20150720T064939
if USE_ICRS:
assert sc_rot_dec + xc < 90 and sc_rot_dec + xc > -90, 'bad correction'
sm.spacecraft_rot = (
sc_rot_dec + xc, # axis lat
(sc_rot_ra + yc + 180) % 360 - 180, # axis lon
(360 - sc_rot_cnca + zc) % 360 - 180, # rotation
)
else:
sc = SkyCoord(ra=sc_rot_ra * units.deg,
dec=sc_rot_dec * units.deg,
frame='icrs',
obstime='J2000')
sc = sc.transform_to('barycentrictrueecliptic')
assert sc.lat.value + xc < 90 and sc.lat.value + xc > -90, 'bad correction'
sm.spacecraft_rot = (
sc.lat.value + xc, # axis lat
(sc.lon.value + yc + 180) % 360 - 180, # axis lon
(sc_rot_cnca + zc + 180) % 360 - 180, # rotation
)
sm.real_spacecraft_rot = sm.spacecraft_rot
## set spacecraft position
##
if USE_ICRS:
sc_ast_ec_p = np.array([sc_ast_x, sc_ast_y, sc_ast_z])
else:
sc = SkyCoord(x=sc_ast_x, y=sc_ast_y, z=sc_ast_z, unit='km', frame='icrs',
representation_type='cartesian', obstime='J2000')\
.transform_to('barycentrictrueecliptic')\
.represent_as('cartesian')
sc_ast_ec_p = np.array([sc.x.value, sc.y.value, sc.z.value])
sm.asteroid.real_position = sun_sc_ec_p + sc_ast_ec_p
# s/c orientation
sco = list(map(math.radians, sm.spacecraft_rot))
scoq = tools.ypr_to_q(*sco)
# project old position to new base vectors
sc2gl_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENGL_FRAME)
scub = tools.q_to_unitbase(scoq * sc2gl_q)
scub_o = tools.q_to_unitbase(scoq)
sc_ast_p = scub.dot(sc_ast_ec_p.transpose())
sm.real_spacecraft_pos = sc_ast_p
# if USE_IMG_LABEL_FOR_SC_POS:
# sm.spacecraft_pos = sc_ast_p
##
## done setting spacecraft position
# use calculated asteroid axis as real axis
sm.asteroid_rotation_from_model()
sm.real_asteroid_axis = sm.asteroid_axis
sm.asteroid.real_sc_ast_vertices = sm.sc_asteroid_vertices(real=True)
if not np.isclose(
float(Decimal(sm.time.value) - Decimal(sm.time.real_value)), 0):
sm.time.real_value = sm.time.value
if DEBUG:
print(
'Strange Python problem where float memory values get corrupted a little in random places of code'
)
if False:
print(
('' + '\nsco:\n%s\n' + '\nscoq:\n%s\n' + '\nscub_o:\n%s\n' +
'\nscub:\n%s\n' + '\nast_sc_ec_p:\n%s\n' + '\nast_sc_p:\n%s\n') %
(
sm.spacecraft_rot,
scoq,
scub,
scub_o,
sc_ast_ec_p,
sc_ast_p,
))
if DEBUG:
lbl_sun_ast_v = (sun_sc_ec_p + sc_ast_ec_p) * 1e3
lbl_se, lbl_dir = tools.solar_elongation(lbl_sun_ast_v, scoq)
m_elong, m_dir = sm.solar_elongation()
mastp = sm.asteroid.position(sm.time.value)
print(('solar elongation (deg), file: %.1f (%.1f), model: %.1f\n' +
'light direction (deg), file: %s, model: %s\n' +
'sun-asteroid loc (Gm), file: %s, model: %s\n') % (
solar_elongation,
math.degrees(lbl_se),
math.degrees(m_elong),
math.degrees(lbl_dir),
math.degrees(m_dir),
lbl_sun_ast_v * 1e-9,
(mastp) * 1e-9,
))
sm.save_state('none', printout=True)
