def RT_transform(pose_src, r, t, T_means, T_stds, rot_coord="MODEL"):
# r: 4(quat) or 3(euler) number
# t: 3 number, (delta_x, delta_y, scale)
r = np.squeeze(r)
if r.shape[0] == 3:
Rm_delta = euler2mat(r[0], r[1], r[2])
elif r.shape[0] == 4:
# QUAT
quat = r / LA.norm(r)
Rm_delta = quat2mat(quat)
else:
raise Exception("Unknown r shape: {}".format(r.shape))
t_delta = np.squeeze(t)
if rot_coord.lower() == "naive":
se3_mx = np.zeros((3, 4))
se3_mx[:, :3] = Rm_delta
se3_mx[:, 3] = t
pose_est = se3_mul(se3_mx, pose_src)
else:
pose_est = np.zeros((3, 4))
pose_est[:3, :3] = R_transform(pose_src[:3, :3], Rm_delta, rot_coord)
pose_est[:3, 3] = T_transform(pose_src[:, 3], t_delta, T_means, T_stds,
rot_coord)
return pose_est
def calc_RT_delta(
pose_src, pose_tgt, T_means, T_stds, rot_coord="MODEL", rot_type="MATRIX"
):
"""
project the points in source corrd to target corrd
:param pose_src: pose matrix of soucre, [R|T], 3x4
:param pose_tgt: pose matrix of target, [R|T], 3x4
:param rot_coord: model/camera
:param rot_type: quat/euler/matrix
:return: Rm_delta
:return: T_delta
"""
if rot_coord.lower() == "naive":
se3_src2tgt = se3_mul(pose_tgt, se3_inverse(pose_src))
Rm_delta = se3_src2tgt[:, :3]
T_delta = se3_src2tgt[:, 3].reshape((3))
else:
Rm_delta = R_inv_transform(pose_src[:3, :3], pose_tgt[:3, :3], rot_coord)
T_delta = T_inv_transform(
pose_src[:, 3], pose_tgt[:, 3], T_means, T_stds, rot_coord
)
if rot_type.lower() == "quat":
r = mat2quat(Rm_delta)
elif rot_type.lower() == "euler":
r = mat2euler(Rm_delta)
elif rot_type.lower() == "matrix":
r = Rm_delta
else:
raise Exception("Unknown rot_type: {}".format(rot_type))
t = np.squeeze(T_delta)
return r, t
def get_point_cloud(pairdb,
config,
scale_ind_list,
X_list=None,
phase="train"):
assert config.TRAIN.MASK_SYN is False, "NOT IMPLEMENTED"
from lib.utils.projection import backproject_camera, se3_inverse, se3_mul
num_pairs = len(pairdb)
X_obj_tensor = []
X_obj_weights_tensor = []
for batch_idx in range(num_pairs):
pair_rec = pairdb[batch_idx]
if "depth_gt_observed" in pair_rec:
depth_observed_raw = cv2.imread(pair_rec["depth_gt_observed"],
cv2.IMREAD_UNCHANGED).astype(
np.float32)
else:
depth_observed_raw = cv2.imread(pair_rec["depth_observed"],
cv2.IMREAD_UNCHANGED).astype(
np.float32)
depth_observed_raw /= config.dataset.DEPTH_FACTOR
# needs to be checked !!!
if "mask_gt_observed" in pair_rec and config.network.MASK_INPUTS:
mask_observed_path = pair_rec["mask_gt_observed"]
assert os.path.exists(
mask_observed_path), "{} does not exist".format(
pair_rec["mask_observed"])
mask_observed = cv2.imread(mask_observed_path,
cv2.IMREAD_UNCHANGED)
depth_observed = np.zeros(depth_observed_raw.shape)
depth_observed[mask_observed ==
pair_rec["mask_idx"]] = depth_observed_raw[
mask_observed == pair_rec["mask_idx"]]
else:
depth_observed = depth_observed_raw
if X_list:
X = X_list[batch_idx]
else:
X = backproject_camera(
depth_observed,
intrinsic_matrix=config.dataset.INTRINSIC_MATRIX)
transform_r2i = se3_mul(pair_rec["pose_rendered"],
se3_inverse(pair_rec["pose_observed"]))
X_obj = np.matmul(
transform_r2i,
np.append(X, np.ones([1, X.shape[1]], dtype=np.float32),
axis=0)).reshape((1, 3, depth_observed.shape[0],
depth_observed.shape[1]))
X_obj_weights = (depth_observed != 0).astype(np.float32)
X_obj_weights = np.tile(X_obj_weights[np.newaxis, np.newaxis, :, :],
(1, 3, 1, 1))
# X_obj_weights = X_obj_weights[np.newaxis, np.newaxis, :, :]
X_obj_tensor.append(X_obj)
X_obj_weights_tensor.append(X_obj_weights)
return X_obj_tensor, X_obj_weights_tensor
def calc_se3(pose_src, pose_tgt):
"""
project the points in source corrd to target corrd
:param pose_src: pose matrix of soucre, [R|T], 3x4
:param pose_tgt: pose matrix of target, [R|T], 3x4
:return: visible: whether points in source can be viewed in target
"""
se3_src2tgt = se3_mul(pose_tgt, se3_inverse(pose_src))
rotm = se3_src2tgt[:, :3]
t = se3_src2tgt[:, 3].reshape((3))
return rotm, t
def evaluate_pose_arp_2d(self, config, all_poses_est, all_poses_gt,
output_dir):
"""
evaluate average re-projection 2d error
:param config:
:param all_poses_est:
:param all_poses_gt:
:param output_dir:
:param logger:
:return:
"""
logger.info("evaluating pose average re-projection 2d error")
num_iter = config.TEST.test_iter
K = config.dataset.INTRINSIC_MATRIX
count_all = np.zeros((self.num_classes, ), dtype=np.float32)
count_correct = {
k: np.zeros((self.num_classes, num_iter), dtype=np.float32)
for k in ["2", "5", "10", "20"]
}
threshold_2 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
threshold_5 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
threshold_10 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
threshold_20 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
dx = 0.1
threshold_mean = np.tile(
np.arange(0, 50, dx).astype(np.float32),
(self.num_classes, num_iter,
1)) # (num_class, num_iter, num_thresh)
num_thresh = threshold_mean.shape[-1]
count_correct["mean"] = np.zeros(
(self.num_classes, num_iter, num_thresh), dtype=np.float32)
for i in range(self.num_classes):
threshold_2[i, :] = 2
threshold_5[i, :] = 5
threshold_10[i, :] = 10
threshold_20[i, :] = 20
num_valid_class = 0
for cls_idx, cls_name in enumerate(self.classes):
if not (all_poses_est[cls_idx][0] and all_poses_gt[cls_idx][0]):
continue
num_valid_class += 1
for iter_i in range(num_iter):
curr_poses_gt = all_poses_gt[cls_idx][0]
num = len(curr_poses_gt)
curr_poses_est = all_poses_est[cls_idx][iter_i]
for j in range(num):
if iter_i == 0:
count_all[cls_idx] += 1
RT = curr_poses_est[j] # est pose
pose_gt = curr_poses_gt[j] # gt pose
error_rotation = re(RT[:3, :3], pose_gt[:3, :3])
if cls_name == "eggbox" and error_rotation > 90:
RT_z = np.array([[-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 1, 0]])
RT_sym = se3_mul(RT, RT_z)
error = arp_2d(RT_sym[:3, :3], RT_sym[:, 3],
pose_gt[:3, :3], pose_gt[:, 3],
self._points[cls_name], K)
else:
error = arp_2d(RT[:3, :3], RT[:, 3], pose_gt[:3, :3],
pose_gt[:,
3], self._points[cls_name], K)
if error < threshold_2[cls_idx, iter_i]:
count_correct["2"][cls_idx, iter_i] += 1
if error < threshold_5[cls_idx, iter_i]:
count_correct["5"][cls_idx, iter_i] += 1
if error < threshold_10[cls_idx, iter_i]:
count_correct["10"][cls_idx, iter_i] += 1
if error < threshold_20[cls_idx, iter_i]:
count_correct["20"][cls_idx, iter_i] += 1
for thresh_i in range(num_thresh):
if error < threshold_mean[cls_idx, iter_i, thresh_i]:
count_correct["mean"][cls_idx, iter_i,
thresh_i] += 1
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
# store plot data
plot_data = {}
sum_acc_mean = np.zeros(num_iter)
sum_acc_02 = np.zeros(num_iter)
sum_acc_05 = np.zeros(num_iter)
sum_acc_10 = np.zeros(num_iter)
sum_acc_20 = np.zeros(num_iter)
for cls_idx, cls_name in enumerate(self.classes):
if count_all[cls_idx] == 0:
continue
plot_data[cls_name] = []
for iter_i in range(num_iter):
logger.info("** {}, iter {} **".format(cls_name, iter_i + 1))
from scipy.integrate import simps
area = simps(count_correct["mean"][cls_idx, iter_i] /
float(count_all[cls_idx]),
dx=dx) / (50.0)
acc_mean = area * 100
sum_acc_mean[iter_i] += acc_mean
acc_02 = 100 * float(count_correct["2"][cls_idx,
iter_i]) / float(
count_all[cls_idx])
sum_acc_02[iter_i] += acc_02
acc_05 = 100 * float(count_correct["5"][cls_idx,
iter_i]) / float(
count_all[cls_idx])
sum_acc_05[iter_i] += acc_05
acc_10 = 100 * float(
count_correct["10"][cls_idx, iter_i]) / float(
count_all[cls_idx])
sum_acc_10[iter_i] += acc_10
acc_20 = 100 * float(
count_correct["20"][cls_idx, iter_i]) / float(
count_all[cls_idx])
sum_acc_20[iter_i] += acc_20
fig = plt.figure()
x_s = np.arange(0, 50, dx).astype(np.float32)
y_s = 100 * count_correct["mean"][cls_idx, iter_i] / float(
count_all[cls_idx])
plot_data[cls_name].append((x_s, y_s))
plt.plot(x_s, y_s, "-")
plt.xlim(0, 50)
plt.ylim(0, 100)
plt.grid(True)
plt.xlabel("px")
plt.ylabel("correctly estimated poses in %")
plt.savefig(os.path.join(
output_dir,
"arp_2d_{}_iter{}.png".format(cls_name, iter_i + 1)),
dpi=fig.dpi)
logger.info("threshold=[0, 50], area: {:.2f}".format(acc_mean))
logger.info(
"threshold=2, correct poses: {}, all poses: {}, accuracy: {:.2f}"
.format(count_correct["2"][cls_idx, iter_i],
count_all[cls_idx], acc_02))
logger.info(
"threshold=5, correct poses: {}, all poses: {}, accuracy: {:.2f}"
.format(count_correct["5"][cls_idx, iter_i],
count_all[cls_idx], acc_05))
logger.info(
"threshold=10, correct poses: {}, all poses: {}, accuracy: {:.2f}"
.format(count_correct["10"][cls_idx, iter_i],
count_all[cls_idx], acc_10))
logger.info(
"threshold=20, correct poses: {}, all poses: {}, accuracy: {:.2f}"
.format(count_correct["20"][cls_idx, iter_i],
count_all[cls_idx], acc_20))
logger.info(" ")
with open(os.path.join(output_dir, "arp_2d_xys.pkl"), "wb") as f:
cPickle.dump(plot_data, f, protocol=2)
logger.info("=" * 30)
print(" ")
# overall performance of arp 2d
for iter_i in range(num_iter):
logger.info(
"---------- arp 2d performance over {} classes -----------".
format(num_valid_class))
logger.info("** iter {} **".format(iter_i + 1))
logger.info("threshold=[0, 50], area: {:.2f}".format(
sum_acc_mean[iter_i] / num_valid_class))
logger.info("threshold=2, mean accuracy: {:.2f}".format(
sum_acc_02[iter_i] / num_valid_class))
logger.info("threshold=5, mean accuracy: {:.2f}".format(
sum_acc_05[iter_i] / num_valid_class))
logger.info("threshold=10, mean accuracy: {:.2f}".format(
sum_acc_10[iter_i] / num_valid_class))
logger.info("threshold=20, mean accuracy: {:.2f}".format(
sum_acc_20[iter_i] / num_valid_class))
logger.info(" ")
logger.info("=" * 30)
def evaluate_pose(self, config, all_poses_est, all_poses_gt):
# evaluate and display
logger.info("evaluating pose")
rot_thresh_list = np.arange(1, 11, 1)
trans_thresh_list = np.arange(0.01, 0.11, 0.01)
num_metric = len(rot_thresh_list)
num_iter = config.TEST.test_iter
rot_acc = np.zeros((self.num_classes, num_iter, num_metric))
trans_acc = np.zeros((self.num_classes, num_iter, num_metric))
space_acc = np.zeros((self.num_classes, num_iter, num_metric))
num_valid_class = 0
for cls_idx, cls_name in enumerate(self.classes):
if not (all_poses_est[cls_idx][0] and all_poses_gt[cls_idx][0]):
continue
num_valid_class += 1
for iter_i in range(num_iter):
curr_poses_gt = all_poses_gt[cls_idx][0]
num = len(curr_poses_gt)
curr_poses_est = all_poses_est[cls_idx][iter_i]
cur_rot_rst = np.zeros((num, 1))
cur_trans_rst = np.zeros((num, 1))
for j in range(num):
r_dist_est, t_dist_est = calc_rt_dist_m(
curr_poses_est[j], curr_poses_gt[j])
if cls_name == "eggbox" and r_dist_est > 90:
RT_z = np.array([[-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 1, 0]])
curr_pose_est_sym = se3_mul(curr_poses_est[j], RT_z)
r_dist_est, t_dist_est = calc_rt_dist_m(
curr_pose_est_sym, curr_poses_gt[j])
cur_rot_rst[j, 0] = r_dist_est
cur_trans_rst[j, 0] = t_dist_est
for thresh_idx in range(num_metric):
rot_acc[cls_idx, iter_i, thresh_idx] = np.mean(
cur_rot_rst < rot_thresh_list[thresh_idx])
trans_acc[cls_idx, iter_i, thresh_idx] = np.mean(
cur_trans_rst < trans_thresh_list[thresh_idx])
space_acc[cls_idx, iter_i, thresh_idx] = np.mean(
np.logical_and(
cur_rot_rst < rot_thresh_list[thresh_idx],
cur_trans_rst < trans_thresh_list[thresh_idx]))
show_list = [1, 4, 9]
logger.info("------------ {} -----------".format(cls_name))
logger.info("{:>24}: {:>7}, {:>7}, {:>7}".format(
"[rot_thresh, trans_thresh", "RotAcc", "TraAcc", "SpcAcc"))
for iter_i in range(num_iter):
logger.info("** iter {} **".format(iter_i + 1))
logger.info("{:<16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
"average_accuracy",
"[{:>2}, {:>4}]".format(-1, -1),
np.mean(rot_acc[cls_idx, iter_i, :]) * 100,
np.mean(trans_acc[cls_idx, iter_i, :]) * 100,
np.mean(space_acc[cls_idx, iter_i, :]) * 100,
))
for i, show_idx in enumerate(show_list):
logger.info(
"{:>16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
"average_accuracy",
"[{:>2}, {:>4}]".format(
rot_thresh_list[show_idx],
trans_thresh_list[show_idx]),
rot_acc[cls_idx, iter_i, show_idx] * 100,
trans_acc[cls_idx, iter_i, show_idx] * 100,
space_acc[cls_idx, iter_i, show_idx] * 100,
))
print(" ")
# overall performance
for iter_i in range(num_iter):
show_list = [1, 4, 9]
logger.info(
"---------- performance over {} classes -----------".format(
num_valid_class))
logger.info("** iter {} **".format(iter_i + 1))
logger.info("{:>24}: {:>7}, {:>7}, {:>7}".format(
"[rot_thresh, trans_thresh", "RotAcc", "TraAcc", "SpcAcc"))
logger.info("{:<16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
"average_accuracy",
"[{:>2}, {:>4}]".format(-1, -1),
np.sum(rot_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100,
np.sum(trans_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100,
np.sum(space_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100,
))
for i, show_idx in enumerate(show_list):
logger.info("{:>16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
"average_accuracy",
"[{:>2}, {:>4}]".format(rot_thresh_list[show_idx],
trans_thresh_list[show_idx]),
np.sum(rot_acc[:, iter_i, show_idx]) / num_valid_class *
100,
np.sum(trans_acc[:, iter_i, show_idx]) / num_valid_class *
100,
np.sum(space_acc[:, iter_i, show_idx]) / num_valid_class *
100,
))
print(" ")
def calc_flow(depth_src,
pose_src,
pose_tgt,
K,
depth_tgt,
thresh=3E-3,
standard_rep=False):
"""
project the points in source corrd to target corrd
:param standard_rep:
:param depth_src: depth image of source(m)
:param pose_src: pose matrix of soucre, [R|T], 3x4
:param depth_tgt: depth image of target
:param pose_tgt: pose matrix of target, [R|T], 3x4
:param K: intrinsic_matrix
:param depth_tgt: depth image of target(m)
:return: visible: whether points in source can be viewed in target
:return: flow: flow from source to target
"""
height = depth_src.shape[0]
width = depth_src.shape[1]
visible = np.zeros(depth_src.shape[:2]).flatten()
X = backproject_camera(depth_src, intrinsic_matrix=K)
transform = np.matmul(K, se3_mul(pose_tgt, se3_inverse(pose_src)))
Xp = np.matmul(
transform,
np.append(X, np.ones([1, X.shape[1]], dtype=np.float32), axis=0))
pz = Xp[2] + 1E-15
pw = Xp[0] / pz
ph = Xp[1] / pz
valid_points = np.where(depth_src.flatten() != 0)[0]
depth_proj_valid = pz[valid_points]
pw_valid_raw = np.round(pw[valid_points]).astype(int)
pw_valid = np.minimum(np.maximum(pw_valid_raw, 0), width - 1)
ph_valid_raw = np.round(ph[valid_points]).astype(int)
ph_valid = np.minimum(np.maximum(ph_valid_raw, 0), height - 1)
p_within = np.logical_and(
np.logical_and(pw_valid_raw >= 0, pw_valid_raw < width),
np.logical_and(ph_valid_raw >= 0, ph_valid_raw < height))
depth_tgt_valid = depth_tgt[ph_valid, pw_valid]
p_within = np.logical_and(
p_within,
np.abs(depth_tgt_valid - depth_proj_valid) < thresh)
p_valid = np.abs(depth_tgt_valid) > 1E-10
fg_points = valid_points[np.logical_and(p_within, p_valid)]
visible[fg_points] = 1
visible = visible.reshape(depth_src.shape[:2])
w_ori, h_ori = np.meshgrid(np.linspace(0, width - 1, width),
np.linspace(0, height - 1, height))
if standard_rep:
flow = np.dstack([
pw.reshape(depth_src.shape[:2]) - w_ori,
ph.reshape(depth_src.shape[:2]) - h_ori
])
else:
# depleted version, only used in old code
flow = np.dstack([
ph.reshape(depth_src.shape[:2]) - h_ori,
pw.reshape(depth_src.shape[:2]) - w_ori
])
flow[np.dstack([visible, visible]) != 1] = 0
assert np.isnan(flow).sum() == 0
X_valid = np.array([c[np.where(visible.flatten())] for c in X])
return flow, visible, X_valid
def evaluate_pose_arp_2d(self, config, all_poses_est, all_poses_gt,
output_dir, logger):
'''
evaluate average re-projection 2d error
:param config:
:param all_poses_est:
:param all_poses_gt:
:param output_dir:
:param logger:
:return:
'''
print_and_log('evaluating pose average re-projection 2d error', logger)
if config.TEST.AVERAGE_ITERS and config.TEST.test_iter >= 4:
print_and_log('average last 2 and 4 iters', logger)
num_iter = config.TEST.test_iter + 2
else:
num_iter = config.TEST.test_iter
K = config.dataset.INTRINSIC_MATRIX
count_all = np.zeros((self.num_classes, ), dtype=np.float32)
count_correct = {
k: np.zeros((self.num_classes, num_iter), dtype=np.float32)
for k in ['5', '10', '20']
}
threshold_5 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
threshold_10 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
threshold_20 = np.zeros((self.num_classes, num_iter), dtype=np.float32)
dx = 0.1
threshold_mean = np.tile(
np.arange(0, 50, dx).astype(np.float32),
(self.num_classes, num_iter,
1)) # (num_class, num_iter, num_thresh)
num_thresh = threshold_mean.shape[-1]
count_correct['mean'] = np.zeros(
(self.num_classes, num_iter, num_thresh), dtype=np.float32)
for i in xrange(self.num_classes):
threshold_5[i, :] = 5
threshold_10[i, :] = 10
threshold_20[i, :] = 20
num_valid_class = 0
for cls_idx, cls_name in enumerate(self.classes):
if not (all_poses_est[cls_idx][0] and all_poses_gt[cls_idx][0]):
continue
num_valid_class += 1
for iter_i in range(num_iter):
curr_poses_gt = all_poses_gt[cls_idx][0]
num = len(curr_poses_gt)
if config.TEST.AVERAGE_ITERS and config.TEST.test_iter >= 4:
if iter_i == num_iter - 2:
curr_poses_est = [
0.5 * (all_poses_est[cls_idx][iter_i - 1][j] +
all_poses_est[cls_idx][iter_i - 2][j])
for j in range(num)
]
elif iter_i == num_iter - 1:
curr_poses_est = [
0.25 * (all_poses_est[cls_idx][iter_i - 2][j] +
all_poses_est[cls_idx][iter_i - 3][j] +
all_poses_est[cls_idx][iter_i - 4][j] +
all_poses_est[cls_idx][iter_i - 5][j])
for j in range(num)
]
else:
curr_poses_est = all_poses_est[cls_idx][iter_i]
else:
curr_poses_est = all_poses_est[cls_idx][iter_i]
for j in xrange(num):
if iter_i == 0:
count_all[cls_idx] += 1
RT = curr_poses_est[j] # est pose
pose_gt = curr_poses_gt[j] # gt pose
error_rotation = re(RT[:3, :3], pose_gt[:3, :3])
if cls_name == 'eggbox' and error_rotation > 90:
RT_z = np.array([[-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 1, 0]])
RT_sym = se3_mul(RT, RT_z)
error = arp_2d(RT_sym[:3, :3], RT_sym[:, 3],
pose_gt[:3, :3], pose_gt[:, 3],
self._points[cls_name], K)
else:
error = arp_2d(RT[:3, :3], RT[:, 3], pose_gt[:3, :3],
pose_gt[:,
3], self._points[cls_name], K)
if error < threshold_5[cls_idx, iter_i]:
count_correct['5'][cls_idx, iter_i] += 1
if error < threshold_10[cls_idx, iter_i]:
count_correct['10'][cls_idx, iter_i] += 1
if error < threshold_20[cls_idx, iter_i]:
count_correct['20'][cls_idx, iter_i] += 1
for thresh_i in xrange(num_thresh):
if error < threshold_mean[cls_idx, iter_i, thresh_i]:
count_correct['mean'][cls_idx, iter_i,
thresh_i] += 1
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
# store plot data
plot_data = {}
for cls_idx, cls_name in enumerate(self.classes):
if count_all[cls_idx] == 0:
continue
plot_data[cls_name] = []
for iter_i in range(num_iter):
print_and_log("** {}, iter {} **".format(cls_name, iter_i + 1),
logger)
from scipy.integrate import simps
area = simps(count_correct['mean'][cls_idx, iter_i] /
float(count_all[cls_idx]),
dx=dx) / (50.0)
fig = plt.figure()
x_s = np.arange(0, 50, dx).astype(np.float32)
y_s = 100 * count_correct['mean'][cls_idx, iter_i] / float(
count_all[cls_idx])
plot_data[cls_name].append((x_s, y_s))
plt.plot(x_s, y_s, '-')
plt.xlim(0, 50)
plt.ylim(0, 100)
plt.grid(True)
plt.xlabel("px")
plt.ylabel("correctly estimated poses in %")
plt.savefig(os.path.join(
output_dir,
'arp_2d_{}_iter{}.png'.format(cls_name, iter_i + 1)),
dpi=fig.dpi)
print_and_log(
'threshold=[0, 50], area: {:.2f}'.format(area * 100),
logger)
print_and_log(
'threshold=5, correct poses: {}, all poses: {}, accuracy: {:.2f}'
.format(
count_correct['5'][cls_idx,
iter_i], count_all[cls_idx],
100 * float(count_correct['5'][cls_idx, iter_i]) /
float(count_all[cls_idx])), logger)
print_and_log(
'threshold=10, correct poses: {}, all poses: {}, accuracy: {:.2f}'
.format(
count_correct['10'][cls_idx,
iter_i], count_all[cls_idx],
100 * float(count_correct['10'][cls_idx, iter_i]) /
float(count_all[cls_idx])), logger)
print_and_log(
'threshold=20, correct poses: {}, all poses: {}, accuracy: {:.2f}'
.format(
count_correct['20'][cls_idx,
iter_i], count_all[cls_idx],
100 * float(count_correct['20'][cls_idx, iter_i]) /
float(count_all[cls_idx])), logger)
print_and_log(" ", logger)
with open(os.path.join(output_dir, 'arp_2d_xys.pkl'), 'wb') as f:
cPickle.dump(plot_data, f, protocol=2)
print_and_log("=" * 30, logger)
def evaluate_pose(self, config, all_poses_est, all_poses_gt, logger):
# evaluate and display
print_and_log('evaluating pose', logger)
rot_thresh_list = np.arange(1, 11, 1)
trans_thresh_list = np.arange(0.01, 0.11, 0.01)
num_metric = len(rot_thresh_list)
if config.TEST.AVERAGE_ITERS and config.TEST.test_iter >= 4:
print_and_log('average last 2 and 4 iters', logger)
num_iter = config.TEST.test_iter + 2
else:
num_iter = config.TEST.test_iter
rot_acc = np.zeros((self.num_classes, num_iter, num_metric))
trans_acc = np.zeros((self.num_classes, num_iter, num_metric))
space_acc = np.zeros((self.num_classes, num_iter, num_metric))
num_valid_class = 0
for cls_idx, cls_name in enumerate(self.classes):
if not (all_poses_est[cls_idx][0] and all_poses_gt[cls_idx][0]):
continue
num_valid_class += 1
for iter_i in range(num_iter):
curr_poses_gt = all_poses_gt[cls_idx][0]
num = len(curr_poses_gt)
if config.TEST.AVERAGE_ITERS and config.TEST.test_iter >= 4:
if iter_i == num_iter - 2:
curr_poses_est = [
0.5 * (all_poses_est[cls_idx][iter_i - 1][j] +
all_poses_est[cls_idx][iter_i - 2][j])
for j in range(num)
]
elif iter_i == num_iter - 1:
curr_poses_est = [
0.25 * (all_poses_est[cls_idx][iter_i - 2][j] +
all_poses_est[cls_idx][iter_i - 3][j] +
all_poses_est[cls_idx][iter_i - 4][j] +
all_poses_est[cls_idx][iter_i - 5][j])
for j in range(num)
]
else:
curr_poses_est = all_poses_est[cls_idx][iter_i]
else:
curr_poses_est = all_poses_est[cls_idx][iter_i]
cur_rot_rst = np.zeros((num, 1))
cur_trans_rst = np.zeros((num, 1))
for j in range(num):
r_dist_est, t_dist_est = calc_rt_dist_m(
curr_poses_est[j], curr_poses_gt[j])
if cls_name == 'eggbox' and r_dist_est > 90:
RT_z = np.array([[-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 1, 0]])
curr_pose_est_sym = se3_mul(curr_poses_est[j], RT_z)
r_dist_est, t_dist_est = calc_rt_dist_m(
curr_pose_est_sym, curr_poses_gt[j])
print('eggbox r_dist_est after symmetry: {}'.format(
r_dist_est))
cur_rot_rst[j, 0] = r_dist_est
cur_trans_rst[j, 0] = t_dist_est
# cur_rot_rst = np.vstack(all_rot_err[cls_idx, iter_i])
# cur_trans_rst = np.vstack(all_trans_err[cls_idx, iter_i])
for thresh_idx in range(num_metric):
rot_acc[cls_idx, iter_i, thresh_idx] = np.mean(
cur_rot_rst < rot_thresh_list[thresh_idx])
trans_acc[cls_idx, iter_i, thresh_idx] = np.mean(
cur_trans_rst < trans_thresh_list[thresh_idx])
space_acc[cls_idx, iter_i, thresh_idx] = np.mean(
np.logical_and(
cur_rot_rst < rot_thresh_list[thresh_idx],
cur_trans_rst < trans_thresh_list[thresh_idx]))
show_list = [1, 4, 9]
print_and_log("------------ {} -----------".format(cls_name),
logger)
print_and_log(
"{:>24}: {:>7}, {:>7}, {:>7}".format(
"[rot_thresh, trans_thresh", "RotAcc", "TraAcc", "SpcAcc"),
logger)
for iter_i in range(num_iter):
print_and_log("** iter {} **".format(iter_i + 1), logger)
print_and_log(
"{:<16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
'average_accuracy', '[{:>2}, {:>4}]'.format(-1, -1),
np.mean(rot_acc[cls_idx, iter_i, :]) * 100,
np.mean(trans_acc[cls_idx, iter_i, :]) * 100,
np.mean(space_acc[cls_idx, iter_i, :]) * 100), logger)
for i, show_idx in enumerate(show_list):
print_and_log(
"{:>16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
'average_accuracy', '[{:>2}, {:>4}]'.format(
rot_thresh_list[show_idx],
trans_thresh_list[show_idx]),
rot_acc[cls_idx, iter_i, show_idx] * 100,
trans_acc[cls_idx, iter_i, show_idx] * 100,
space_acc[cls_idx, iter_i, show_idx] * 100),
logger)
print(' ')
# overall performance
for iter_i in range(num_iter):
show_list = [1, 4, 9]
print_and_log(
"---------- performance over {} classes -----------".format(
num_valid_class), logger)
print_and_log("** iter {} **".format(iter_i + 1), logger)
print_and_log(
"{:>24}: {:>7}, {:>7}, {:>7}".format(
"[rot_thresh, trans_thresh", "RotAcc", "TraAcc", "SpcAcc"),
logger)
print_and_log(
"{:<16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
'average_accuracy', '[{:>2}, {:>4}]'.format(-1, -1),
np.sum(rot_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100,
np.sum(trans_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100,
np.sum(space_acc[:, iter_i, :]) /
(num_valid_class * num_metric) * 100), logger)
for i, show_idx in enumerate(show_list):
print_and_log(
"{:>16}{:>8}: {:>7.2f}, {:>7.2f}, {:>7.2f}".format(
'average_accuracy',
'[{:>2}, {:>4}]'.format(rot_thresh_list[show_idx],
trans_thresh_list[show_idx]),
np.sum(rot_acc[:, iter_i, show_idx]) /
num_valid_class * 100,
np.sum(trans_acc[:, iter_i, show_idx]) /
num_valid_class * 100,
np.sum(space_acc[:, iter_i, show_idx]) /
num_valid_class * 100), logger)
print(' ')
