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(' ')