def costfun(x, sm, T, Y, verbose=0):
sm.asteroid.rotation_pm = tools.wrap_rads(x[0])
if len(x) > 1:
sm.asteroid.rotation_velocity = x[1]
if len(x) > 2:
sm.asteroid.axis_latitude = x[2]
sm.asteroid.axis_longitude = x[3]
sm.asteroid_rotation_from_model()
errs = []
for i, y in enumerate(Y):
sm.time.value = T[i]
ast_q = sm.asteroid_q
errs.append(np.abs(tools.wrap_degs(math.degrees(tools.angle_between_q(ast_q, y)))))
errs = np.array(errs)
max_err = np.percentile(np.abs(errs), 99)
I = np.abs(errs) < max_err
err = np.sqrt(np.mean(errs[I]**2))
if verbose > 1:
#plt.plot((T[I]-np.min(T))/np.ptp(T), errs[I])
plt.plot(errs[I])
plt.show()
if verbose > 0:
base_w = 2 * math.pi / 12.4043 * 24
print(('%.2f' + (', %.6f' if len(x) > 1 else '') + (', %.2f, %.2f' if len(x) > 2 else '') + ' => %.3f') % (
(math.degrees(tools.wrap_rads(x[0])),)
+ ((math.degrees(x[1]*3600*24 - base_w),) if len(x) > 1 else tuple())
+ ((math.degrees(x[2]), math.degrees(x[3]),) if len(x) > 2 else tuple())
+ (err,)))
return err
def add_noise(self, sm):
rotation_noise = True if self._state_list is None else self._rotation_noise
## add measurement noise to
# - datetime (seconds)
if rotation_noise:
meas_time = sm.time.real_value + np.random.normal(0, self._noise_time)
sm.time.value = meas_time
assert np.isclose(sm.time.value, meas_time), 'Failed to set time value'
# - asteroid state estimate
ax_lat, ax_lon, ax_phs = map(rad, sm.real_asteroid_axis)
noise_da = np.random.uniform(0, rad(self._noise_ast_rot_axis))
noise_dd = np.random.uniform(0, 2*math.pi)
meas_ax_lat = ax_lat + noise_da*math.sin(noise_dd)
meas_ax_lon = ax_lon + noise_da*math.cos(noise_dd)
meas_ax_phs = ax_phs + np.random.normal(0, rad(self._noise_ast_phase_shift))
if rotation_noise:
sm.asteroid_axis = map(deg, (meas_ax_lat, meas_ax_lon, meas_ax_phs))
# - spacecraft orientation measure
sc_lat, sc_lon, sc_rot = map(rad, sm.real_spacecraft_rot)
meas_sc_lat = max(-math.pi/2, min(math.pi/2, sc_lat
+ np.random.normal(0, rad(self._noise_sco_lat))))
meas_sc_lon = wrap_rads(sc_lon
+ np.random.normal(0, rad(self._noise_sco_lon)))
meas_sc_rot = wrap_rads(sc_rot
+ np.random.normal(0, rad(self._noise_sco_rot)))
if rotation_noise:
sm.spacecraft_rot = map(deg, (meas_sc_lat, meas_sc_lon, meas_sc_rot))
# - spacecraft position noise
if self.enable_initial_location:
sm.spacecraft_pos = self._noisy_sc_position(sm)
else:
sm.spacecraft_pos = self._unknown_sc_pos
# return this initial state
return self._initial_state(sm)
def asteroid_q(self, new_q):
ast = self.asteroid
sc2ast_q = SystemModel.frm_conv_q(SystemModel.SPACECRAFT_FRAME,
SystemModel.ASTEROID_FRAME,
ast=ast)
ast.axis_latitude, ast.axis_longitude, new_theta = tools.q_to_ypr(
new_q * sc2ast_q)
old_theta = ast.rotation_theta(self.time.value)
ast.rotation_pm = tools.wrap_rads(ast.rotation_pm + new_theta -
old_theta)
self.asteroid_rotation_from_model()
def calc_err(self, rvec, tvec, i1, j1, warn=False):
q_res = tools.angleaxis_to_q(rvec)
lat1, roll1 = self._fdb_sc_ast_perms[i1]
lat2, roll2 = self._fdb_sc_ast_perms[j1]
q_src = tools.ypr_to_q(lat1, 0, roll1)
q_trg = tools.ypr_to_q(lat2, 0, roll2)
q_rel = q_trg * q_src.conj()
# q_res.x = -q_res.x
# np.quaternion(0.707106781186547, 0, -0.707106781186547, 0)
m = self.system_model
q_frame = m.frm_conv_q(m.OPENGL_FRAME, m.OPENCV_FRAME)
q_res = q_frame * q_res.conj() * q_frame.conj()
err1 = math.degrees(tools.wrap_rads(tools.angle_between_q(q_res, q_rel)))
err2 = np.linalg.norm(tvec - np.array((0, 0, -self.render_z)).reshape((3, 1)))
ok = not (abs(err1) > 10 or abs(err2) > 0.10 * abs(self.render_z))
if not ok and warn:
print('at (%s, %s), err1: %.1fdeg, err2: %.1fkm\n\tq_real: %s\n\tq_est: %s' % (
i1, j1, err1, err2, q_rel, q_res))
return ok, err1, err2
def run(self, sce_img, outfile, **kwargs):
centroid_result = None
keypoint_ok = False
try:
if True:
self._centroid.adjust_iteratively(sce_img, None, **kwargs)
sc_r1 = self.system_model.spacecraft_rot
ast_r1 = self.system_model.asteroid_axis
rel_q1 = self.system_model.sc_asteroid_rel_q()
centroid_result = self.system_model.spacecraft_pos
else:
centroid_result = self.system_model.real_spacecraft_pos
#kwargs['init_z'] = centroid_result[2]
x_off, y_off = self._cam.calc_img_xy(*centroid_result)
uncertainty_radius = math.tan(math.radians(MixedAlgo.EXPECTED_LATERAL_ERR_ANGLE_SD) * 2) \
* abs(centroid_result[2]) * (1 + MixedAlgo.EXPECTED_RELATIVE_DISTANCE_ERR_SD * 2)
kwargs['match_mask_params'] = (
x_off - self._cam.width / 2,
y_off - self._cam.height / 2,
centroid_result[2],
uncertainty_radius,
)
d1 = np.linalg.norm(centroid_result)
except PositioningException as e:
if str(e) == 'No asteroid found':
raise e
elif not 'Asteroid too close' in str(e) and DEBUG:
print('Centroid algo failed with: %s' % (e, ))
centroid_ok = centroid_result is not None
fallback = centroid_ok and kwargs.get('centroid_fallback', False) \
and self.system_model.max_distance > d1 > self.system_model.min_med_distance
try:
self._keypoint.solve_pnp(sce_img, outfile, **kwargs)
d2 = np.linalg.norm(self.system_model.spacecraft_pos)
rel_q2 = self.system_model.sc_asteroid_rel_q()
d_ang = abs(tools.wrap_rads(tools.angle_between_q(
rel_q1, rel_q2))) if fallback else None
if fallback and (d2 > d1 * 1.2 or d_ang > math.radians(20)):
# if keypoint res distance significantly larger than from centroid method, override
# also, if orientation significantly different from initial, override
self.system_model.spacecraft_pos = centroid_result
self.system_model.spacecraft_rot = sc_r1
self.system_model.asteroid_axis = ast_r1
elif fallback and d2 > self.system_model.max_med_distance:
# if distance more than max medum distance, assume orientation result is wrong
self.system_model.spacecraft_rot = sc_r1
self.system_model.asteroid_axis = ast_r1
else:
keypoint_ok = True
except PositioningException as e:
if fallback:
self.system_model.spacecraft_pos = centroid_result
self.system_model.spacecraft_rot = sc_r1
self.system_model.asteroid_axis = ast_r1
if DEBUG:
print('Using centroid result as keypoint algo failed: %s' %
(e, ))
else:
raise e
return (centroid_ok or keypoint_ok, keypoint_ok)
def calculate_result(self, sm, i, imgfile, ok, initial, **kwargs):
# save function values from optimization
fvals = getattr({
'phasecorr': self.phasecorr,
'keypoint+': self.mixedalgo,
'keypoint': self.keypoint,
'centroid': self.centroid,
}[kwargs['method']], 'extra_values', None)
final_fval = fvals[-1] if fvals else None
real_rel_rot = q_to_ypr(sm.real_sc_asteroid_rel_q())
elong, direc = sm.solar_elongation(real=True)
r_ast_axis = sm.real_asteroid_axis
# real system state
params = (sm.time.real_value, *r_ast_axis,
*sm.real_spacecraft_rot, deg(elong), deg(direc),
*sm.real_spacecraft_pos, sm.real_spacecraft_altitude, *map(deg, real_rel_rot),
imgfile, final_fval)
# calculate added noise
#
getcontext().prec = 6
time_noise = float(Decimal(initial['time']) - Decimal(sm.time.real_value))
ast_rot_noise = (
initial['ast_axis'][0]-r_ast_axis[0],
initial['ast_axis'][1]-r_ast_axis[1],
360*time_noise/sm.asteroid.rotation_period
+ (initial['ast_axis'][2]-r_ast_axis[2])
)
sc_rot_noise = tuple(np.subtract(initial['sc_rot'], sm.real_spacecraft_rot))
dev_angle = deg(angle_between_ypr(map(rad, ast_rot_noise),
map(rad, sc_rot_noise)))
if self.enable_initial_location:
sc_loc_noise = tuple(np.array(initial['sc_pos']) - np.array(sm.real_spacecraft_pos))
else:
sc_loc_noise = ('', '', '')
noise = sc_loc_noise + (time_noise,) + ast_rot_noise + sc_rot_noise + (dev_angle,)
if np.all(ok):
ok_pos, ok_rot = True, True
elif not np.any(ok):
ok_pos, ok_rot = False, False
else:
ok_pos, ok_rot = ok
if ok_pos:
pos = sm.spacecraft_pos
pos_err = tuple(np.subtract(pos, sm.real_spacecraft_pos))
else:
pos = float('nan')*np.ones(3)
pos_err = tuple(float('nan')*np.ones(3))
if ok_rot:
rel_rot = q_to_ypr(sm.sc_asteroid_rel_q())
rot_err = (deg(wrap_rads(angle_between_ypr(rel_rot, real_rel_rot))),)
else:
rel_rot = float('nan')*np.ones(3)
rot_err = (float('nan'),)
alt = float('nan')
if ok_pos and ok_rot:
est_vertices = sm.sc_asteroid_vertices()
max_shift = tools.sc_asteroid_max_shift_error(
est_vertices, sm.asteroid.real_sc_ast_vertices)
alt = sm.spacecraft_altitude
both_err = (max_shift, alt - sm.real_spacecraft_altitude)
else:
both_err = (float('nan'), float('nan'),)
err = pos_err + rot_err + both_err
return params, noise, pos, alt, map(deg, rel_rot), fvals, err
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()