def print_trans_rot_errors(gts, obj_id, ts_est, ts_est_old, Rs_est, Rs_est_old): t_errs = [] obj_gts = [] for gt in gts: if gt['obj_id'] == obj_id: t_errs.append(ts_est[0]-gt['cam_t_m2c'].squeeze()) obj_gts.append(gt) min_t_err_idx = np.argmin(np.linalg.norm(np.array(t_errs),axis=1)) print min_t_err_idx print np.array(t_errs).shape print len(obj_gts) gt = obj_gts[min_t_err_idx].copy() try: print 'Translation Error before refinement' print ts_est_old[0]-gt['cam_t_m2c'].squeeze() print 'Translation Error after refinement' print t_errs[min_t_err_idx] print 'Rotation Error before refinement' print pose_error.re(Rs_est_old[0],gt['cam_R_m2c']) print 'Rotation Error after refinement' R_err = pose_error.re(Rs_est[0],gt['cam_R_m2c']) print R_err except: pass return (t_errs[min_t_err_idx], R_err)
def main2(): R_errors = [] for R in range(100000): R_gt = transform.random_rotation_matrix()[:3,:3] R_est = transform.random_rotation_matrix()[:3,:3] R_errors.append(pose_error.re(R_est,R_gt)) plot_R_err_hist2(R_errors,'',bins=90,save=False) azimuth_range = (0, 2 * np.pi) elev_range = (-0.5 * np.pi, 0.5 * np.pi) views, _ = view_sampler.sample_views( 2563, 100, azimuth_range, elev_range ) Rs = [] for view in views: R_errors.append(pose_error.re(view['R'],views[np.random.randint(0,len(views))]['R'])) Rs.append(view['R']) plot_R_err_hist2(R_errors,'',bins=45,save=False) plot_viewsphere_for_embedding(np.array(Rs),'',np.array(R_errors),save=False) plt.show()
def refined_nearest_rotation(self, session, target_view, top_n, R_init=None, t_init=None, budget=10, epochs=3, high=6. / 180 * np.pi, obj_id=0, top_n_refine=1, target_bb=None): from sixd_toolkit.pysixd import transform, pose_error from sklearn.metrics.pairwise import cosine_similarity if target_view.dtype == 'uint8': target_view = target_view / 255. if target_view.ndim == 3: target_view = np.expand_dims(target_view, 0) cosine_similar, orig_in_emb = session.run( [self.cos_similarity, self.normalized_embedding_query], {self._encoder.x: target_view}) if top_n_refine == 1: idcs = np.argmax(cosine_similar, axis=1) # orig_cosine_sim = cosine_similar[0,idcs] else: unsorted_max_idcs = np.argpartition(-cosine_similar.squeeze(), top_n_refine)[:top_n_refine] idcs = unsorted_max_idcs[np.argsort( -cosine_similar.squeeze()[unsorted_max_idcs])] # orig_cosine_sim = cosine_similar[0,idcs[0]] print('original cosine sim: ', cosine_similar[0, idcs]) ### intitializing rotation estimates from existing codebook Rs = self._dataset.viewsphere_for_embedding[idcs].copy() Rs_new = [Rs[0]] for R in Rs: res = [pose_error.re(R_new, R) for R_new in Rs_new] if np.min(res) > 80: Rs_new.append(R) if R_init is None: Rs = Rs_new[:] ###### else: Rs = [R_init] top_n_new = len(Rs) max_cosine_sim = 0.0 K = eval(self._dataset._kw['k']) K = np.array(K).reshape(3, 3) render_dims = eval(self._dataset._kw['render_dims']) clip_near = float(self._dataset._kw['clip_near']) clip_far = float(self._dataset._kw['clip_far']) pad_factor = float(self._dataset._kw['pad_factor']) R_perts = [] fine_views = [] bbs = [] for j in range(epochs): noof_perts = budget * top_n_new for i in range(noof_perts): if j > 0 and i == 0: R_perts = Rs print(noof_perts) # fine_views = list(view_best) if not isinstance(view_best, list) else view_best # bbs = list(bb_best) if not isinstance(bbs, list) else bbs continue if i < top_n_new and j == 0: R_off = np.eye(3, 3) else: rand_direction = transform.make_rand_vector(3) rand_angle = np.random.uniform(0, high / (j + 1)) R_off = transform.rotation_matrix(rand_angle, rand_direction)[:3, :3] R_pert = np.dot(R_off, Rs[i % top_n_new]) R_perts.append(R_pert) if target_bb is not None and t_init is not None: bgr_full, _ = self._dataset.renderer.render( obj_id=obj_id, W=render_dims[0], H=render_dims[1], K=K.copy(), R=R_pert, t=t_init, near=clip_near, far=clip_far, random_light=False) bgr = self._dataset.extract_square_patch( bgr_full, target_bb, pad_factor) obj_bb = np.array([0, 0, 1, 1]) else: bgr, obj_bb = self._dataset.render_rot(R_pert, downSample=1, obj_id=obj_id, return_bb=True) fine_views.append(bgr) bbs.append(obj_bb) float_imgs = session.run(self.image_ph_tofloat, {self._image_ph: np.array(fine_views)}) normalized_embedding_query = session.run( self.normalized_embedding_query, {self._encoder.x: float_imgs}) cosine_sim = cosine_similarity(orig_in_emb, normalized_embedding_query) idx = np.argmax(cosine_sim, axis=1) R_perts = np.array(R_perts) if cosine_sim[0, idx] >= max_cosine_sim: max_cosine_sim = cosine_sim[0, idx] fine_views = np.array(fine_views) bbs = np.array(bbs) unsorted_max_idcs = np.argpartition( -cosine_sim.squeeze(), top_n_refine)[:top_n_refine] idcs = unsorted_max_idcs[np.argsort( -cosine_sim.squeeze()[unsorted_max_idcs])] Rs = R_perts[idcs] view_best = fine_views[idcs[0]] bb_best = bbs[idcs[0]] # if top_n_new > 1:ยด # cv2.imshow('refined',view_best) # cv2.imshow('orig_rendered',fine_views[0]) # cv2.imshow('orig_in',x[0]) # cv2.waitKey(0) view_best_new = [view_best.squeeze()] bb_best_new = [bb_best.squeeze()] ## if more than one neighbor, look for far apart alternatives Rs_new = [Rs[0]] for r, R in enumerate(Rs): res = [pose_error.re(R_new, R) for R_new in Rs_new] if np.min(res) > 80: Rs_new.append(R) view_best_new.append(fine_views[idcs[r]]) bb_best_new.append(bbs[idcs[r]]) Rs = Rs_new[:] fine_views = view_best_new[:] bbs = bb_best_new[:] top_n_new = len(view_best_new) print('refined') # cv2.imshow('chosen', fine_views[idx]) # cv2.waitKey(0) # Rs_best = list(Rs) else: print('not refined') print('final cosine sim: ', max_cosine_sim) # idx = np.argmax(cosine_sim) return np.array(Rs)[0:top_n], bbs[0:top_n]
def eval_calc_errors(eval_args, eval_dir): # Results for which the errors will be calculated #------------------------------------------------------------------------------- # result_base = '/path/to/results/' # result_paths = [ # pjoin(result_base, 'hodan-iros15_hinterstoisser'), # # pjoin(result_base, 'hodan-iros15_tless_primesense'), # ] #[METHOD] method = eval_args.get('METHOD', 'METHOD') #[DATA] dataset = eval_args.get('DATA', 'DATASET') test_type = eval_args.get('DATA', 'CAM_TYPE') #[METRIC] # Top N pose estimates (with the highest score) to be evaluated for each # object in each image n_top = eval_args.getint( 'EVALUATION', 'TOP_N_EVAL' ) # 0 = all estimates, -1 = given by the number of GT poses n_top_str = eval_args.getint('METRIC', 'TOP_N') # Pose error function error_types = eval(eval_args.get( 'METRIC', 'ERROR_TYPE')) # 'vsd', 'adi', 'add', 'cou', 're', 'te' # VSD parameters vsd_delta = eval_args.getint('METRIC', 'VSD_DELTA') vsd_tau = eval_args.getint('METRIC', 'VSD_TAU') vsd_cost = eval_args.get('METRIC', 'VSD_COST') # 'step', 'tlinear' result_path = eval_dir print('Processing: ' + result_path) # Other paths #------------------------------------------------------------------------------- for error_type in error_types: # Mask of path to the output file with calculated errors # errors_mpath = pjoin('{result_path}', '..', '..', 'eval', '{result_name}', # '{error_sign}', 'errors_{scene_id:02d}.yml') errors_mpath = '{result_path}/{error_sign}/errors_{scene_id:02d}.yml' # Error signature error_sign = 'error=' + error_type + '_ntop=' + str(n_top_str) if error_type == 'vsd': error_sign += '_delta={}_tau={}_cost={}'.format( vsd_delta, vsd_tau, vsd_cost) # Error calculation #------------------------------------------------------------------------------- # Select data type if dataset == 'tless': cam_type = test_type if error_type in ['adi', 'add']: model_type = 'cad_subdivided' else: model_type = 'cad' else: model_type = '' cam_type = '' # Load dataset parameters dp = get_dataset_params(dataset, model_type=model_type, test_type=test_type, cam_type=cam_type) # Load object models if error_type in ['vsd', 'add', 'adi', 'cou', 'proj', 'projamb']: print('Loading object models...') models = {} for obj_id in range(1, dp['obj_count'] + 1): models[obj_id] = inout.load_ply( dp['model_mpath'].format(obj_id)) test_sensor = pjoin(dp['base_path'], dp['test_dir']) # Directories with results for individual scenes scene_dirs = sorted([ d for d in glob.glob(os.path.join(result_path, '*')) if os.path.isdir(d) and os.path.basename(d).isdigit() ]) print scene_dirs for scene_dir in scene_dirs: scene_id = int(os.path.basename(scene_dir)) # Load info and GT poses for the current scene scene_info = inout.load_info( dp['scene_info_mpath'].format(scene_id)) scene_gt = inout.load_gt(dp['scene_gt_mpath'].format(scene_id)) res_paths = sorted(glob.glob(os.path.join(scene_dir, '*.yml'))) errs = [] im_id = -1 depth_im = None for res_id, res_path in enumerate(res_paths): # t = time.time() # Parse image ID and object ID from the filename filename = os.path.basename(res_path).split('.')[0] im_id_prev = im_id im_id, obj_id = map(int, filename.split('_')) if res_id % 10 == 0: dataset_str = dataset if test_type != '': dataset_str += ' - {}'.format(test_type) print('Calculating error: {}, {}, {}, {}, {}, {}'.format( error_type, method, dataset_str, scene_id, im_id, obj_id)) # Load depth image if VSD is selected if error_type == 'vsd' and im_id != im_id_prev: depth_path = dp['test_depth_mpath'].format(scene_id, im_id) # depth_im = inout.load_depth(depth_path) depth_im = inout.load_depth2(depth_path) # Faster depth_im *= dp['cam']['depth_scale'] # to [mm] # Load camera matrix if error_type in ['vsd', 'cou', 'proj', 'projamb']: K = scene_info[im_id]['cam_K'] # Load pose estimates res = inout.load_results_sixd17(res_path) ests = res['ests'] # Sort the estimates by score (in descending order) ests_sorted = sorted(enumerate(ests), key=lambda x: x[1]['score'], reverse=True) # Select the required number of top estimated poses if n_top == 0: # All estimates are considered n_top_curr = None elif n_top == -1: # Given by the number of GT poses n_gt = sum( [gt['obj_id'] == obj_id for gt in scene_gt[im_id]]) n_top_curr = n_gt else: n_top_curr = n_top ests_sorted = ests_sorted[slice(0, n_top_curr)] for est_id, est in ests_sorted: est_errs = [] R_e = est['R'] t_e = est['t'] errs_gts = {} # Errors w.r.t. GT poses of the same object for gt_id, gt in enumerate(scene_gt[im_id]): if gt['obj_id'] != obj_id: continue e = -1.0 R_g = gt['cam_R_m2c'] t_g = gt['cam_t_m2c'] if error_type == 'vsd': e = pose_error.vsd(R_e, t_e, R_g, t_g, models[obj_id], depth_im, K, vsd_delta, vsd_tau, vsd_cost) elif error_type == 'add': e = pose_error.add(R_e, t_e, R_g, t_g, models[obj_id]) elif error_type == 'adi': e = pose_error.adi(R_e, t_e, R_g, t_g, models[obj_id]) elif error_type == 'proj': e = pose_error.arp_2d(R_e, t_e, R_g, t_g, models[obj_id], K) elif error_type == 'projamb': e = pose_error.arpi_2d(R_e, t_e, R_g, t_g, models[obj_id], K) elif error_type == 'cou': e = pose_error.cou(R_e, t_e, R_g, t_g, models[obj_id], dp['test_im_size'], K) elif error_type == 're': e = pose_error.re(R_e, R_g) elif error_type == 'te': e = pose_error.te(t_e, t_g) errs_gts[gt_id] = e errs.append({ 'im_id': im_id, 'obj_id': obj_id, 'est_id': est_id, 'score': est['score'], 'errors': errs_gts }) # print('Evaluation time: {}s'.format(time.time() - t)) print('Saving errors...') errors_path = errors_mpath.format(result_path=result_path, error_sign=error_sign, scene_id=scene_id) misc.ensure_dir(os.path.dirname(errors_path)) inout.save_errors(errors_path, errs) print('') print('Done.') return True
def compute_pose_errors(res_dict, args_latent, dataset): num_obj = args_latent.getint('Data', 'num_obj') num_views = args_latent.getint('Data', 'num_views') test_class = args_latent.get('Data', 'test_class') K = eval(dataset._kw['k']) K = np.array(K).reshape(3, 3) K = np.array([[572.4114, 0, 320.], [0, 573.57043, 240], [0, 0, 1]]) # LM R_init_errs = [] R_1_errs = [] R_2_errs = [] R_3_errs = [] t_init_errs = [] t_1_errs = [] t_2_errs = [] t_3_errs = [] add_recalls_init = [] add_recalls = [] proj_recalls_init = [] proj_recalls = [] proj_recalls2 = [] all_model_pts = [np.array(v) for v in dataset.renderer.verts] diameters = [] for model_pts in all_model_pts: # model_pts_01 = model_pts * 0.1 vec = model_pts.max(0) - model_pts.min(0) print(vec) diameters.append(np.linalg.norm(vec)) print(diameters) for i in range(0, num_obj): for j in range(num_views): R_target = res_dict['preds'][i]['R_init'][j] t_target = res_dict['preds'][i]['t_init'][j] R_init_errs.append( pose_error.re(R_target, res_dict['preds'][i]['R_init_pert'][j])) R_1_errs.append( pose_error.re(R_target, res_dict['preds'][i]['R_1'][j])) R_2_errs.append( pose_error.re(R_target, res_dict['preds'][i]['R_2'][j])) R_3_errs.append( pose_error.re(R_target, res_dict['preds'][i]['R_3'][j])) t_init_errs.append( pose_error.te(t_target, res_dict['preds'][i]['t_init_pert'][j])) t_1_errs.append( pose_error.te(t_target, res_dict['preds'][i]['t_1'][j])) t_2_errs.append( pose_error.te(t_target, res_dict['preds'][i]['t_2'][j])) t_3_errs.append( pose_error.te(t_target, res_dict['preds'][i]['t_3'][j])) add_recalls_init.append( add_recall_diameter(res_dict['preds'][i]['R_init_pert'][j], res_dict['preds'][i]['t_init_pert'][j], R_target, t_target, {'pts': all_model_pts[i]}, diameters[i])) add_recalls.append( add_recall_diameter(res_dict['preds'][i]['R_3'][j], res_dict['preds'][i]['t_3'][j], R_target, t_target, {'pts': all_model_pts[i]}, diameters[i])) proj_recalls_init.append( proj_recall_diameter(res_dict['preds'][i]['R_init_pert'][j], res_dict['preds'][i]['t_init_pert'][j], R_target, t_target, {'pts': all_model_pts[i]}, diameters[i], K)) proj_recalls.append( proj_recall_diameter(res_dict['preds'][i]['R_3'][j], res_dict['preds'][i]['t_3'][j], R_target, t_target, {'pts': all_model_pts[i]}, diameters[i], K)) proj_recalls2.append( proj_recall_diameter(res_dict['preds'][i]['R_3'][j], res_dict['preds'][i]['t_2'][j], R_target, t_target, {'pts': all_model_pts[i]}, diameters[i], K)) R_init_errs = np.array(R_init_errs) R_1_errs = np.array(R_1_errs) R_2_errs = np.array(R_2_errs) R_3_errs = np.array(R_3_errs) t_init_errs = np.array(t_init_errs) t_1_errs = np.array(t_1_errs) t_2_errs = np.array(t_2_errs) t_3_errs = np.array(t_3_errs) res = {} # res['R_init_errs'] = np.array(R_init_errs) # res['R_1_errs'] = np.array(R_1_errs) # res['R_2_errs'] = np.array(R_2_errs) # res['R_3_errs'] = np.array(R_3_errs) # res['t_init_errs'] = np.array(t_init_errs) # res['t_1_errs'] = np.array(t_1_errs) # res['t_2_errs'] = np.array(t_2_errs) res['mean_add_recall_init'] = np.mean(add_recalls_init) res['mean_add_recall'] = np.mean(add_recalls) res['mean_proj_recall_init'] = np.mean(proj_recalls_init) res['mean_proj_recall'] = np.mean(proj_recalls) res['mean_proj_recall2'] = np.mean(proj_recalls2) res['<5deg_<5cm_init'] = len(R_init_errs[ (R_init_errs <= 5) & (t_init_errs <= 50)]) / 1.0 / len(R_init_errs) res['<5deg_<5cm_R1'] = len( R_1_errs[(R_1_errs <= 5) & (t_init_errs <= 50)]) / 1.0 / len(R_1_errs) res['<5deg_<5cm_R1_t1'] = len( R_1_errs[(R_1_errs <= 5) & (t_1_errs <= 50)]) / 1.0 / len(R_1_errs) res['<5deg_<5cm_R2_t1'] = len( R_2_errs[(R_2_errs <= 5) & (t_1_errs <= 50)]) / 1.0 / len(R_2_errs) res['<5deg_<5cm_R2_t2'] = len( R_2_errs[(R_2_errs <= 5) & (t_2_errs <= 50)]) / 1.0 / len(R_2_errs) res['<5deg_<5cm_R2_t2'] = len( R_2_errs[(R_2_errs <= 5) & (t_2_errs <= 50)]) / 1.0 / len(R_2_errs) res['<5deg_<5cm_R3_t2'] = len( R_3_errs[(R_3_errs <= 5) & (t_2_errs <= 50)]) / 1.0 / len(R_3_errs) res['<5deg_<5cm'] = len( R_3_errs[(R_3_errs <= 5) & (t_3_errs <= 50)]) / 1.0 / len(R_3_errs) res['mean_rot_err'] = np.mean(R_3_errs) res['median_rot_err'] = np.median(R_3_errs) print(res) print(('pose_errs_init: median: ' + str(np.median(R_init_errs)) + ', mean: ' + str(np.mean(R_init_errs)) + ', <5deg & <5cm: ' + str( len(R_init_errs[(R_init_errs <= 5) & (t_init_errs <= 50)]) / 1.0 / len(R_init_errs)) + ', <5deg: ' + str(len(R_init_errs[(R_init_errs <= 5)]) / 1.0 / len(R_init_errs)) + ', <5cm: ' + str(len(t_init_errs[t_init_errs <= 50]) / 1.0 / len(t_init_errs)))) print(('pose_errs_final: median: ' + str(np.median(R_3_errs)) + ', mean: ' + str(np.mean(R_3_errs)) + ', <5deg & <5cm: ' + str( len(R_3_errs[(R_3_errs <= 5) & (t_3_errs <= 50)]) / 1.0 / len(R_3_errs)) + ', <5deg: ' + str(len(R_3_errs[(R_3_errs <= 5)]) / 1.0 / len(R_3_errs)) + ', <5cm: ' + str(len(t_3_errs[t_3_errs <= 50]) / 1.0 / len(t_3_errs)))) if args_latent.getboolean('Visualization', 'rot_err_histogram'): plt.figure(1) plt.hist(R_1_errs, bins=180) plt.title('R_1_errs: median: ' + str(np.median(R_1_errs)) + ', mean: ' + str(np.mean(R_1_errs)) + ', <5deg: ' + str(len(R_1_errs[R_1_errs < 5]) / 1.0 / len(R_1_errs))) plt.figure(2) plt.hist(R_3_errs, bins=180) plt.title('R_3_errs: median: ' + str(np.median(R_3_errs)) + ', mean: ' + str(np.mean(R_3_errs)) + ', <5deg & <5cm: ' + str( len(R_3_errs[(R_3_errs <= 5) & (t_2_errs <= 50)]) / 1.0 / len(R_3_errs)) + ', <5deg: ' + str(len(R_3_errs[(R_3_errs <= 5)]) / 1.0 / len(R_3_errs))) plt.figure(3) plt.hist(t_2_errs, bins=180) plt.title('t_2_errs: median: ' + str(np.median(t_2_errs)) + ', mean: ' + str(np.mean(t_2_errs)) + ', <5cm: ' + str(len(t_2_errs[t_2_errs <= 50]) / 1.0 / len(t_2_errs))) plt.show() return res
from sixd_toolkit.pysixd import pose_error, transform, view_sampler R_errors = [] # R_gt = transform.random_rotation_matrix()[:3,:3] # for R in xrange(100000): # R_est = transform.random_rotation_matrix()[:3,:3] # R_errors.append(pose_error.re(R_est,R_gt)) # plot_R_err_hist2(R_errors,'',bins=90) azimuth_range = (0, 2 * np.pi) elev_range = (-0.5 * np.pi, 0.5 * np.pi) views, _ = view_sampler.sample_views(2562, 100, azimuth_range, elev_range) Rs = [] for view in views: R_errors.append(pose_error.re(view['R'], views[0]['R'])) Rs.append(view['R']) # plot_viewsphere_for_embedding(np.array(Rs),'',np.array(R_errors),save=False) # plot_R_err_hist2(R_errors,'',bins=45,save=False) from mpl_toolkits.basemap import Basemap import matplotlib.pyplot as plt def cart2sph(x, y, z): dxy = np.sqrt(x**2 + y**2) r = np.sqrt(dxy**2 + z**2) theta = np.arctan2(y, x) phi = np.arctan2(z, dxy) theta, phi = np.rad2deg([theta, phi])