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 initialize_frame(self, time, image, measure):
nf = super(VisualGPSNav, self).initialize_frame(time, image, measure)
if nf.measure:
old_kf = [kf for kf in self.state.keyframes if kf.measure]
nf.measure.time_adj = old_kf[-1].measure.time_adj if len(old_kf) > 0 else 0
geodetic_origin = self.geodetic_origin
lat, lon, alt = nf.measure.data[0:3]
roll, pitch, yaw = nf.measure.data[3:6]
b2c_roll, b2c_pitch, b2c_yaw = nf.measure.data[6:9]
# world frame: +z up, +x is east, +y is north
# body frame: +z down, +x is fw towards north, +y is right wing (east)
# camera frame: +z into the image plane, -y is up, +x is right
# wf2bf_q = tools.eul_to_q((np.pi, -np.pi / 2), 'xz')
# bf2cf = Pose(None, tools.eul_to_q((np.pi / 2, np.pi / 2), 'yz'))
if 1:
w2b_bf_q = tools.ypr_to_q(yaw, pitch, roll)
b2c_bf_q = tools.ypr_to_q(b2c_yaw, b2c_pitch, b2c_roll)
yaw, pitch, roll = tools.q_to_ypr(w2b_bf_q * b2c_bf_q * (self.ori_off_q or quaternion.one))
cf_w2c = tools.lla_ypr_to_loc_quat(geodetic_origin, [lat, lon, alt], [yaw, pitch, roll], b2c=self.bf2cf)
if len(old_kf) > 0:
cf_w2c.vel = (cf_w2c.loc - (-old_kf[-1].pose.prior).loc) / (nf.time - old_kf[-1].time).total_seconds()
else:
cf_w2c.vel = np.array([0, 0, 0])
nf.pose.prior = -cf_w2c
else:
wf_body_r = tools.to_cartesian(lat, lon, alt, *geodetic_origin)
bf_world_body_q = tools.ypr_to_q(yaw, pitch, roll)
if 0:
cf_world_q = bf_cam_q.conj() * bf_world_body_q.conj()
cf_body_r = tools.q_times_v(bf_cam_q.conj(), -bf_cam_r)
cf_body_world_r = tools.q_times_v(cf_world_q * self.wf2bf_q.conj(), -wf_body_r)
nf.pose.prior = Pose(cf_body_r + cf_body_world_r, cf_world_q * bf_cam_q)
else:
bf_world_q = bf_world_body_q.conj() * self.wf2bf_q.conj()
cf_world_q = bf_cam_q.conj() * bf_world_q
cf_body_r = tools.q_times_v(bf_cam_q.conj(), -bf_cam_r)
cf_body_world_r = tools.q_times_v(cf_world_q, -wf_body_r)
# bf_cam_q.conj() * bf_world_body_q.conj() * self.wf2bf_q.conj()
nf.pose.prior = Pose(cf_body_r + cf_body_world_r, bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q)
# bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q
# (NokiaSensor.w2b_q * NokiaSensor.b2c_q) * prior.quat.conj()
# self.wf2bf_q * bf_cam_q * (bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q).conj()
# => self.wf2bf_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q * bf_cam_q //*//
# self.wf2bf_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q.conj() * self.wf2bf_q.conj()
return nf
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 main():
cam_mx = np.array([[1580.356552, 0.000000, 994.026697],
[0.000000, 1580.553177, 518.938726],
[0.000000, 0.000000, 1.000000]])
sc_v0, sc_v1 = np.array([0, 0, -100]), np.array([0, -5, -100])
sc_q0, sc_q1 = tools.ypr_to_q(0, 0,
0), tools.ypr_to_q(0, 0, math.radians(-2))
w_q0, w_q1 = sc_q0.conj(), sc_q1.conj()
w_v0, w_v1 = tools.q_times_v(w_q0, -sc_v0), tools.q_times_v(w_q1, -sc_v1)
f0 = Frame(0, None, 1.0, PoseEstimate(None, Pose(w_v0, w_q0), 0))
f1 = Frame(1, None, 1.0, PoseEstimate(None, Pose(w_v1, w_q1), 0))
ref_pts4d = np.concatenate(
(np.random.uniform(-30, 30, (100, 2)), np.zeros(
(100, 1)), np.ones((100, 1))),
axis=1)
P0, P1 = cam_mx.dot(f0.to_mx()), cam_mx.dot(f1.to_mx())
uvs0, uvs1 = P0.dot(ref_pts4d.T).T, P1.dot(ref_pts4d.T).T
uvs0, uvs1 = uvs0[:, :2] / uvs0[:, 2:], uvs1[:, :2] / uvs1[:, 2:]
plt.figure(1)
for uv0, uv1 in zip(uvs0, uvs1):
plt.plot([uv0[0], uv1[0]], [uv0[1], uv1[1]])
plt.plot(uvs1[:, 0], uvs1[:, 1], '.')
plt.gca().invert_yaxis()
pts3d = []
for uv0, uv1 in zip(uvs0, uvs1):
kp4d = cv2.triangulatePoints(P0, P1, uv0.reshape((-1, 1, 2)),
uv1.reshape((-1, 1, 2)))
pt3d = (kp4d.T[:, :3] / kp4d.T[:, 3:])[0]
pts3d.append(pt3d)
pts3d = np.array(pts3d)
fig = plt.figure(2)
ax = fig.add_subplot(111, projection='3d')
ax.plot(pts3d[:, 0], pts3d[:, 1], pts3d[:, 2], '.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
print('done')
def flux_density_voxels(self,
lf_ast_v,
lf_ast_q,
mask,
solar_flux,
down_scaling=1,
quad_lim=15):
# TODO: implement this someday in OpenGL, e.g. like this: http://www.alexandre-pestana.com/volumetric-lights
assert down_scaling >= 1, 'only values of >=1 make sense for down_scaling'
dq = lf_ast_q * self.voxels.lf_ast_q.conj()
dv = tools.q_times_v(dq.conj(), lf_ast_v - self.voxels.lf_ast_v)
ny, nx, nz = self.voxels.voxel_data.shape
dx, dy, dz = [self.voxels.cell_size] * 3
gx = np.linspace(-(nx - 1) * dx / 2, (nx - 1) * dx / 2, nx)
gy = np.linspace(-(ny - 1) * dy / 2, (ny - 1) * dy / 2, ny)
gz = np.linspace(-(nz - 1) * dz / 2, (nz - 1) * dz / 2, nz)
interp3d = RegularGridInterpolator((gx, gy, gz),
self.voxels.voxel_data,
bounds_error=False,
fill_value=0.0)
#interp3d = NearestNDInterpolator((gx, gy, gz), self.voxels.voxel_data)
ray_axes, sc_shape = self._px_ray_axes(
1 / down_scaling, dq) # tools.ypr_to_q(0, np.pi, 0) * dq)
margin = 0.0
dist = np.linalg.norm(dv)
fg_near = dist - nz * dz / 2
fg_far = dist - margin / 2
bg_near = dist + margin / 2
bg_far = dist + nz * dz / 2
def quad_fn(interp3d, ray_axes, near, far):
points = None #np.linspace(near, far, nx/2)
res = integrate.quad_vec(lambda r: interp3d(ray_axes * r - dv),
near,
far,
points=points,
limit=quad_lim)
return res[0]
# integrate density along the rays (quad_vec requires at least scipy 1.4.x)
res = quad_fn(interp3d, ray_axes, bg_near, bg_far)
bg_res = cv2.resize(
res.reshape(sc_shape).astype(np.float32), mask.shape)
bg_res[mask] = 0
# integrate density along the rays (quad_vec requires at least scipy 1.4.x)
res = quad_fn(interp3d, ray_axes, fg_near, fg_far)
fg_res = cv2.resize(
res.reshape(sc_shape).astype(np.float32), mask.shape)
result = ((0 if bg_res is None else bg_res) + (0 if fg_res is None else fg_res)) \
* Particles.CONE_INTENSITY_COEF * solar_flux * self.voxels.intensity
return result
def render(self, name_suffix):
self.prepare()
for i, o in self._objs.values():
o.prepare(self)
for c in self._cams.values():
c.prepare(self)
sun_sc_v = np.mean(np.array([o.loc - self._sun_loc for _, o in self._objs.values()]).reshape((-1, 3)), axis=0)
sun_distance = np.linalg.norm(sun_sc_v)
obj_idxs = [i for i, o in self._objs.values()]
for cam_name, c in self._cams.items():
rel_pos_v = {}
rel_rot_q = {}
for i, o in self._objs.values():
rel_pos_v[i] = tools.q_times_v(c.q.conj(), o.loc - c.loc)
rel_rot_q[i] = c.q.conj() * o.q
# make sure correct order, correct scale
rel_pos_v = [rel_pos_v[i]/self.object_scale for i in obj_idxs]
rel_rot_q = [rel_rot_q[i] for i in obj_idxs]
light_v = tools.q_times_v(c.q.conj(), tools.normalize_v(sun_sc_v))
self._renderer.set_frustum(c.model.x_fov, c.model.y_fov, c.frustum_near, c.frustum_far)
flux = TestLoop.render_navcam_image_static(None, self._renderer, obj_idxs, rel_pos_v, rel_rot_q,
light_v, c.q, sun_distance, cam=c.model, auto_gain=False,
use_shadows=True, use_textures=True, fluxes_only=True,
stars=self.stars, lens_effects=self.lens_effects,
reflmod_params=self.hapke_params, star_db=RenderScene.STAR_DB)
image = flux if self.flux_only else c.model.sense(flux, exposure=c.exposure, gain=c.gain)
if self.normalize:
image /= np.max(image)
if self.debug:
sc = 1536/image.shape[0]
img = cv2.resize(image, None, fx=sc, fy=sc) / (np.max(image) if self.flux_only else 1)
cv2.imshow('result', img)
cv2.waitKey()
# save image
self._save_img(image, cam_name, name_suffix)
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.lat_lon_roll_to_q(0, math.pi, 0)
# logger.info('Bug with solvePnPRansac, correcting:\np\t%s => %s\np\t%s => %s'%(
# new_sc_pos, tpos, tools.q_to_lat_lon_roll(cv_cam_delta_q), tools.q_to_lat_lon_roll(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
) # TODO: check that rotation ok, changed from sc_q = sc_q * dq to sc_q = dq * sc_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 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 _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 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)
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
control = RenderController(outpath)
control.create_scene('test_sc')
control.set_scene_config({
'debug': True,
'flux_only': False,
'normalize': False,
'stars': True,
'lens_effects': True, # includes the sun
'hapke_params': RenderObject.HAPKE_PARAMS,
})
control.create_camera('test_cam', scenes='test_sc')
control.configure_camera('test_cam', lens=35.0, sensor=5e-3*1024)
control.set_exposure(0.001 if target == 'sun' else 0.3 if target == 'sssb' else 50.0)
control.set_resolution((1024, 1024))
control.set_output_format('png', '8')
control.set_sun_location(tools.q_times_v(tools.ypr_to_q(math.radians(-90+120), 0, 0), [1.496e11, 0, 0]))
control.set_camera_location('test_cam')
objfile1, url1 = os.path.join(datapath, 'ryugu+tex-d1-16k.obj'), 'https://drive.google.com/uc?authuser=0&id=1Lu48I4nnDKYtvArUN7TnfQ4b_MhSImKM&export=download'
objfile2, url2 = os.path.join(datapath, 'ryugu+tex-d1-16k.mtl'), 'https://drive.google.com/uc?authuser=0&id=1qf0YMbx5nIceGETqhNqePyVNZAmqNyia&export=download'
objfile3, url3 = os.path.join(datapath, 'ryugu.png'), 'https://drive.google.com/uc?authuser=0&id=19bT_Qd1MBfxM1wmnCgx6PT58ujnDFmsK&export=download'
RenderController.download_file(url1, objfile1, maybe=True)
RenderController.download_file(url2, objfile2, maybe=True)
RenderController.download_file(url3, objfile3, maybe=True)
obj = control.load_object(os.path.join(datapath, 'ryugu+tex-d1-16k.obj'), 'ryugu-16k')
obj.location = (0, 0, 0)
if target == 'sun':
control.target_camera(control.SOL, "test_cam")
elif target == 'sssb':
control.target_camera(obj, "test_cam")
else:
control.set_camera_location("test_cam", None, (0, 1, 0, 0))
def export(sm, dst_path, src_path=None, src_imgs=None, trg_shape=(224, 224), crop=False, debug=False,
img_prefix="", title=""):
trg_w, trg_h = trg_shape
assert (src_path is not None) + (src_imgs is not None) == 1, 'give either src_path or src_imgs, not both'
if debug:
renderer = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0)
obj_idx = renderer.load_object(sm.asteroid.target_model_file,
smooth=sm.asteroid.render_smooth_faces)
algo = AlgorithmBase(sm, renderer, obj_idx)
metadatafile = os.path.join(dst_path, 'dataset_all.txt')
if not os.path.exists(metadatafile):
with open(metadatafile, 'w') as f:
f.write('\n'.join(['%s, camera centric coordinate frame used' % title,
'Image ID, ImageFile, Target Pose [X Y Z W P Q R], Sun Vector [X Y Z]', '', '']))
files = list(os.listdir(src_path)) if src_imgs is None else src_imgs
files = sorted(files)
id = 0
for i, fn in enumerate(files):
if src_imgs is not None or re.search(r'(?<!far_)\d{4}\.png$', fn):
c = 2 if src_imgs is None else 1
tools.show_progress(len(files)//c, i//c)
id += 1
# read system state, write out as relative to s/c
fname = os.path.basename(fn)
if src_imgs is None:
fn = os.path.join(src_path, fn)
lbl_fn = re.sub(r'_%s(\d{4})' % img_prefix, r'_\1', fn[:-4]) + '.lbl'
sm.load_state(lbl_fn)
sm.swap_values_with_real_vals()
if not crop:
shutil.copy2(fn, os.path.join(dst_path, fname))
if os.path.exists(fn[:-4] + '.d.exr'):
shutil.copy2(fn[:-4] + '.d.exr', os.path.join(dst_path, fname[:-4] + '.d.exr'))
if os.path.exists(fn[:-4] + '.xyz.exr'):
shutil.copy2(fn[:-4] + '.xyz.exr', os.path.join(dst_path, fname[:-4] + '.xyz.exr'))
if os.path.exists(fn[:-4] + '.s.exr'):
shutil.copy2(fn[:-4] + '.s.exr', os.path.join(dst_path, fname[:-4] + '.s.exr'))
_write_metadata(metadatafile, id, fname, sm.get_system_scf())
continue
from visnav.algo.absnet import AbsoluteNavigationNN
# read image, detect box, resize, adjust relative pose
img = cv2.imread(fn, cv2.IMREAD_GRAYSCALE)
assert img is not None, 'image file %s not found' % fn
# detect target, get bounds
x, y, w, h = ImageProc.single_object_bounds(img, threshold=AbsoluteNavigationNN.DEF_LUMINOSITY_THRESHOLD,
crop_marg=AbsoluteNavigationNN.DEF_CROP_MARGIN,
min_px=AbsoluteNavigationNN.DEF_MIN_PIXELS, debug=debug)
if x is None:
continue
# write image metadata
system_scf = sm.get_cropped_system_scf(x, y, w, h)
_write_metadata(metadatafile, id, fname, system_scf)
others, (depth, coords, px_size), k = [], [False] * 3, 1
if os.path.exists(fn[:-4] + '.d.exr'):
depth = True
others.append(cv2.imread(fn[:-4] + '.d.exr', cv2.IMREAD_UNCHANGED))
if os.path.exists(fn[:-4] + '.xyz.exr'):
coords = True
others.append(cv2.imread(fn[:-4] + '.xyz.exr', cv2.IMREAD_UNCHANGED))
if os.path.exists(fn[:-4] + '.s.exr'):
px_size = True
others.append(cv2.imread(fn[:-4] + '.s.exr', cv2.IMREAD_UNCHANGED))
# crop & resize image, write it
cropped = ImageProc.crop_and_zoom_image(img, x, y, w, h, None, (trg_w, trg_h), others=others)
cv2.imwrite(os.path.join(dst_path, fname), cropped[0], [cv2.IMWRITE_PNG_COMPRESSION, 9])
if depth:
cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.d.exr'), cropped[k],
(cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT))
k += 1
if coords:
cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.xyz.exr'), cropped[k],
(cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT))
k += 1
if px_size:
cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.s.exr'), cropped[k],
(cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT))
if debug:
sc, dq = sm.cropped_system_tf(x, y, w, h)
sm.spacecraft_pos = tools.q_times_v(SystemModel.sc2gl_q.conj(), sc_ast_lf_r)
sm.rotate_spacecraft(dq)
#sm.set_cropped_system_scf(x, y, w, h, sc_ast_lf_r, sc_ast_lf_q)
if False:
sm.load_state(lbl_fn)
sm.swap_values_with_real_vals()
imgd = cv2.resize(img, (trg_h, trg_w))
imge = algo.render(center=False, depth=False, shadows=True)
h, w = imge.shape
imge = cv2.resize(imge[:, (w - h)//2:(w - h)//2+h], cropped[0].shape)
cv2.imshow('equal?', np.hstack((
cropped[0],
np.ones((cropped[0].shape[0], 1), dtype=cropped[0].dtype) * 255,
imge,
)))
cv2.waitKey()
if i > 60:
quit()
if __name__ == '__main__':
from visnav.batch1 import get_system_model
# sm = get_system_model('rose024', False)
# params = load_image_meta(r'C:\projects\visnav\data\rosetta-mtp024\ROS_CAM1_20151223T145135.LBL', sm)
sm = get_system_model('rose018', False)
params = load_image_meta(r'C:\projects\visnav\data\rosetta-mtp018\ROS_CAM1_20150710T074301.LBL', sm)
sun_ast_igrf_r = sm.asteroid.real_position
sun_sc_igrf_r = params['sun_sc_ec_p']
ast_sc_igrf_r = -params['sc_ast_ec_p']
ast_igrf_q = sm.asteroid.rotation_q(sm.time.real_value)
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_ra = sm.asteroid.rotation_theta(sm.time.real_value)
arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr])
print('sun_ast_igrf_r [m]: %s' % arr2str(sun_ast_igrf_r * 1e3))
print('sun_sc_igrf_r [m]: %s' % arr2str(sun_sc_igrf_r * 1e3))
print('ast_sc_igrf_r [m]: %s' % arr2str(ast_sc_igrf_r * 1e3))
print('ast_axis_ra [deg]: %f' % math.degrees(ast_axis_ra))
print('ast_axis_dec [deg]: %f' % math.degrees(ast_axis_dec))
print('ast_zlon_ra [deg]: %f' % math.degrees(ast_zlon_ra))
aa = quaternion.as_rotation_vector(sm.spacecraft_q)
angle = np.linalg.norm(aa)
def get_ba_params(path, ff, lf, results, kapt, sensor_id):
frames = [(id, fname[sensor_id])
for id, fname in kapt.records_camera.items()
if id >= ff and (lf is None or id < lf)]
fname2id = {fname: id for id, fname in frames}
poses = np.array([[
*tools.q_to_angleaxis(kapt.trajectories[id][sensor_id].r, True),
*kapt.trajectories[id][sensor_id].t
] for id, fname in frames]).astype(float)
pts3d = kapt.points3d[:, :3]
if SET_3D_POINT_ALT:
pts3d[:, 1] = -TAKEOFF_LAWN_ALT
feat = kapt.keypoints[FEATURE_NAME]
uv_map = {}
for id_f, fname in frames:
uvs = image_keypoints_from_file(
os.path.join(path, 'kapture', 'reconstruction', 'keypoints',
FEATURE_NAME, fname + '.kpt'), feat.dtype, feat.dsize)
uv_map[id_f] = uvs
f_uv = {}
for id3, r in kapt.observations.items():
for fname, id2 in r[FEATURE_NAME]:
if fname in fname2id:
id_f = fname2id[fname]
f_uv.setdefault(id_f, {})[id3] = uv_map[id_f][id2, :]
# f_uv = {id_f: {id3: uv_map[id_f][id2, :]
# for id3 in range(len(pts3d))
# for id2 in range(len(uv_map[id_f]))
# if (fname, id2) in kapt.observations.get(id3, {}).get(VO_FEATURE_NAME, {})}
# for id_f, fname in frames}
obs_kp = list(set.union(*[set(m.keys()) for m in f_uv.values()]))
cam_idxs, pt3d_idxs, pts2d = list(
map(
np.array,
zip(*[(i, id3, uv.flatten())
for i, (id_f, kps_uv) in enumerate(f_uv.items())
for id3, uv in kps_uv.items()])))
meas_idxs = np.array([
i for i, r in enumerate(results)
if r['meas'] is not None and i < len(poses)
],
dtype=int)
meas_q = {i: results[i]['pose'].prior.quat.conj() for i in meas_idxs}
meas_r = np.array([
tools.q_times_v(meas_q[i], -results[i]['pose'].prior.loc)
for i in meas_idxs
],
dtype=np.float32)
meas_aa = np.array(
[tools.q_to_angleaxis(meas_q[i], compact=True) for i in meas_idxs],
dtype=np.float32)
return poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp
) #keypoint_algo=VisualOdometry.KEYPOINT_FAST)
# ast_q = quaternion.one
ast_q = tools.rand_q(math.radians(15))
current_sc = 1 / np.linalg.norm(ast_v)
sc_threshold = 3
for t in range(100):
ast_v[2] += 0.001
#ast_q = q**t
#ast_q = tools.rand_q(math.radians(0.1)) * ast_q
#n_ast_q = ast_q * tools.rand_q(math.radians(.3))
n_ast_q = ast_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,
def plot_results(keyframes=None,
map3d=None,
frame_names=None,
meta_names=None,
ground_truth=None,
ba_errs=None,
file='result.pickle',
nadir_looking=False):
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
# TODO: remove override
nadir_looking = False
if keyframes is None:
with open(file, 'rb') as fh:
keyframes, map3d, frame_names, meta_names, ground_truth, *ba_errs = pickle.load(
fh)
ba_errs = ba_errs[0] if len(ba_errs) else None
w2b, b2c = NokiaSensor.w2b, NokiaSensor.b2c
nl_dq = tools.eul_to_q(
(-np.pi / 2, ), 'y') if nadir_looking else quaternion.one
est_loc = np.ones((len(keyframes), 3)) * np.nan
est_ori = np.ones((len(keyframes), 3)) * np.nan
meas_loc = np.ones((len(keyframes), 3)) * np.nan
meas_ori = np.ones((len(keyframes), 3)) * np.nan
for i, kf in enumerate(keyframes):
if kf and kf['pose'] and kf['pose'].post:
if kf['pose'].method == VisualOdometry.POSE_2D3D:
est_loc[i, :] = ((
-kf['pose'].post).to_global(b2c).to_global(w2b)).loc
meas_loc[i, :] = ((
-kf['pose'].prior).to_global(b2c).to_global(w2b)).loc
est_ori[i, :] = tools.q_to_ypr(
nl_dq.conj() * ((-kf['pose'].post).to_global(b2c)).quat)
meas_ori[i, :] = tools.q_to_ypr(
nl_dq.conj() * ((-kf['pose'].prior).to_global(b2c)).quat)
est_ori = est_ori / np.pi * 180
meas_ori = meas_ori / np.pi * 180
if nadir_looking:
# TODO: better way, now somehow works heuristically
est_ori = est_ori[:, (2, 0, 1)]
meas_ori = meas_ori[:, (2, 0, 1)]
fst = np.where(np.logical_not(np.isnan(est_loc[:, 0])))[0][0]
idx = np.where([kf['pose'] is not None for kf in keyframes])[0]
idx2 = np.where([
kf['pose'] is not None and kf['meas'] is not None for kf in keyframes
])[0]
t0 = keyframes[idx[0]]['time'].timestamp()
t = np.array([keyframes[i]['time'].timestamp() - t0 for i in idx])
t2 = np.array([
keyframes[i]['time'].timestamp() - t0 + keyframes[i]['meas'].time_off +
keyframes[i]['meas'].time_adj for i in idx2
])
dt = np.array([keyframes[i]['meas'].time_adj for i in idx2])
rng = np.nanmax(est_loc[idx, :2], axis=0) - np.nanmin(est_loc[idx, :2],
axis=0)
rng2 = 0 if len(idx2) == 0 else np.nanmax(
meas_loc[idx2, :2], axis=0) - np.nanmin(meas_loc[idx2, :2], axis=0)
mrg = 0.05 * max(np.max(rng), np.max(rng2))
min_x = min(np.nanmin(est_loc[idx, 0]),
9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 0])) - mrg
max_x = max(np.nanmax(est_loc[idx, 0]),
-9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 0])) + mrg
min_y = min(np.nanmin(est_loc[idx, 1]),
9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 1])) - mrg
max_y = max(np.nanmax(est_loc[idx, 1]),
-9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 1])) + mrg
min_z = min(np.nanmin(est_loc[idx, 2]),
9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 2])) - mrg
max_z = max(np.nanmax(est_loc[idx, 2]),
-9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 2])) + mrg
xx, yy = np.meshgrid(np.linspace(min_x, max_x, 10),
np.linspace(min_y, max_y, 10))
zz = np.ones(xx.shape) * min(min_z + mrg / 2, 0)
min_z = min(0, min_z)
fig = plt.figure(1)
ax = fig.add_subplot(111, projection='3d')
ax.plot_surface(xx, yy, zz, zorder=2)
line = ax.plot(est_loc[idx, 0],
est_loc[idx, 1],
est_loc[idx, 2],
'C0',
zorder=3)
ax.scatter(est_loc[fst, 0],
est_loc[fst, 1],
est_loc[fst, 2],
'C2',
zorder=4)
line2 = ax.plot(meas_loc[idx2, 0],
meas_loc[idx2, 1],
meas_loc[idx2, 2],
'C1',
zorder=5)
ax.set_xlim(min_x, max_x)
ax.set_ylim(min_y, max_y)
ax.set_zlim(min_z, max_z)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
tools.hover_annotate(fig, ax, line[0], frame_names)
tools.hover_annotate(fig, ax, line2[0], [meta_names[i] for i in idx2])
if 1:
fig, axs = plt.subplots(1, 1)
axs = [axs]
axs[0].set_aspect('equal')
rng_x = max_x - min_x
rng_y = max_y - min_y
if True or rng_x > rng_y:
axs[0].plot(est_loc[fst, 0], est_loc[fst, 1], 'oC0', mfc='none')
line = axs[0].plot(est_loc[idx, 0], est_loc[idx, 1],
'C0') # , '+-')
line2 = axs[0].plot(meas_loc[idx2, 0], meas_loc[idx2, 1],
'C1') # , '+-')
axs[0].set_xlim(min_x, max_x)
axs[0].set_ylim(min_y, max_y)
axs[0].set_xlabel('x')
axs[0].set_ylabel('y')
else:
axs[0].plot(est_loc[fst, 1], est_loc[fst, 0], 'oC0', mfc='none')
line = axs[0].plot(est_loc[idx, 1], est_loc[idx, 0],
'C0') # , '+-')
line2 = axs[0].plot(meas_loc[idx2, 1], meas_loc[idx2, 0],
'C1') # , '+-')
axs[0].set_xlim(min_y, max_y)
axs[0].set_ylim(min_x, max_x)
axs[0].set_xlabel('y')
axs[0].set_ylabel('x')
tools.hover_annotate(fig, axs[0], line[0], frame_names)
tools.hover_annotate(fig, axs[0], line2[0],
[meta_names[i] for i in idx2])
fig, axs = plt.subplots(2, 1, sharex=True)
axs[0].plot(t[fst], est_loc[fst, 2], 'oC0', mfc='none')
line = axs[0].plot(t, est_loc[idx, 2], 'C0') # , '+-')
line2 = axs[0].plot(t2, meas_loc[idx2, 2], 'C1') # , '+-')
tools.hover_annotate(fig, axs[0], line[0], frame_names)
tools.hover_annotate(fig, axs[0], line2[0],
[meta_names[i] for i in idx2])
axs[0].set_ylabel('z')
axs[0].set_xlabel('t')
if ba_errs is None:
line = axs[1].plot(t2, dt, 'C0') #, '+-')
tools.hover_annotate(fig, axs[1], line[0],
[meta_names[i] for i in idx2])
axs[1].set_ylabel('dt')
else:
id2idx = {kf['id']: i for i, kf in enumerate(keyframes)}
t3 = [
keyframes[id2idx[int(id)]]['time'].timestamp() - t0
for id in ba_errs[:, 0]
]
axs[1].plot(t3, ba_errs[:, 1], 'C0', label='repr [px]')
axs[1].plot(t3, ba_errs[:, 2], 'C1', label='loc [m]')
axs[1].plot(t3, ba_errs[:, 3], 'C2', label='ori [deg]')
axs[1].set_title('BA errors')
axs[1].legend()
axs[1].set_xlabel('t')
fig, axs = plt.subplots(3, 1, sharex=True)
for i, title in enumerate(('yaw', 'pitch', 'roll')):
axs[i].plot(t[fst], est_ori[fst, i], 'oC0', mfc='none')
line = axs[i].plot(t, est_ori[idx, i], 'C0') # , '+-')
line2 = axs[i].plot(t2, meas_ori[idx2, i], 'C1') # , '+-')
tools.hover_annotate(fig, axs[i], line[0], frame_names)
tools.hover_annotate(fig, axs[i], line2[0],
[meta_names[j] for j in idx2])
axs[i].set_ylabel(title)
axs[i].set_xlabel('t')
plt.tight_layout()
if map3d is not None and len(map3d) > 0:
x, y, z = np.array(
[tools.q_times_v(w2b.quat * b2c.quat, pt.pt3d) for pt in map3d]).T
if 1:
fig, ax = plt.subplots(1, 1)
ax.set_aspect('equal')
ax.set_xlabel("east", fontsize=12)
ax.set_ylabel("north", fontsize=12)
line = ax.scatter(x,
y,
s=20,
c=z,
marker='o',
vmin=-5.,
vmax=100.,
cmap=cm.get_cmap('jet')) #, vmax=20.)
fig.colorbar(line)
tools.hover_annotate(fig, ax, line, ['%.1f' % v for v in z])
else:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(x, y, z, '.')
ax.set_xlabel("east", fontsize=12)
ax.set_ylabel("north", fontsize=12)
ax.set_zlabel("alt", fontsize=12)
plt.show()
print('ok: %.1f%%, delta loc std: %.3e' % (
100 * (1 - np.mean(np.isnan(est_loc[:, 0]))),
np.nanstd(np.linalg.norm(np.diff(est_loc[:, :3], axis=0), axis=1)),
))
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])
control.set_scene_config({
'debug': True,
'flux_only': False,
'normalize': False,
'stars': True,
'lens_effects': True, # includes the sun
'brdf_params': RenderObject.HAPKE_PARAMS,
})
control.create_camera('test_cam', scenes='test_sc')
control.configure_camera('test_cam', lens=35.0, sensor=5e-3 * 1024)
control.set_exposure(0.001 if target == 'sun' else 0.3 if target ==
'sssb' else 50.0)
control.set_resolution((1024, 1024))
control.set_output_format('png', '8')
control.set_sun_location(
tools.q_times_v(tools.ypr_to_q(math.radians(-90 + 120), 0, 0),
[1.496e11, 0, 0]))
control.set_camera_location('test_cam')
objfile1, url1 = os.path.join(
datapath, 'ryugu+tex-d1-16k.obj'
), 'https://drive.google.com/uc?authuser=0&id=1Lu48I4nnDKYtvArUN7TnfQ4b_MhSImKM&export=download'
objfile2, url2 = os.path.join(
datapath, 'ryugu+tex-d1-16k.mtl'
), 'https://drive.google.com/uc?authuser=0&id=1qf0YMbx5nIceGETqhNqePyVNZAmqNyia&export=download'
objfile3, url3 = os.path.join(
datapath, 'ryugu.png'
), 'https://drive.google.com/uc?authuser=0&id=19bT_Qd1MBfxM1wmnCgx6PT58ujnDFmsK&export=download'
RenderController.download_file(url1, objfile1, maybe=True)
RenderController.download_file(url2, objfile2, maybe=True)
RenderController.download_file(url3, objfile3, maybe=True)
obj = control.load_object(os.path.join(datapath, 'ryugu+tex-d1-16k.obj'),
'ryugu-16k')
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
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(self, name_suffix):
self.prepare()
for i, o in self._objs.values():
o.prepare(self)
for c in self._cams.values():
c.prepare(self)
sun_sc_v = np.mean(np.array(
[o.loc - self._sun_loc for _, o in self._objs.values()]).reshape(
(-1, 3)),
axis=0)
sun_distance = np.linalg.norm(sun_sc_v)
obj_idxs = [i for i, o in self._objs.values()]
for cam_name, c in self._cams.items():
rel_pos_v = {}
rel_rot_q = {}
for i, o in self._objs.values():
rel_pos_v[i] = tools.q_times_v(c.q.conj(), o.loc - c.loc)
rel_rot_q[i] = c.q.conj() * o.q
# make sure correct order, correct scale
rel_pos_v = [rel_pos_v[i] / self.object_scale for i in obj_idxs]
rel_rot_q = [rel_rot_q[i] for i in obj_idxs]
light_v = tools.q_times_v(c.q.conj(), tools.normalize_v(sun_sc_v))
brdf_params = RenderObject.HAPKE_PARAMS if self.brdf_params is None else self.brdf_params
self._renderer.set_frustum(c.model.x_fov, c.model.y_fov,
c.frustum_near, c.frustum_far)
flux = TestLoop.render_navcam_image_static(
None,
self._renderer,
obj_idxs,
rel_pos_v,
rel_rot_q,
light_v,
self._gl2sc(c.q),
sun_distance,
cam=c.model,
auto_gain=False,
use_shadows=True,
use_textures=True,
fluxes_only=True,
stars=self.stars,
lens_effects=self.lens_effects,
particles=self._particles,
reflmod_params=brdf_params,
star_db=self._stardb)
if self.flux_only:
image = flux
elif self.sispo_cam:
image = self.sispo_cam.sense(flux)
else:
image = c.model.sense(flux, exposure=c.exposure, gain=c.gain)
if self.normalize or self.sispo_cam:
tmp = np.max(image)
image /= tmp if tmp > 0 else 1 # in case whole image is just zeros
# TODO: possibly call code related to self.with_infobox or self.with_clipping at compositor.py
if self.debug:
sc = 1536 / image.shape[0]
img = cv2.resize(image, None, fx=sc, fy=sc) / (
np.max(image) if self.flux_only else 1)
cv2.imshow('result', img)
cv2.waitKey()
# save image
self._save_img(image, cam_name, name_suffix)
if compare:
title = 'real vs synthetic'
img = np.concatenate(
[real, 255 * np.ones(
(real.shape[0], 1), dtype='uint8')] + synth,
axis=1)
else:
title = 'synthetic'
img = synth[0]
sc = min(1536 / img.shape[1], 1024 / img.shape[0])
cv2.imshow(title, cv2.resize(img, None, fx=sc, fy=sc))
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 flux_density_cones(self,
lf_ast_v,
lf_ast_q,
mask,
solar_flux,
down_scaling=1,
quad_lim=25):
"""
- The jet generated is a truncated cone that has a density proportional to (truncation_distance/distance_from_untruncated_origin)**2
- Truncation so that base width is 0.1 of mask diameter
- base_loc gives the coordinates of the base in camera frame (opengl type, -z camera axis, +y is up)
- 95% of luminosity lost when distance from base is `length`
- angular_radius [rad] of the cone, 95% of luminosity lost if this much off axis, uses normal distribution
- intensity of the cone at truncation point (>0)
- if phase_angle < pi/2, cone not drawn on top of masked parts of image as it starts behind object
- direction: 0 - to the right, pi/2 - up, pi - left, -pi/2 - down
"""
assert down_scaling >= 1, 'only values of >=1 make sense for down_scaling'
scaling = 1 / down_scaling
base_locs = [c.base_loc for c in self.cones]
phase_angles = [c.phase_angle for c in self.cones]
directions = [c.direction for c in self.cones]
trunc_lens = [c.trunc_len for c in self.cones]
angular_radii = [c.angular_radius for c in self.cones]
intensities = [c.intensity for c in self.cones]
axes = []
for i in range(len(self.cones)):
# q = np.quaternion(math.cos(-direction / 2), 0, 0, math.sin(-direction / 2)) \
# * np.quaternion(math.cos(phase_angle / 2), 0, math.sin(phase_angle / 2), 0)
q1 = np.quaternion(math.cos(-phase_angles[i] / 2), 0,
math.sin(-phase_angles[i] / 2), 0)
q2 = np.quaternion(math.cos(directions[i] / 2), 0, 0,
math.sin(directions[i] / 2))
q = q2 * q1
axis = tools.q_times_v(q, np.array([0, 0, -1]))
axes.append(axis)
base_locs[i] -= axis * trunc_lens[i]
# density function of the jet
def density(loc_arr, base_loc, axis, d0, angular_radius, intensity):
loc_arr = loc_arr - base_loc
r, d = tools.dist_across_and_along_vect(loc_arr, axis)
# r, d = tools.point_vector_dist(loc_arr, axis, dist_along_v=True)
# get distance along axis
coef = np.zeros((len(loc_arr), 1))
coef[d > d0] = (d0 / d[d > d0])**2
# get radial distance from axis, use normal dist pdf but scaled so that max val is 1
r_sd = d[coef > 0] * np.tan(angular_radius)
coef[coef > 0] *= np.exp((-0.5 / r_sd**2) * (r[coef > 0]**2))
return coef * intensity
dq = lf_ast_q * self.cones.lf_ast_q.conj()
dv = tools.q_times_v(dq.conj(), lf_ast_v - self.cones.lf_ast_v)
ray_axes, sc_shape = self._px_ray_axes(scaling, dq)
def i_fun(r, arg_arr):
result = None
for args in arg_arr:
res = density(ray_axes * r - dv, *args)
if result is None:
result = res
else:
result += res
return result
bg_args_arr, bg_near, bg_far = [], np.inf, -np.inf
fg_args_arr, fg_near, fg_far = [], np.inf, -np.inf
for args in zip(base_locs, axes, trunc_lens, angular_radii,
intensities):
base_loc, axis, trunc_len, angular_radius, intensity = args
dist = np.linalg.norm(base_loc)
if axis[2] < 0:
# z-component of axis is negative => jet goes away from cam => starts behind object
bg_args_arr.append(args)
bg_near = min(bg_near, dist - trunc_len)
bg_far = max(bg_far,
2 * dist) # crude heuristic, is it enough?
else:
# jet goes towards cam
fg_args_arr.append(args)
fg_near = min(fg_near, 0)
fg_far = max(fg_far, dist + trunc_len)
bg_res, fg_res = None, None
if bg_args_arr:
# integrate density along the rays (quad_vec requires at least scipy 1.4.x)
res = integrate.quad_vec(lambda r: i_fun(r, bg_args_arr),
bg_near,
bg_far,
limit=quad_lim)
# bg_sc = np.max(res[0])/maxval
bg_res = cv2.resize(res[0].reshape(sc_shape).astype(np.float32),
mask.shape)
# bg_res = cv2.resize((res[0]/bg_sc).reshape(xx.shape).astype(img.dtype), img.shape).astype(np.float32)*bg_sc
bg_res[mask] = 0
if fg_args_arr:
# integrate density along the rays (quad_vec requires at least scipy 1.4.x)
res = integrate.quad_vec(lambda r: i_fun(r, fg_args_arr),
fg_near,
fg_far,
limit=quad_lim)
# fg_sc = np.max(res[0])/maxval
fg_res = cv2.resize(res[0].reshape(sc_shape).astype(np.float32),
mask.shape)
# fg_res = cv2.resize((res[0]/fg_sc).reshape(xx.shape).astype(img.dtype), img.shape).astype(np.float32)*fg_sc
result = ((0 if bg_res is None else bg_res) + (0 if fg_res is None else fg_res)) \
* Particles.CONE_INTENSITY_COEF * solar_flux
# max_r = np.max(result)
# if max_r > 0:
# maxval = ImageProc._img_max_valid(img)
# result = (result / max_r) * maxval * np.max(intensities)
return result
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()
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 _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 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))
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
