def main(outfile='render-test.jpg'):
logger = logging.getLogger(__name__)
logger.info('Setting up renderer and loading the 3d model...')
sm = RosettaSystemModel(hi_res_shape_model=False, skip_obj_load=True)
re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0)
re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.1 * sm.min_distance, 1.1 * sm.max_distance)
obj_path = os.path.join(os.path.dirname(__file__), 'data', '67p-16k.obj')
obj_idx = re.load_object(obj_path)
pos = np.array([0, 0, -sm.min_med_distance * 1])
q = tools.angleaxis_to_q((math.radians(-20), 0, 1, 0))
light_v = np.array([1, 0, -1]) / math.sqrt(2)
logger.info('Start rendering...')
with tools.Stopwatch() as time:
img = re.render(obj_idx, pos, q, light_v, gamma=1.8, get_depth=False, shadows=True, textures=True,
reflection=RenderEngine.REFLMOD_HAPKE)
logger.info('Rendered one image in %f secs' % time.elapsed)
ok = cv2.imwrite(outfile, img)
if ok:
logger.info('Result saved in file %s' % outfile)
else:
logger.warning('Failed to save image in file %s' % outfile)
def render_didymos(show=False):
sm = DidymosSystemModel(use_narrow_cam=False,
target_primary=False,
hi_res_shape_model=False)
re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0)
re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.05, 2)
obj_idx = re.load_object(sm.asteroid.real_shape_model)
pos = np.array([0, 0, -sm.min_med_distance * 1])
q = tools.angleaxis_to_q((math.radians(20), 0, 1, 0))
light_v = np.array([1, 0, -1]) / math.sqrt(2)
img = re.render(obj_idx,
pos,
q,
light_v,
gamma=1.8,
get_depth=False,
shadows=True,
textures=True,
reflection=RenderEngine.REFLMOD_HAPKE)
output(img, show)
def render_itokawa(show=False):
cam = Camera(4096,
4096,
aperture=1.5,
focal_length=120.8,
sensor_size=[12.288] * 2,
px_saturation_e=30e3)
shape_model_file = [
r'C:\projects\sispo\data\models\itokawa_16k.obj',
r'C:\projects\sispo\data\models\itokawa_f3145728.obj',
][0]
RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_HAPKE] = [
553.38, # J 0
26, # th_p 26
0.31, # w 0.31
-0.35, # b -0.35
0, # c 0
0.86, # B_SH0 0.86
0.00021, # hs 0.00021
0, # B_CB0 0
0.005, # hc 0.005
1, # K 1
]
re = RenderEngine(cam.width, cam.height,
antialias_samples=0) #, enable_extra_data=True)
re.set_frustum(cam.x_fov, cam.y_fov, 0.05, 12)
obj_idx = re.load_object(shape_model_file)
if 1:
g_sc_q = tools.angleaxis_to_q(
[1.847799, -0.929873, 0.266931, -0.253146])
#x, y, z = -13.182141, -64.694813, 116.263134
x, y, z = 93.818, -8.695, 1.263
g_ast_q = get_ast_q(x, y, z)
g_sol_ast_v = np.array([146226194732.0, -68812326932.0, -477863381.0
]) * 1e-3
g_sol_sc_v = np.array([146226188132.0, -68812322442.0, -477863711.0
]) * 1e-3
g_sc_ast_v = g_sol_ast_v - g_sol_sc_v
l_ast_sc_v = tools.q_times_v(
SystemModel.sc2gl_q.conj() * g_sc_q.conj(), g_sc_ast_v)
l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj(
) * g_ast_q * SystemModel.sc2gl_q
l_light_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(),
g_sol_ast_v / np.linalg.norm(g_sol_ast_v))
else:
l_ast_sc_v = np.array([0, 0, -7.990 * 1])
l_ast_q = tools.angleaxis_to_q((math.radians(20), 0, 1, 0))
l_light_v = np.array([1, 0, -1]) / math.sqrt(2)
sc_mode = 0
a, b, c = [0] * 3
ds, dq = 0.135, tools.ypr_to_q(math.radians(1.154), 0,
math.radians(-5.643))
# ds, dq = 0.535, quaternion.one # itokawa centered
while True:
img = re.render(obj_idx,
tools.q_times_v(dq, l_ast_sc_v * ds),
l_ast_q,
l_light_v,
flux_density=1.0,
gamma=1.0,
get_depth=False,
shadows=True,
textures=True,
reflection=RenderEngine.REFLMOD_HAPKE)
# data = re.render_extra_data(obj_idx, tools.q_times_v(dq, l_ast_sc_v*ds), l_ast_q, l_light_v)
# import matplotlib.pyplot as plt
# plt.imshow(data[:, :, 0])
k = output(img, show, maxval=0.90)
if k is None or k == 27:
break
tmp = 1 if k in (ord('a'), ord('s'), ord('q')) else -1
if k in (ord('a'), ord('d')):
if sc_mode:
dq = tools.ypr_to_q(math.radians(tmp * 0.033), 0, 0) * dq
else:
b += tmp
if k in (ord('w'), ord('s')):
if sc_mode:
dq = tools.ypr_to_q(0, 0, math.radians(tmp * 0.033)) * dq
else:
a += tmp
if k in (ord('q'), ord('e')):
if sc_mode:
ds *= 0.9**tmp
else:
c += tmp
if k == ord('i'):
y, p, r = tools.q_to_ypr(dq)
print('+c: %.3f, %.3f, %.3f' % (x + a, y + b, z + c))
print('ds, h, v: %.3f, %.3f, %.3f' %
(ds, math.degrees(y), math.degrees(r)))
g_ast_q = get_ast_q(x + a, y + b, z + c)
l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj(
) * g_ast_q * SystemModel.sc2gl_q
def render_67p(show=False):
sm = RosettaSystemModel(hi_res_shape_model=False, res_mult=1.0)
re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0)
re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.05, sm.max_distance)
obj_idx = re.load_object(sm.asteroid.real_shape_model)
RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_HAPKE] = [
553.38, # J 0
27, # th_p 27
0.034, # w 0.034
-0.08, # b -0.078577
0, # c 0
2.25, # B_SH0 2.25
math.radians(0.061), # hs math.radians(0.061)
0, # B_CB0 0
0.005, # hc 0.005
1, # K 1
]
g_sc_q = tools.angleaxis_to_q([1.892926, 0.781228, -0.540109, -0.312995])
# x, y, z = 69.54, 64.11, 162.976134
x, y, z = 146.540, 167.110, 154.976 # asteroid
g_ast_q = get_ast_q(x, y, z)
g_sol_ast_v = np.array([163613595198.0, 101637176823.0, 36457220690.0
]) * 1e-3
g_sol_sc_v = np.array([163613518304.0, 101637309778.0, 36457190373.0
]) * 1e-3
g_sc_ast_v = g_sol_ast_v - g_sol_sc_v
l_ast_sc_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(),
g_sc_ast_v)
l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj(
) * g_ast_q * SystemModel.sc2gl_q
l_light_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(),
g_sol_ast_v / np.linalg.norm(g_sol_ast_v))
l_vx_ast_q = SystemModel.sc2gl_q.conj() * quaternion.one.conj(
) * g_ast_q * SystemModel.sc2gl_q
print(str(l_vx_ast_q))
particles = load_particles(
r'C:\projects\sispo\data\models\Jets--ROS_CAM1_20150710T074301.exr',
lf_ast_q=l_vx_ast_q,
cell_size=0.066667)
a, b, c = [0] * 3
w = 10
while True:
print('%.3f, %.3f, %.3f' % (x + a, y + b, z + c))
if particles is None and 0:
img = re.render(obj_idx,
l_ast_sc_v,
l_ast_q,
l_light_v,
flux_density=1.0,
gamma=1.0,
get_depth=False,
shadows=True,
textures=False,
reflection=RenderEngine.REFLMOD_HAPKE)
else:
img = TestLoop.render_navcam_image_static(
None,
re, [obj_idx],
rel_pos_v=l_ast_sc_v,
rel_rot_q=l_ast_q,
light_v=l_light_v,
sc_q=g_sc_q,
sun_distance=np.linalg.norm(g_sol_ast_v) * 1e3,
exposure=None,
gain=None,
gamma=1.8,
auto_gain=True,
reflmod_params=RenderEngine.REFLMOD_PARAMS[
RenderEngine.REFLMOD_HAPKE],
use_shadows=True,
use_textures=False,
cam=sm.cam,
fluxes_only=True,
stars=True,
lens_effects=False,
particles=particles,
return_depth=False)
k = output(img, show, maxval=0.50, gamma=1.8)
#k = output(img, show, maxval=0.70, gamma=1.0)
if k is None or k == 27:
break
if k in (ord('a'), ord('d')):
b += (1 if k == ord('a') else -1) * w
if k in (ord('w'), ord('s')):
a += (1 if k == ord('s') else -1) * w
if k in (ord('q'), ord('e')):
c += (1 if k == ord('q') else -1) * w
if 0:
l_light_v = tools.q_times_v(
SystemModel.sc2gl_q.conj() * tools.ypr_to_q(
math.radians(a), math.radians(b), math.radians(c)) *
g_sc_q.conj(), g_sol_ast_v / np.linalg.norm(g_sol_ast_v))
elif 1:
g_ast_q = get_ast_q(x + a, y + b, z + c)
l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj(
) * g_ast_q * SystemModel.sc2gl_q
cam_q = SystemModel.cv2gl_q * n_ast_q.conj(
) * SystemModel.cv2gl_q.conj()
cam_v = tools.q_times_v(cam_q * SystemModel.cv2gl_q, -ast_v)
prior = Pose(cam_v, cam_q, np.ones((3, )) * 0.1, np.ones((3, )) * 0.01)
if False and 1 / np.linalg.norm(ast_v) > current_sc * sc_threshold:
# TODO: implement crop_model and augment_model
obj = tools.crop_model(obj, cam_v, cam_q, sm.cam.x_fov,
sm.cam.y_fov)
obj = tools.augment_model(obj, multiplier=sc_threshold)
obj_idx = re.load_object(obj)
current_sc = 1 / np.linalg.norm(ast_v)
image = re.render(obj_idx,
ast_v,
ast_q,
np.array([1, 0, -1]) / math.sqrt(2),
gamma=1.8,
get_depth=False)
# estimate pose
res, bias_sds, scale_sd = odo.process(image,
datetime.fromtimestamp(t0 + t),
prior, quaternion.one)
if res is not None:
tq = res.quat * SystemModel.cv2gl_q
est_q = tq.conj() * res.quat.conj() * tq
err_q = ast_q.conj() * est_q
err_angle = tools.angle_between_q(ast_q, est_q)
est_v = -tools.q_times_v(tq.conj(), res.loc)
err_v = est_v - ast_v
re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0)
re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.05, 2)
obj = sm.asteroid.real_shape_model
obj_idx = re.load_object(obj)
light = np.array([1, 0, -0.5])
light /= np.linalg.norm(light)
cam_ast_v0 = np.array([0, 0, -sm.min_med_distance * 0.7])
cam_ast_q0 = quaternion.one
dq = tools.angleaxis_to_q((math.radians(1), 0, 1, 0))
inputs = []
for i in range(60):
cam_ast_v = cam_ast_v0
cam_ast_q = dq**i * cam_ast_q0
image = re.render(obj_idx, cam_ast_v, cam_ast_q, light, gamma=1.8, get_depth=False)
cam_ast_cv_v = tools.q_times_v(SystemModel.cv2gl_q, cam_ast_v)
cam_ast_cv_q = SystemModel.cv2gl_q * cam_ast_q * SystemModel.cv2gl_q.conj()
inputs.append([image, cam_ast_cv_v, cam_ast_cv_q])
if 0:
for image, _, _ in inputs:
cv2.imshow('t', cv2.resize(image, None, fx=0.5, fy=0.5))
cv2.waitKey()
else:
t = TestOdometry()
t.setUp(verbose=True)
t.test_rotating_object(inputs=inputs)
else:
unittest.main()
class ApiServer:
SERVER_READY_NOTICE = 'server started, waiting for connections'
MAX_ORI_DIFF_ANGLE = 360 # in deg
(
FRAME_GLOBAL,
FRAME_LOCAL,
) = range(2)
def __init__(self,
mission,
hires=False,
addr='127.0.0.1',
port=50007,
result_rendering=True,
result_frame=FRAME_LOCAL):
self._pid = os.getpid()
self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._addr = addr
self._port = port
self._sock.bind(('', port))
self._sock.listen(1)
self._sock.settimeout(5)
self._result_rendering = result_rendering
self._result_frame = result_frame
self._hires = hires
self._mission = mission
self._sm = sm = get_system_model(mission, hires)
self._target_d2 = '2' in mission
self._use_nac = mission[-1] == 'n'
self._autolevel = True # not self._use_nac
self._current_level = False
self._level_lambda = 0.9
if not self._target_d2:
# so that D2 always contained in frustrum
sm.max_distance += 1.3
self._renderer = RenderEngine(sm.cam.width,
sm.cam.height,
antialias_samples=16 if hires else 0)
self._renderer.set_frustum(sm.cam.x_fov, sm.cam.y_fov,
sm.min_altitude * .1, sm.max_distance)
if isinstance(sm, DidymosSystemModel):
self.asteroids = [
DidymosPrimary(hi_res_shape_model=hires),
DidymosSecondary(hi_res_shape_model=hires),
]
else:
self.asteroids = [sm.asteroid]
self._obj_idxs = []
self._wireframe_obj_idxs = [
self._renderer.load_object(os.path.join(
DATA_DIR, 'ryugu+tex-%s-100.obj' % ast),
wireframe=True) for ast in ('d1', 'd2')
] if result_rendering else []
self._logpath = os.path.join(LOG_DIR, 'api-server', self._mission)
os.makedirs(self._logpath, exist_ok=True)
self._onboard_renderer = RenderEngine(sm.view_width,
sm.view_height,
antialias_samples=0)
self._target_obj_idx = self._onboard_renderer.load_object(
sm.asteroid.target_model_file,
smooth=sm.asteroid.render_smooth_faces)
self._keypoint = KeypointAlgo(sm, self._onboard_renderer,
self._target_obj_idx)
self._centroid = CentroidAlgo(sm, self._onboard_renderer,
self._target_obj_idx)
self._odometry = {}
# laser measurement range given in dlem-20 datasheet
self._laser_min_range, self._laser_max_range, self._laser_nominal_max_range = 10, 2100, 5000
self._laser_fail_prob = 0.05 # probability of measurement fail because e.g. asteroid low albedo
self._laser_false_prob = 0.01 # probability for random measurement even if no target
self._laser_err_sigma = 0.5 # 0.5m sigma1 accuracy given in dlem-20 datasheet
# laser algo params
self._laser_adj_loc_weight = 1
self._laser_meas_err_weight = 0.3
self._laser_max_adj_dist = 100 # in meters
def _maybe_load_objects(self):
if len(self._obj_idxs) == 0:
for ast in self.asteroids:
file = ast.hires_target_model_file if self._hires else ast.target_model_file
cache_file = os.path.join(
CACHE_DIR,
os.path.basename(file).split('.')[0] + '.pickle')
self._obj_idxs.append(
self._renderer.load_object(file,
smooth=ast.render_smooth_faces,
cache_file=cache_file))
self._obj_idxs.append(
self._renderer.load_object(self._sm.sc_model_file,
smooth=False))
@staticmethod
def _parse_poses(params, offset):
d1_v = np.array(params[offset][:3]) * 0.001
d1_q = np.quaternion(*(params[offset][3:7])).normalized()
d2_v = np.array(params[offset + 1][:3]) * 0.001
d2_q = np.quaternion(*(params[offset + 1][3:7])).normalized()
sc_v = np.array(params[offset + 2][:3]) * 0.001
sc_q = np.quaternion(*(params[offset + 2][3:7])).normalized()
return d1_v, d1_q, d2_v, d2_q, sc_v, sc_q
def _laser_meas(self, params):
time = params[0]
d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params,
offset=1)
d1, d2 = self.asteroids
q = SystemModel.sc2gl_q.conj() * sc_q.conj()
rel_rot_q = np.array(
[q * d1_q * d1.ast2sc_q.conj(), q * d2_q * d2.ast2sc_q.conj()])
rel_pos_v = np.array(
[tools.q_times_v(q, d1_v - sc_v),
tools.q_times_v(q, d2_v - sc_v)])
self._maybe_load_objects()
dist = self._renderer.ray_intersect_dist(self._obj_idxs[0:2],
rel_pos_v, rel_rot_q)
if dist is None:
if np.random.uniform(0, 1) < self._laser_false_prob:
noisy_dist = np.random.uniform(self._laser_min_range,
self._laser_nominal_max_range)
else:
noisy_dist = None
else:
if np.random.uniform(0, 1) < self._laser_fail_prob:
noisy_dist = None
else:
noisy_dist = dist * 1000 + np.random.normal(
0, self._laser_err_sigma)
if noisy_dist < self._laser_min_range or noisy_dist > self._laser_max_range:
noisy_dist = None
return json.dumps(noisy_dist)
def _laser_algo(self, params):
time = params[0]
dist_meas = params[1]
if not dist_meas or dist_meas < self._laser_min_range or dist_meas > self._laser_max_range:
raise PositioningException(
'invalid laser distance measurement: %s' % dist_meas)
d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params,
offset=2)
q = SystemModel.sc2gl_q.conj() * sc_q.conj()
d1, d2 = self.asteroids
ast = d2 if self._target_d2 else d1
ast_v = d2_v if self._target_d2 else d1_v
ast_q = d2_q if self._target_d2 else d1_q
rel_rot_q = q * ast_q * ast.ast2sc_q.conj()
rel_pos_v = tools.q_times_v(q, ast_v - sc_v) * 1000
max_r = ast.max_radius
max_diam = 2 * max_r / 1000
# set orthographic projection
self._onboard_renderer.set_orth_frustum(max_diam, max_diam,
-max_diam / 2, max_diam / 2)
# render orthographic depth image
_, zz = self._onboard_renderer.render(self._target_obj_idx, [0, 0, 0],
rel_rot_q, [1, 0, 0],
get_depth=True,
shadows=False,
textures=False)
# restore regular perspective projection
self._onboard_renderer.set_frustum(self._sm.cam.x_fov,
self._sm.cam.y_fov,
self._sm.min_altitude * .1,
self._sm.max_distance)
zz[zz > max_diam / 2 * 0.999] = float('nan')
zz = zz * 1000 - rel_pos_v[2]
xx, yy = np.meshgrid(
np.linspace(-max_r, max_r, self._sm.view_width) - rel_pos_v[0],
np.linspace(-max_r, max_r, self._sm.view_height) - rel_pos_v[1])
x_expected = np.clip(
(rel_pos_v[0] + max_r) / max_r / 2 * self._sm.view_width + 0.5, 0,
self._sm.view_width - 1.001)
y_expected = np.clip(
(rel_pos_v[1] + max_r) / max_r / 2 * self._sm.view_height + 0.5, 0,
self._sm.view_height - 1.001)
dist_expected = tools.interp2(zz,
x_expected,
y_expected,
discard_bg=True)
# mse cost function balances between adjusted location and measurement error
adj_dist_sqr = (zz - dist_meas)**2 + xx**2 + yy**2
cost = self._laser_adj_loc_weight * adj_dist_sqr \
+ (self._laser_meas_err_weight - self._laser_adj_loc_weight) * (zz - dist_meas)**2
j, i = np.unravel_index(np.nanargmin(cost), cost.shape)
if np.isnan(zz[j, i]):
raise PositioningException(
'laser algo results in off asteroid pointing')
if math.sqrt(adj_dist_sqr[j, i]) >= self._laser_max_adj_dist:
raise PositioningException(
'laser algo solution too far (%.0fm, limit=%.0fm), spurious measurement assumed'
% (math.sqrt(adj_dist_sqr[j, i]), self._laser_max_adj_dist))
dx, dy, dz = xx[0, i], yy[j, 0], zz[j, i] - dist_meas
if self._result_frame == ApiServer.FRAME_GLOBAL:
# return global ast-sc vector
est_sc_ast_v = ast_v * 1000 - tools.q_times_v(
q.conj(), rel_pos_v + np.array([dx, dy, dz]))
else:
# return local sc-ast vector
est_sc_ast_v = tools.q_times_v(SystemModel.sc2gl_q,
rel_pos_v + np.array([dx, dy, dz]))
dist_expected = float(
dist_expected) if not np.isnan(dist_expected) else -1.0
return json.dumps([list(est_sc_ast_v), dist_expected])
def _render(self, params):
time = params[0]
sun_distance = np.linalg.norm(np.array(params[1][:3])) # in meters
sun_ast_v = tools.normalize_v(np.array(params[1][:3]))
d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params,
offset=2)
d1, d2 = self.asteroids
q = SystemModel.sc2gl_q.conj() * sc_q.conj()
rel_rot_q = np.array([
q * d1_q * d1.ast2sc_q.conj(), q * d2_q * d2.ast2sc_q.conj(),
np.quaternion(1, 0, 1, 0).normalized()
]) # last one is the for the spacecraft
rel_pos_v = np.array([
tools.q_times_v(q, d1_v - sc_v),
tools.q_times_v(q, d2_v - sc_v), [0, 0, 0]
])
light_v = tools.q_times_v(q, sun_ast_v)
self._maybe_load_objects() # lazy load objects
exp_range = (0.001, 3.5)
for i in range(20):
if self._autolevel and self._current_level:
level = self._current_level
else:
level = 3 * 2.5 * 1.3e-3 if self._use_nac else 1.8 * 2.5 * 1.3e-3
exp, gain = self._sm.cam.level_to_exp_gain(level, exp_range)
img = TestLoop.render_navcam_image_static(self._sm,
self._renderer,
self._obj_idxs,
rel_pos_v,
rel_rot_q,
light_v,
sc_q,
sun_distance,
exposure=exp,
gain=gain,
auto_gain=False,
gamma=1.0,
use_shadows=True,
use_textures=True)
if self._autolevel:
v = np.percentile(img, 100 - 0.0003)
level_trg = level * 170 / v
print(
'autolevel (max_v=%.1f, e=%.3f, g=%.1f) current: %.3f, target: %.3f'
% (v, exp, gain, level, level_trg))
self._current_level = level_trg if not self._current_level else \
(self._current_level*self._level_lambda + level_trg*(1-self._level_lambda))
if v < 85 or (v == 255 and level > exp_range[0]):
level = level_trg if v < 85 else level * 70 / v
self._current_level = level
continue
break
if False:
img = ImageProc.default_preprocess(img)
date = datetime.fromtimestamp(time, pytz.utc) # datetime.now()
fname = os.path.join(self._logpath,
date.isoformat()[:-6].replace(':', '')) + '.png'
cv2.imwrite(fname, img, [cv2.IMWRITE_PNG_COMPRESSION, 0])
return fname
def _get_pose(self, params, algo_id=1):
# load target navcam image
fname = params[0]
img = cv2.imread(fname, cv2.IMREAD_GRAYSCALE)
# asteroid position relative to the sun
self._sm.asteroid.real_position = np.array(params[1][:3])
d1_v, d1_q, d2_v, d2_q, sc_v, init_sc_q = self._parse_poses(params,
offset=2)
# set asteroid orientation
d1, d2 = self.asteroids
ast = d2 if self._target_d2 else d1
init_ast_q = d2_q if self._target_d2 else d1_q
self._sm.asteroid_q = init_ast_q * ast.ast2sc_q.conj()
# set spacecraft orientation
self._sm.spacecraft_q = init_sc_q
# initial rel q
init_rel_q = init_sc_q.conj() * init_ast_q
# sc-asteroid relative location
ast_v = d2_v if self._target_d2 else d1_v # relative to barycenter
init_sc_pos = tools.q_times_v(
SystemModel.sc2gl_q.conj() * init_sc_q.conj(), ast_v - sc_v)
self._sm.spacecraft_pos = init_sc_pos
# run keypoint algo
err = None
rot_ok = True
try:
if algo_id == 1:
self._keypoint.solve_pnp(
img,
fname[:-4],
KeypointAlgo.AKAZE,
verbose=1 if self._result_rendering else 0)
elif algo_id == 2:
self._centroid.adjust_iteratively(img, fname[:-4])
rot_ok = False
else:
assert False, 'invalid algo_id=%s' % algo_id
except PositioningException as e:
err = e
rel_gf_q = np.quaternion(*([np.nan] * 4))
if err is None:
sc_q = self._sm.spacecraft_q
# resulting sc-ast relative orientation
rel_lf_q = self._sm.asteroid_q * ast.ast2sc_q if rot_ok else np.quaternion(
*([np.nan] * 4))
rel_gf_q = sc_q.conj() * rel_lf_q
# sc-ast vector in meters
rel_lf_v = tools.q_times_v(
SystemModel.sc2gl_q,
np.array(self._sm.spacecraft_pos) * 1000)
rel_gf_v = tools.q_times_v(sc_q, rel_lf_v)
# collect to one result list
if self._result_frame == ApiServer.FRAME_GLOBAL:
result = [
list(rel_gf_v),
list(quaternion.as_float_array(rel_gf_q))
]
else:
result = [
list(rel_lf_v),
list(quaternion.as_float_array(rel_lf_q))
]
diff_angle = math.degrees(
tools.angle_between_q(init_rel_q, rel_gf_q))
if diff_angle > ApiServer.MAX_ORI_DIFF_ANGLE:
err = PositioningException(
'Result orientation too different than initial one, diff %.1f°, max: %.1f°'
% (diff_angle, ApiServer.MAX_ORI_DIFF_ANGLE))
# render a result image
if self._result_rendering:
self._render_result([fname] + [
list(np.array(self._sm.spacecraft_pos) * 1000) +
list(quaternion.as_float_array(rel_gf_q))
] + [
list(np.array(init_sc_pos) * 1000) +
list(quaternion.as_float_array(init_rel_q))
])
if err is not None:
raise err
# send back in json format
return json.dumps(result)
def _odometry_track(self, params):
VO_EST_CAM_POSE = False
session = params[0]
fname = params[1]
img = cv2.imread(fname, cv2.IMREAD_GRAYSCALE)
orig_time = params[2]
time = datetime.fromtimestamp(orig_time, pytz.utc)
sun_ast_v = tools.normalize_v(np.array(params[3][:3]))
d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params,
offset=4)
ast_q = d2_q if self._target_d2 else d1_q
ast_v = d2_v if self._target_d2 else d1_v
cv2sc_q = SystemModel.cv2gl_q * SystemModel.sc2gl_q.conj()
sc_ast_cvf_q = cv2sc_q * sc_q.conj() * ast_q * cv2sc_q.conj()
sc_ast_cvf_v = tools.q_times_v(cv2sc_q * sc_q.conj(),
ast_v - sc_v) * 1000
if VO_EST_CAM_POSE:
# still from global to old ast-sc frame
sc_ast_cvf_q = cv2sc_q * ast_q.conj() * sc_q * cv2sc_q.conj()
sc_ast_cvf_v = tools.q_times_v(cv2sc_q * ast_q.conj(),
sc_v - ast_v) * 1000
# sc_ast_cvf_v = tools.q_times_v(sc_ast_cvf_q * cv2sc_q, sc_v - ast_v)
# TODO: modify so that uncertainties are given (or not)
prior = Pose(sc_ast_cvf_v, sc_ast_cvf_q,
np.ones((3, )) * 0.1,
np.ones((3, )) * 0.01)
# tracemalloc.start()
# current0, peak0 = tracemalloc.get_traced_memory()
# print("before: %.0fMB" % (current0/1024/1024))
if session not in self._odometry:
self._odometry[session] = VisualOdometry(
self._sm.cam,
self._sm.view_width * 2,
verbose=1,
use_scale_correction=False,
est_cam_pose=VO_EST_CAM_POSE)
post, bias_sds, scale_sd = self._odometry[session].process(
img, time, prior, sc_q)
# current, peak = tracemalloc.get_traced_memory()
# print("transient: %.0fMB" % ((peak - current)/1024/1024))
# tracemalloc.stop()
if post is not None:
est_sc_ast_scf_q = cv2sc_q.conj() * post.quat * cv2sc_q
est_sc_ast_scf_v = tools.q_times_v(cv2sc_q.conj(), post.loc)
if VO_EST_CAM_POSE:
# still from old ast-sc frame to local sc-ast frame
est_sc_ast_scf_q = cv2sc_q.conj() * post.quat.conj() * cv2sc_q
# NOTE: we use prior orientation instead of posterior one as the error there grows over time
est_sc_ast_scf_v = -tools.q_times_v(
cv2sc_q.conj() * prior.quat.conj(), post.loc)
if self._result_frame == ApiServer.FRAME_GLOBAL:
est_sc_ast_scf_q = sc_q * est_sc_ast_scf_q
est_sc_ast_scf_v = tools.q_times_v(sc_q, est_sc_ast_scf_v)
# TODO: (1) return in sc local frame
# - distance err sd
# - lateral err sd
# - orientation err sd (pitch & yaw)
# - orientation err sd (roll)
# - distance bias drift sd
# - lateral bias drift sd
# - orientation bias drift sd (pitch & yaw)
# - orientation bias drift sd (roll)
# - scale drift sd
dist = np.linalg.norm(sc_ast_cvf_v)
bias_sds = bias_sds * dist
est_sc_ast_lf_v_s2 = post.loc_s2 * dist
est_sc_ast_lf_so3_s2 = post.so3_s2
result = [
list(est_sc_ast_scf_v),
list(quaternion.as_float_array(est_sc_ast_scf_q)),
list(est_sc_ast_lf_v_s2) + list(est_sc_ast_lf_so3_s2) +
list(bias_sds) + [scale_sd],
orig_time,
]
else:
raise PositioningException('No tracking result')
# send back in json format
return json.dumps(result)
def _render_result(self, params):
fname = params[0]
img = cv2.imread(fname, cv2.IMREAD_COLOR)
if np.all(np.logical_not(np.isnan(params[1]))):
rel_pos_v = np.array(params[1][:3]) * 0.001
rel_rot_q = np.quaternion(*(params[1][3:7]))
color = np.array((0, 1, 0)) * 0.6
else:
rel_pos_v = np.array(params[2][:3]) * 0.001
rel_rot_q = np.quaternion(*(params[2][3:7]))
color = np.array((0, 0, 1)) * 0.6
# ast_v = np.array(params[1][:3])
# ast_q = np.quaternion(*(params[1][3:7]))
# sc_v = np.array(params[2][:3])
# sc_q = np.quaternion(*(params[2][3:7]))
#
ast_idx = 1 if self._target_d2 else 0
ast = self.asteroids[ast_idx]
# q = SystemModel.sc2gl_q.conj() * sc_q.conj()
# rel_rot_q = q * ast_q * ast.ast2sc_q.conj()
# rel_pos_v = tools.q_times_v(q, ast_v - sc_v) * 0.001
rel_rot_q = SystemModel.sc2gl_q.conj() * rel_rot_q * ast.ast2sc_q.conj(
)
#rel_pos_v = tools.q_times_v(SystemModel.sc2gl_q.conj(), rel_pos_v) * 0.001
overlay = self._renderer.render_wireframe(
self._wireframe_obj_idxs[ast_idx], rel_pos_v, rel_rot_q, color)
overlay = cv2.resize(overlay, (img.shape[1], img.shape[0]))
blend_coef = 0.6
alpha = np.zeros(list(img.shape[:2]) + [1])
alpha[np.any(overlay > 0, axis=2)] = blend_coef
result = (overlay * alpha + img * (1 - alpha)).astype('uint8')
fout = fname[:-4] + '-res.png'
cv2.imwrite(fout, result, [cv2.IMWRITE_PNG_COMPRESSION, 9])
return fout
def _handle(self, call):
if len(call) == 0:
return None
if call == 'quit':
raise QuitException()
elif call == 'ping':
return 'pong'
error = False
rval = False
idx = call.find(' ')
mission, command = (call[:idx] if idx >= 0 else call).split('|')
if mission != self._mission:
assert False, 'wrong mission for this server instance, expected %s but got %s, command was %s' % (
self._mission, mission, command)
params = []
try:
params = json.loads(call[idx + 1:]) if idx >= 0 else []
except JSONDecodeError as e:
error = 'Invalid parameters: ' + str(e) + ' "' + call[idx +
1:] + '"'
if command == 'quit':
raise QuitException()
last_exception = None
if not error:
try:
get_pose = re.match(r'^get_pose(\d+)$', command)
if get_pose:
try:
rval = self._get_pose(params, algo_id=int(get_pose[1]))
except PositioningException as e:
error = 'algo failed: ' + str(e)
elif command == 'odometry':
try:
rval = self._odometry_track(params)
except PositioningException as e:
error = str(e)
elif command == 'render':
rval = self._render(params)
elif command == 'laser_meas':
# return a laser measurement
rval = self._laser_meas(params)
elif command == 'laser_algo':
# return new sc-target vector based on laser measurement
try:
rval = self._laser_algo(params)
except PositioningException as e:
error = 'algo failed: ' + str(e)
else:
error = 'invalid command: ' + command
except (ValueError, TypeError) as e:
error = 'invalid args: ' + str(e)
self.print(
str(e) +
''.join(traceback.format_exception(*sys.exc_info())))
except Exception as e:
last_exception = ''.join(
traceback.format_exception(*sys.exc_info()))
self.print('Exception: %s' % last_exception)
if last_exception is not None:
error = 'Exception encountered: %s' % last_exception
out = ' '.join((('0' if error else '1'), ) +
((error, ) if error else (rval, ) if rval else tuple()))
return out
def listen(self):
# main loop here
self.print('%s on port %d' % (self.SERVER_READY_NOTICE, self._port))
while True:
# outer loop accepting connections (max 1)
try:
conn, addr = self._sock.accept()
try:
with conn:
while True:
# inner loop accepting multiple requests on same connection
req = self._receive(conn).strip(' \n\r\t')
if req != '':
for call in req.split('\n'):
# in case multiple calls in one request
out = self._handle(call.strip(' \n\r\t'))
if out is not None:
out = out.strip(' \n\r\t') + '\n'
conn.sendall(out.encode('utf-8'))
except ConnectionAbortedError:
self.print('client closed the connection')
except socket.timeout:
pass
def print(self, msg, start=False, finish=False):
prefix = '%s [%d]' % (datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
self._pid)
if start:
print(' '.join((prefix, msg)), end='', flush=True)
elif finish:
print(msg)
else:
print(' '.join((prefix, msg)))
def _receive(self, conn):
chunks = []
buffer_size = 1024
while True:
try:
chunk = conn.recv(buffer_size)
chunks.append(chunk)
if chunk == b'':
if len(chunks) > 1:
break
else:
raise ConnectionAbortedError()
elif len(chunk) < buffer_size:
break
except socket.timeout:
pass
return (b''.join(chunks)).decode('utf-8')
def close(self):
self._sock.close()