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 relative_pose_err(sm, imgs):
try:
img0, img1 = [cv2.imread(img, cv2.IMREAD_UNCHANGED) for img in imgs]
kp0, desc0, det = KeypointAlgo.detect_features(img0,
KeypointAlgo.AKAZE,
max_feats=1000,
for_ref=True,
maxmem=0)
kp1, desc1, det = KeypointAlgo.detect_features(img1,
KeypointAlgo.AKAZE,
max_feats=1000,
for_ref=False,
maxmem=0)
matches = KeypointAlgo.match_features(desc1,
desc0,
det.defaultNorm(),
method='brute')
depth0 = cv2.imread(imgs[0][:-4] + '.d.exr', cv2.IMREAD_UNCHANGED)
kp0_3d = KeypointAlgo.inverse_project(
sm, [kp0[m.trainIdx].pt for m in matches], depth0,
np.nanmax(depth0), 1)
kp1_2d = np.array([kp1[m.queryIdx].pt for m in matches],
dtype='float32')
rvec, tvec, inliers = KeypointAlgo.solve_pnp_ransac(
sm, kp1_2d, kp0_3d, 8)
except PositioningException as e:
inliers = []
err = np.nan
if len(inliers) >= KeypointAlgo.MIN_FEATURES:
rel_q = true_relative_pose(sm, imgs)
sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME)
est_rel_cv_q = tools.angleaxis_to_q(rvec)
est_rel_q1 = sc2cv_q * est_rel_cv_q * sc2cv_q.conj()
# solvePnPRansac has some bug that apparently randomly gives 180deg wrong answer
est_rel_q2 = sc2cv_q * est_rel_cv_q * tools.ypr_to_q(
0, math.pi, 0) * sc2cv_q.conj()
err = math.degrees(
min(tools.angle_between_q(est_rel_q1, rel_q),
tools.angle_between_q(est_rel_q2, rel_q)))
return err
def assertQuatAlmostEqual(self, quat0, quat1, delta=1e-4, msg=None):
if quat0 is None and quat1 is None:
return
diff = math.degrees(tools.angle_between_q(quat0, quat1))
self.assertAlmostEqual(0,
diff,
delta=delta,
msg=None if msg is None else
(msg + ': angle[deg] %f > %f' % (diff, delta)))
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 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
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
#print('\n')
# print('rea ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a) for a in tools.q_to_ypr(cam_q)))
# print('est ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a) for a in tools.q_to_ypr(res_q)))
print('rea ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a)
for a in tools.q_to_ypr(ast_q)))
print('est ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a)
for a in tools.q_to_ypr(est_q)))
# print('err ypr: %s' % ' '.join('%.2fdeg' % math.degrees(a) for a in tools.q_to_ypr(err_q)))
print('err angle: %.2fdeg' % math.degrees(err_angle))
# print('rea v: %s' % ' '.join('%.1fm' % a for a in cam_v*1000))
# print('est v: %s' % ' '.join('%.1fm' % a for a in res_v*1000))
print('rea v: %s' % ' '.join('%.1fm' % a for a in ast_v * 1000))
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 _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)
