def light_rel_dir(self, err_q=False, discretize_tol=False):
""" direction of light relative to spacecraft in s/c coords """
assert not discretize_tol, 'discretize_tol deprecated at light_rel_dir function'
light_v = tools.normalize_v(self.asteroid.position(self.time.value))
sc_q = self.spacecraft_q
err_q = (err_q or np.quaternion(1, 0, 0, 0))
# old, better way to discretize light, based on asteroid rotation axis, now not in use
if discretize_tol:
ast_q = self.asteroid_q
light_ast_v = tools.q_times_v(ast_q.conj(), light_v)
dlv, _ = tools.discretize_v(light_ast_v, discretize_tol)
err_angle = tools.angle_between_v(light_ast_v, dlv)
light_v = tools.q_times_v(ast_q, dlv)
return tools.q_times_v(err_q.conj() * sc_q.conj(), light_v),\
err_angle if discretize_tol else False
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 gl_sc_asteroid_rel_q(self, discretize_tol=False):
""" rotation of asteroid relative to spacecraft in opengl coords """
assert not discretize_tol, 'discretize_tol deprecated at gl_sc_asteroid_rel_q function'
self.update_asteroid_model()
sc_ast_rel_q = SystemModel.sc2gl_q.conj() * self.sc_asteroid_rel_q()
if discretize_tol:
qq, _ = tools.discretize_q(sc_ast_rel_q, discretize_tol)
err_q = sc_ast_rel_q * qq.conj()
sc_ast_rel_q = qq
if not BATCH_MODE and DEBUG:
print('asteroid x-axis: %s' %
tools.q_times_v(sc_ast_rel_q, np.array([1, 0, 0])))
return sc_ast_rel_q, err_q if discretize_tol else False
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 _render_params(self, discretize_tol=False, center_model=False):
assert not discretize_tol, 'discretize_tol deprecated at _render_params function'
m = self.system_model
# NOTE: with wide angle camera, would need to take into account
# im_xoff, im_yoff, im_width and im_height
xc_off = (self.im_xoff + self.im_width / 2 - self._cam.width / 2)
xc_angle = xc_off / self._cam.width * math.radians(self._cam.x_fov)
yc_off = (self.im_yoff + self.im_height / 2 - self._cam.height / 2)
yc_angle = yc_off / self._cam.height * math.radians(self._cam.y_fov)
# first rotate around x-axis, then y-axis,
# note that diff angle in image y direction corresponds to rotation
# around x-axis and vise versa
q_crop = (np.quaternion(math.cos(-yc_angle / 2), math.sin(
-yc_angle / 2), 0, 0) * np.quaternion(math.cos(-xc_angle / 2), 0,
math.sin(-xc_angle / 2), 0))
x = m.x_off.value
y = m.y_off.value
z = m.z_off.value
# rotate offsets using q_crop
x, y, z = tools.q_times_v(q_crop.conj(), np.array([x, y, z]))
# maybe put object in center of view
if center_model:
x, y = 0, 0
# get object rotation and turn it a bit based on cropping effect
q, err_q = m.gl_sc_asteroid_rel_q(discretize_tol)
sc2gl_q = m.frm_conv_q(m.SPACECRAFT_FRAME, m.OPENGL_FRAME)
self.latest_discretization_err_q = sc2gl_q * err_q * sc2gl_q.conj(
) if discretize_tol else False
qfin = (q * q_crop.conj())
# light direction
light, err_angle = m.gl_light_rel_dir(err_q, discretize_tol)
self.latest_discretization_light_err_angle = err_angle if discretize_tol else False
return (x, y, z), qfin, light
def renderParams(self):
m = self.systemModel
# NOTE: with wide angle camera, would need to take into account
# im_xoff, im_yoff, im_width and im_height
xc_off = (self.im_xoff + self.im_width / 2 - m.cam.width / 2)
xc_angle = xc_off / m.cam.width * math.radians(m.cam.x_fov)
yc_off = (self.im_yoff + self.im_height / 2 - m.cam.height / 2)
yc_angle = yc_off / m.cam.height * math.radians(m.cam.y_fov)
# first rotate around x-axis, then y-axis,
# note that diff angle in image y direction corresponds to rotation
# around x-axis and vise versa
q_crop = (np.quaternion(math.cos(-yc_angle / 2), math.sin(
-yc_angle / 2), 0, 0) * np.quaternion(math.cos(-xc_angle / 2), 0,
math.sin(-xc_angle / 2), 0))
x = m.x_off.value
y = m.y_off.value
z = m.z_off.value
# rotate offsets using q_crop
x, y, z = tools.q_times_v(q_crop.conj(), np.array([x, y, z]))
# maybe put object in center of view
if self._center_model:
x, y = 0, 0
# get object rotation and turn it a bit based on cropping effect
q, err_q = m.gl_sc_asteroid_rel_q(self._discretize_tol)
if self._discretize_tol:
self.latest_discretization_err_q = err_q
qfin = (q * q_crop.conj())
rv = tools.q_to_angleaxis(qfin)
# light direction
light, _ = m.gl_light_rel_dir(err_q)
res = (light, (x, y, z), (math.degrees(rv[0]), ) + tuple(rv[1:]))
return res
def gl_light_rel_dir(self, err_q=False, discretize_tol=False):
""" direction of light relative to spacecraft in opengl coords """
assert not discretize_tol, 'discretize_tol deprecated at gl_light_rel_dir function'
light_v, err_angle = self.light_rel_dir(err_q=False,
discretize_tol=False)
err_q = (err_q or np.quaternion(1, 0, 0, 0))
light_gl_v = tools.q_times_v(err_q.conj() * SystemModel.sc2gl_q.conj(),
light_v)
# new way to discretize light, consistent with real fdb inplementation
if discretize_tol:
dlv, _ = tools.discretize_v(
light_gl_v,
discretize_tol,
lat_range=(-math.pi / 2, math.radians(90 - self.min_elong)))
err_angle = tools.angle_between_v(light_gl_v, dlv)
light_gl_v = dlv
return light_gl_v, err_angle
def export_state(self, filename):
""" saves state in an easy to access format """
qn = ('w', 'x', 'y', 'z')
vn = ('x', 'y', 'z')
lines = [['type'] + ['ast_q' + i
for i in qn] + ['sc_q' + i for i in qn] +
['ast_sc_v' + i for i in vn] + ['sun_ast_v' + i for i in vn]]
for t in ('initial', 'real'):
# if settings.USE_ICRS, all in solar system barycentric equatorial frame
ast_q = self.asteroid.rotation_q(self.time.value)
sc_q = self.spacecraft_q
ast_sc_v = tools.q_times_v(sc_q, self.spacecraft_pos)
sun_ast_v = self.asteroid.position(self.time.value)
lines.append((t, ) + tuple(
'%f' % f
for f in (tuple(ast_q.components) + tuple(sc_q.components) +
tuple(ast_sc_v) + tuple(sun_ast_v))))
self.swap_values_with_real_vals()
with open(filename, 'w') as f:
f.write('\n'.join(['\t'.join(l) for l in lines]))
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()
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()
