def rotate_image(image, t, q, K):
""" Apply warping corresponding to a random in-plane rotation
Arguments:
- image: Input image
- t, q: Object pose (location,orientation)
- K: Camera calibration matrix
- magnitude: 2 * maximum perturbation per roll
Return:
- image_warped: Output image
- t_new, q_new: Updated object pose
"""
change = (np.random.rand(1) - 0.5) * 170
R_change = se3lib.euler2SO3_left(0, 0, change[0])
# Construct warping (perspective) matrix
M = K * R_change * np.linalg.inv(K)
height, width = np.shape(image)[:2]
image_warped = cv2.warpPerspective(image, M, (width, height),
cv2.WARP_INVERSE_MAP)
# Update pose
t_new = np.array(np.matrix(t) * R_change.T)[0]
q_change = se3lib.SO32quat(R_change)
q_new = np.array(se3lib.quat_mult(q_change, q))[0]
return image_warped, t_new, q_new
def detect_dataset(model, dataset, nr_images):
""" Tests model on N random images of the dataset
and shows the results.
"""
# Variance used only for prob. orientation estimation
delta = model.config.BETA / model.config.ORI_BINS_PER_DIM
var = delta**2 / 12
for i in range(nr_images):
image_id = random.choice(dataset.image_ids)
# Load pose in all formats
loc_gt = dataset.load_location(image_id)
q_gt = dataset.load_quaternion(image_id)
I, I_meta, loc_encoded_gt, ori_encoded_gt = \
net.load_image_gt(dataset, model.config, image_id)
image_ori = dataset.load_image(image_id)
info = dataset.image_info[image_id]
# Run detection
results = model.detect([image_ori], verbose=1)
# Retrieve location
if model.config.REGRESS_LOC:
loc_est = results[0]['loc']
else:
loc_pmf = utils.stable_softmax(results[0]['loc'])
# Compute location mean according to first moment
loc_est = np.asmatrix(loc_pmf) * np.asmatrix(
dataset.histogram_3D_map)
# Compute loc encoding error
loc_encoded_gt = np.asmatrix(loc_encoded_gt) * np.asmatrix(
dataset.histogram_3D_map)
loc_encoded_err = np.linalg.norm(loc_encoded_gt - loc_gt)
# Retrieve orientation
if model.config.REGRESS_ORI:
if model.config.ORIENTATION_PARAM == 'quaternion':
q_est = results[0]['ori']
elif model.config.ORIENTATION_PARAM == 'euler_angles':
q_est = se3lib.SO32quat(
se3lib.euler2SO3_left(results[0]['ori'][0],
results[0]['ori'][1],
results[0]['ori'][2]))
elif model.config.ORIENTATION_PARAM == 'angle_axis':
theta = np.linalg.norm(results[0]['ori'])
if theta < 1e-6:
v = [0, 0, 0]
else:
v = results[0]['ori'] / theta
q_est = se3lib.angleaxis2quat(v, theta)
else:
ori_pmf = utils.stable_softmax(results[0]['ori'])
# Compute mean quaternion
q_est, q_est_cov = se3lib.quat_weighted_avg(
dataset.ori_histogram_map, ori_pmf)
# Multimodal estimation
# Uncomment this block to try the EM framework
# nr_EM_iterations = 5
# Q_mean, Q_var, Q_priors, model_scores = fit_GMM_to_orientation(dataset.ori_histogram_map, ori_pmf, nr_EM_iterations, var)
# print('Multimodal errors',2 * np.arccos(np.abs(np.asmatrix(Q_mean) * np.asmatrix(q_gt).transpose())) * 180 / np.pi)
#
# q_est_1 = Q_mean[0, :]
# q_est_2 = Q_mean[1, :]
# utils.polar_plot(q_est_1, q_est_2)
# Compute Errors
angular_err = 2 * np.arccos(
np.abs(np.asmatrix(q_est) *
np.asmatrix(q_gt).transpose())) * 180 / np.pi
loc_err = np.linalg.norm(loc_est - loc_gt)
print('GT location: ', loc_gt)
print('Est location: ', loc_est)
print('Processed Image:', info['path'])
print('Est orientation: ', q_est)
print('GT_orientation: ', q_gt)
print('Location error: ', loc_err)
print('Angular error: ', angular_err)
# Visualize PMFs
if not model.config.REGRESS_ORI:
nr_bins_per_dim = model.config.ORI_BINS_PER_DIM
utils.visualize_weights(ori_encoded_gt, ori_pmf, nr_bins_per_dim)
# Show image
fig, (ax_1, ax_2) = plt.subplots(1, 2, figsize=(12, 8))
ax_1.imshow(image_ori)
ax_1.set_xticks([])
ax_1.set_yticks([])
ax_2.imshow(image_ori)
ax_2.set_xticks([])
ax_2.set_yticks([])
height_ori = np.shape(image_ori)[0]
width_ori = np.shape(image_ori)[1]
# Recover focal lengths
fx = dataset.camera.fx
fy = dataset.camera.fy
K = np.matrix([[fx, 0, width_ori / 2], [0, fy, height_ori / 2],
[0, 0, 1]])
# Speed labels expresses q_obj_cam whereas
# Urso labels expresses q_cam_obj
if dataset.name == 'Speed':
q_est = se3lib.quat_inv(q_est)
q_gt = se3lib.quat_inv(q_gt)
utils.visualize_axes(ax_1, q_gt, loc_gt, K, 100)
utils.visualize_axes(ax_2, q_est, loc_est, K, 100)
utils.polar_plot(q_gt, q_est)
# Location overlap visualization
fig, ax = plt.subplots()
ax.imshow(image_ori)
# Project 3D coords for visualization
x_est = loc_est[0] / loc_est[2]
y_est = loc_est[1] / loc_est[2]
x_gt = loc_gt[0] / loc_gt[2]
y_gt = loc_gt[1] / loc_gt[2]
if not model.config.REGRESS_LOC:
x_decoded_gt = loc_encoded_gt[0, 0] / loc_encoded_gt[0, 2]
y_decoded_gt = loc_encoded_gt[0, 1] / loc_encoded_gt[0, 2]
circ = Circle((x_decoded_gt * fx + width_ori / 2,
height_ori / 2 + y_decoded_gt * fy),
7,
facecolor='b',
label='encoded')
ax.add_patch(circ)
# Plot locations
circ_gt = Circle(
(x_gt * fx + width_ori / 2, height_ori / 2 + y_gt * fy),
15,
facecolor='r',
label='gt')
ax.add_patch(circ_gt)
circ = Circle(
(x_est * fx + width_ori / 2, height_ori / 2 + y_est * fy),
10,
facecolor='g',
label='pred')
ax.add_patch(circ)
ax.legend(loc='upper right', shadow=True, fontsize='x-small')
plt.show()
def test_and_submit(model, dataset_virtual, dataset_real):
""" Evaluates model on ESA challenge test-set (no labels)
and outputs submission file in a format compatible with the ESA server (probably down by now)
"""
# ESA API
from submission import SubmissionWriter
submission = SubmissionWriter()
# TODO: Make the next 2 loops a nested loop
# Synthetic test set
for image_id in dataset_virtual.image_ids:
print('Image ID:', image_id)
image = dataset_virtual.load_image(image_id)
info = dataset_virtual.image_info[image_id]
results = model.detect([image], verbose=1)
# Retrieve location
if model.config.REGRESS_LOC:
loc_est = results[0]['loc']
else:
loc_pmf = utils.stable_softmax(results[0]['loc'])
# Compute location mean according to first moment
loc_est = np.asmatrix(loc_pmf) * np.asmatrix(
dataset_virtual.histogram_3D_map)
# Retrieve orientation
if model.config.REGRESS_ORI:
if model.config.ORIENTATION_PARAM == 'quaternion':
q_est = results[0]['ori']
elif model.config.ORIENTATION_PARAM == 'euler_angles':
q_est = se3lib.SO32quat(
se3lib.euler2SO3_left(results[0]['ori'][0],
results[0]['ori'][1],
results[0]['ori'][2]))
elif model.config.ORIENTATION_PARAM == 'angle_axis':
theta = np.linalg.norm(results[0]['ori'])
if theta < 1e-6:
v = [0, 0, 0]
else:
v = results[0]['ori'] / theta
q_est = se3lib.angleaxis2quat(v, theta)
else:
ori_pmf = utils.stable_softmax(results[0]['ori'])
# Compute mean quaternion
q_est, q_est_cov = se3lib.quat_weighted_avg(
dataset_virtual.ori_histogram_map, ori_pmf)
# Change quaternion order
q_rect = [q_est[3], q_est[0], q_est[1], q_est[2]]
submission.append_test(info['path'].split('/')[-1], q_rect, loc_est)
# Real test set
for image_id in dataset_real.image_ids:
print('Image ID:', image_id)
image = dataset_real.load_image(image_id)
info = dataset_real.image_info[image_id]
results = model.detect([image], verbose=1)
# Retrieve location
if model.config.REGRESS_LOC:
loc_est = results[0]['loc']
else:
loc_pmf = utils.stable_softmax(results[0]['loc'])
# Compute location mean according to first moment
loc_est = np.asmatrix(loc_pmf) * np.asmatrix(
dataset_real.histogram_3D_map)
# Retrieve orientation
if model.config.REGRESS_ORI:
if model.config.ORIENTATION_PARAM == 'quaternion':
q_est = results[0]['ori']
elif model.config.ORIENTATION_PARAM == 'euler_angles':
q_est = se3lib.SO32quat(
se3lib.euler2SO3_left(results[0]['ori'][0],
results[0]['ori'][1],
results[0]['ori'][2]))
elif model.config.ORIENTATION_PARAM == 'angle_axis':
theta = np.linalg.norm(results[0]['ori'])
if theta < 1e-6:
v = [0, 0, 0]
else:
v = results[0]['ori'] / theta
q_est = se3lib.angleaxis2quat(v, theta)
else:
ori_pmf = utils.stable_softmax(results[0]['ori'])
# Compute mean quaternion
q_est, q_est_cov = se3lib.quat_weighted_avg(
dataset_real.ori_histogram_map, ori_pmf)
# Change quaternion order
q_rect = [q_est[3], q_est[0], q_est[1], q_est[2]]
submission.append_real_test(info['path'].split('/')[-1], q_rect,
loc_est)
submission.export(suffix='debug')
print('Submission exported.')
def evaluate(model, dataset):
""" Evaluates model on all dataset images. Assumes all images have corresponding pose labels.
"""
loc_err_acc = []
loc_encoded_err_acc = []
ori_err_acc = []
ori_encoded_err_acc = []
distances_acc = []
esa_scores_acc = []
# Variance used only for prob. orientation estimation
delta = model.config.BETA / model.config.ORI_BINS_PER_DIM
var = delta**2 / 12
for image_id in dataset.image_ids:
print('Image ID:', image_id)
# Load pose in all formats
loc_gt = dataset.load_location(image_id)
q_gt = dataset.load_quaternion(image_id)
image = dataset.load_image(image_id)
results = model.detect([image], verbose=1)
if model.config.REGRESS_KEYPOINTS:
# Experimental
I, I_meta, loc_gt, k1_gt, k2_gt = \
net.load_image_gt(dataset, model.config, image_id)
loc_est = results[0]['loc']
k1_est = results[0]['k1']
k2_est = results[0]['k2']
# Prepare keypoint matches
# TODO: take scale into account and get rid of magic numbers
P1 = np.zeros((3, 3))
P1[2, 0] = 3.0
P1[1, 1] = 3.0
P2 = np.zeros((3, 3))
P2[:, 0] = k1_est
P2[:, 1] = k2_est
P2[:, 2] = loc_est
t, R = se3lib.pose_3Dto3D(np.asmatrix(P1), np.asmatrix(P2))
q_est = se3lib.SO32quat(R.T)
else:
I, I_meta, loc_encoded_gt, ori_encoded_gt = \
net.load_image_gt(dataset, model.config, image_id)
# Retrieve location
if model.config.REGRESS_LOC:
loc_est = results[0]['loc']
else:
loc_pmf = utils.stable_softmax(results[0]['loc'])
# Compute location mean according to first moment
loc_est = np.asmatrix(loc_pmf) * np.asmatrix(
dataset.histogram_3D_map)
# Compute loc encoding error
loc_decoded_gt = np.asmatrix(loc_encoded_gt) * np.asmatrix(
dataset.histogram_3D_map)
loc_encoded_err = np.linalg.norm(loc_decoded_gt - loc_gt)
loc_encoded_err_acc.append(loc_encoded_err)
# Retrieve orientation
if model.config.REGRESS_ORI:
if model.config.ORIENTATION_PARAM == 'quaternion':
q_est = results[0]['ori']
elif model.config.ORIENTATION_PARAM == 'euler_angles':
q_est = se3lib.SO32quat(
se3lib.euler2SO3_left(results[0]['ori'][0],
results[0]['ori'][1],
results[0]['ori'][2]))
elif model.config.ORIENTATION_PARAM == 'angle_axis':
theta = np.linalg.norm(results[0]['ori'])
if theta < 1e-6:
v = [0, 0, 0]
else:
v = results[0]['ori'] / theta
q_est = se3lib.angleaxis2quat(v, theta)
else:
ori_pmf = utils.stable_softmax(results[0]['ori'])
# Compute mean quaternion
q_est, q_est_cov = se3lib.quat_weighted_avg(
dataset.ori_histogram_map, ori_pmf)
# Multimodal estimation
# Uncomment this block to try the EM framework
# nr_EM_iterations = 5
# Q_mean, Q_var, Q_priors, model_scores = fit_GMM_to_orientation(dataset.ori_histogram_map, ori_pmf,
# nr_EM_iterations, var)
#
# print('Err:', angular_err)
# angular_err = 2*np.arccos(np.abs(np.asmatrix(Q_mean)*np.asmatrix(q_gt).transpose()))*180/np.pi
#
# # Select best of two
# if len(angular_err) == 1 or angular_err[0]<angular_err[1]:
# q_est = Q_mean[0, :]
# else:
# q_est = Q_mean[1, :]
#
# print('Err:',angular_err)
# Compute encoded error
q_encoded_gt, _ = se3lib.quat_weighted_avg(
dataset.ori_histogram_map, ori_encoded_gt)
ori_encoded_err = 2 * np.arccos(
np.abs(
np.asmatrix(q_encoded_gt) *
np.asmatrix(q_gt).transpose())) * 180 / np.pi
ori_encoded_err_acc.append(ori_encoded_err)
# 3. Angular error
angular_err = 2 * np.arccos(
np.abs(np.asmatrix(q_est) *
np.asmatrix(q_gt).transpose())) * 180 / np.pi
ori_err_acc.append(angular_err.item(0))
# 4. Loc error
loc_err = np.linalg.norm(loc_est - loc_gt)
loc_err_acc.append(loc_err)
print('Loc Error: ', loc_err)
print('Ori Error: ', angular_err)
# Compute ESA score
esa_score = loc_err / np.linalg.norm(loc_gt) + 2 * np.arccos(
np.abs(np.asmatrix(q_est) * np.asmatrix(q_gt).transpose()))
esa_scores_acc.append(esa_score)
# Store depth
distances_acc.append(loc_gt[2])
print('Mean est. location error: ', np.mean(loc_err_acc))
print('Mean est. orientation error: ', np.mean(ori_err_acc))
print('ESA score: ', np.mean(esa_scores_acc))
print('Mean encoded location error: ', np.mean(loc_encoded_err_acc))
# Dump results
pd.DataFrame(np.asarray(ori_err_acc)).to_csv("ori_err.csv")
pd.DataFrame(np.asarray(loc_err_acc)).to_csv("loc_err.csv")
pd.DataFrame(np.asarray(distances_acc)).to_csv("dists_err.csv")
def evaluate_image(model, dataset, image_id):
# Load pose in all formats
loc_gt = dataset.load_location(image_id)
q_gt = dataset.load_quaternion(image_id)
image = dataset.load_image(image_id)
I, I_meta, loc_encoded_gt, ori_encoded_gt = \
net.load_image_gt(dataset, model.config, image_id)
results = model.detect([image], verbose=1)
# Retrieve location
if model.config.REGRESS_LOC:
loc_est = results[0]['loc']
else:
loc_pmf = utils.stable_softmax(results[0]['loc'])
# Compute location mean according to first moment
loc_est = np.asmatrix(loc_pmf) * np.asmatrix(dataset.histogram_3D_map)
# Compute loc encoding error
loc_decoded_gt = np.asmatrix(loc_encoded_gt) * np.asmatrix(
dataset.histogram_3D_map)
loc_encoded_err = np.linalg.norm(loc_decoded_gt - loc_gt)
# Retrieve orientation
if model.config.REGRESS_ORI:
if model.config.ORIENTATION_PARAM == 'quaternion':
q_est = results[0]['ori']
elif model.config.ORIENTATION_PARAM == 'euler_angles':
q_est = se3lib.SO32quat(
se3lib.euler2SO3_left(results[0]['ori'][0],
results[0]['ori'][1],
results[0]['ori'][2]))
elif model.config.ORIENTATION_PARAM == 'angle_axis':
theta = np.linalg.norm(results[0]['ori'])
if theta < 1e-6:
v = [0, 0, 0]
else:
v = results[0]['ori'] / theta
q_est = se3lib.angleaxis2quat(v, theta)
else:
ori_pmf = utils.stable_softmax(results[0]['ori'])
# Compute mean quaternion
q_est, q_est_cov = se3lib.quat_weighted_avg(dataset.ori_histogram_map,
ori_pmf)
# Compute encoded error
q_encoded_gt, _ = se3lib.quat_weighted_avg(dataset.ori_histogram_map,
ori_encoded_gt)
ori_encoded_err = 2 * np.arccos(
np.abs(np.asmatrix(q_encoded_gt) *
np.asmatrix(q_gt).transpose())) * 180 / np.pi
# Compute errors
angular_err = 2 * np.arccos(
np.abs(np.asmatrix(q_est) * np.asmatrix(q_gt).transpose()))
# angular_err_in_deg = angular_err* 180 / np.pi
loc_err = np.linalg.norm(loc_est - loc_gt)
loc_rel_err = loc_err / np.linalg.norm(loc_gt)
# Compute ESA score
esa_score = loc_rel_err + angular_err
return loc_err, angular_err, loc_rel_err, esa_score