def stat_se3(pairdb, config): ''' stat mean and std of se3 between real and rendered poses in a pairdb :param pairdb: :param config: :return: ''' num_pair = len(pairdb) tic() se3_i2r_tensor = np.zeros([ num_pair, 6 ]) if config.network.SE3_TYPE == "EULER" else np.zeros([num_pair, 7]) se3_i2r_dist = np.zeros([num_pair, 2]) for i in range(num_pair): rot_i2r, trans_i2r = calc_RT_delta(pairdb[i]['pose_rendered'], pairdb[i]['pose_real'], np.zeros(3), np.ones(3), config.network.ROT_COORD, config.network.SE3_TYPE) se3_i2r_tensor[i, :-3] = rot_i2r se3_i2r_tensor[i, -3:] = trans_i2r R_dist, T_dist = calc_rt_dist_m(pairdb[i]['pose_rendered'], pairdb[i]['pose_real']) se3_i2r_dist[i, 0] = R_dist se3_i2r_dist[i, 1] = T_dist print("stat finished, using {} seconds".format(toc())) se3_mean = np.mean(se3_i2r_tensor, axis=0) se3_std = np.std(se3_i2r_tensor, axis=0) print('mean: {}, \nstd: {}'.format(se3_mean, se3_std)) print("R_max: {}, T_max: {}".format(np.max(se3_i2r_dist[:, 0]), np.max(se3_i2r_dist[:, 1]))) return se3_mean, se3_std
def main(): for cls_name in tqdm(sel_classes): print(cls_name) # if cls_name != 'driller': # continue rd_stat = [] td_stat = [] pose_observed = [] pose_rendered = [] observed_set_file = os.path.join( observed_set_dir, "NDtrain_observed_{}.txt".format(cls_name) ) with open(observed_set_file) as f: image_list = [x.strip() for x in f.readlines()] for data in image_list: pose_observed_path = os.path.join( gt_observed_dir, cls_name, data + "-pose.txt" ) src_pose_m = np.loadtxt(pose_observed_path, skiprows=1) src_euler = np.squeeze(mat2euler(src_pose_m[:, :3])) src_quat = euler2quat(src_euler[0], src_euler[1], src_euler[2]).reshape( 1, -1 ) src_trans = src_pose_m[:, 3] pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3))))) for rendered_idx in range(num_rendered_per_observed): tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3) x_error = np.random.normal(0, x_std, 1)[0] y_error = np.random.normal(0, y_std, 1)[0] z_error = np.random.normal(0, z_std, 1)[0] tgt_trans = src_trans + np.array([x_error, y_error, z_error]) tgt_pose_m = np.hstack( ( euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]), tgt_trans.reshape((3, 1)), ) ) r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m) transform = np.matmul(K, tgt_trans.reshape(3, 1)) center_x = transform[0] / transform[2] center_y = transform[1] / transform[2] count = 0 while r_dist > angle_max or not ( 48 < center_x < (640 - 48) and 48 < center_y < (480 - 48) ): tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3) x_error = np.random.normal(0, x_std, 1)[0] y_error = np.random.normal(0, y_std, 1)[0] z_error = np.random.normal(0, z_std, 1)[0] tgt_trans = src_trans + np.array([x_error, y_error, z_error]) tgt_pose_m = np.hstack( ( euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]), tgt_trans.reshape((3, 1)), ) ) r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m) transform = np.matmul(K, tgt_trans.reshape(3, 1)) center_x = transform[0] / transform[2] center_y = transform[1] / transform[2] count += 1 if count == 100: print(rendered_idx) print( "{}: {}, {}, {}, {}".format( data, r_dist, t_dist, center_x, center_y ) ) print( "count: {}, image_path: {}, rendered_idx: {}".format( count, pose_observed_path.replace("pose.txt", "color.png"), rendered_idx, ) ) tgt_quat = euler2quat(tgt_euler[0], tgt_euler[1], tgt_euler[2]).reshape( 1, -1 ) pose_rendered.append(np.hstack((tgt_quat, tgt_trans.reshape(1, 3)))) rd_stat.append(r_dist) td_stat.append(t_dist) rd_stat = np.array(rd_stat) td_stat = np.array(td_stat) print("r dist: {} +/- {}".format(np.mean(rd_stat), np.std(rd_stat))) print("t dist: {} +/- {}".format(np.mean(td_stat), np.std(td_stat))) output_file_name = os.path.join( pose_dir, "LM6d_occ_dsm_{}_NDtrain_rendered_pose_{}.txt".format(version, cls_name), ) with open(output_file_name, "w") as text_file: for x in pose_rendered: text_file.write("{}\n".format(" ".join(map(str, np.squeeze(x))))) print("{} finished".format(__file__))
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 get_batch_parallel(self): cur_from = self.cur cur_to = min(cur_from + self.batch_size, self.size) pairdb = [self.pairdb[self.index[i]] for i in range(cur_from, cur_to)] # decide multi device slice work_load_list = self.work_load_list ctx = self.ctx if work_load_list is None: work_load_list = [1] * len(ctx) assert isinstance(work_load_list, list) and len(work_load_list) == len( ctx), "Invalid settings for work load. " slices = _split_input_slice(self.batch_size, work_load_list) multiprocess_results = [] for idx, islice in enumerate(slices): """ ipairdb = [pairdb[i] for i in range(islice.start, islice.stop)] multiprocess_results.append(self.pool.apply_async( get_data_pair_train_batch, (ipairdb, self.config))) """ for i in range(islice.start, islice.stop): multiprocess_results.append( self.pool.apply_async(get_data_pair_train_batch, ([pairdb[i]], self.config))) if False: temp = get_data_pair_train_batch([pairdb[islice.start]], self.config) # for debug print("**" * 20) print(pairdb[0]["image_observed"]) print("data:") for k in temp["data"].keys(): print("\t{}, {}".format(k, temp["data"][k].shape)) print("label:") for k in temp["label"].keys(): print("\t{}, {}".format(k, temp["label"][k].shape)) print(temp["label"]["rot"]) print(temp["label"]["trans"]) from lib.pair_matching.RT_transform import calc_rt_dist_m r_dist, t_dist = calc_rt_dist_m(temp["data"]["src_pose"][0], temp["data"]["tgt_pose"][0]) print("{}: R_dist: {}, T_dist: {}".format(self.cur, r_dist, t_dist)) print("**" * 20) image_real = ( temp["data"]["image_observed"][0].transpose([1, 2, 0]) + 128).astype(np.uint8) print(np.max(image_real)) print(np.min(image_real)) image_rendered = ( temp["data"]["image_rendered"][0].transpose([1, 2, 0]) + 128).astype(np.uint8) mask_real_gt = np.squeeze(temp["label"]["mask_gt_observed"]) mask_real_est = np.squeeze(temp["data"]["mask_observed"]) mask_rendered = np.squeeze(temp["data"]["mask_rendered"]) if "flow" in temp["label"]: print("in loader, flow: ", temp["label"]["flow"].shape, np.unique(temp["label"]["flow"])) print( "in loader, flow weights: ", temp["label"]["flow_weights"].shape, np.unique(temp["label"]["flow_weights"]), ) import matplotlib.pyplot as plt plt.subplot(2, 3, 1) plt.imshow(mask_real_est) plt.subplot(2, 3, 2) plt.imshow(mask_real_gt) plt.subplot(2, 3, 3) plt.imshow(mask_rendered) plt.subplot(2, 3, 4) plt.imshow(image_real) plt.subplot(2, 3, 5) plt.imshow(image_rendered) plt.show() # plt.savefig('image_real_rendered_{}'.format(self.cur), aspect='normal') rst = [ multiprocess_result.get() for multiprocess_result in multiprocess_results ] batch_per_gpu = int(self.batch_size / len(ctx)) data_list = [] label_list = [] for i in range(len(ctx)): sample_data_list = [_["data"] for _ in rst] sample_label_list = [_["label"] for _ in rst] batch_data = {} batch_label = {} for key in sample_data_list[0]: batch_data[key] = my_tensor_vstack([ sample_data_list[j][key] for j in range(i * batch_per_gpu, (i + 1) * batch_per_gpu) ]) for key in sample_label_list[0]: batch_label[key] = my_tensor_vstack([ sample_label_list[j][key] for j in range(i * batch_per_gpu, (i + 1) * batch_per_gpu) ]) data_list.append(batch_data) label_list.append(batch_label) """ data_list = [_['data'] for _ in rst] label_list = [_['label'] for _ in rst] """ self.data = [[mx.nd.array(data_on_i[key]) for key in self.data_name] for data_on_i in data_list] self.label = [[ mx.nd.array(label_on_i[key]) for key in self.label_name ] for label_on_i in label_list]
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(' ')
src_quat = euler2quat(src_euler[0], src_euler[1], src_euler[2]).reshape(1, -1) src_trans = src_pose_m[:, 3] pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3))))) for rendered_idx in range(num_rendered_per_observed): tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3) x_error = np.random.normal(0, x_std, 1)[0] y_error = np.random.normal(0, y_std, 1)[0] z_error = np.random.normal(0, z_std, 1)[0] tgt_trans = src_trans + np.array([x_error, y_error, z_error]) tgt_pose_m = np.hstack( (euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]), tgt_trans.reshape((3, 1)))) r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m) transform = np.matmul(K, tgt_trans.reshape(3, 1)) center_x = transform[0] / transform[2] center_y = transform[1] / transform[2] count = 0 while r_dist > angle_max or not (16 < center_x < (640 - 16) and 16 < center_y < (480 - 16)): tgt_euler = src_euler + np.random.normal( 0, angle_std / 180 * pi, 3) x_error = np.random.normal(0, x_std, 1)[0] y_error = np.random.normal(0, y_std, 1)[0] z_error = np.random.normal(0, z_std, 1)[0] tgt_trans = src_trans + np.array([x_error, y_error, z_error]) tgt_pose_m = np.hstack( (euler2mat(tgt_euler[0], tgt_euler[1],
def pred_eval(config, predictor, test_data, imdb_test, vis=False, ignore_cache=None, logger=None, pairdb=None): """ wrapper for calculating offline validation for faster data analysis in this example, all threshold are set by hand :param predictor: Predictor :param test_data: data iterator, must be non-shuffle :param imdb_test: image database :param vis: controls visualization :param ignore_cache: ignore the saved cache file :param logger: the logger instance :return: """ logger.info(imdb_test.result_path) logger.info("test iter size: {}".format(config.TEST.test_iter)) pose_err_file = os.path.join( imdb_test.result_path, imdb_test.name + "_pose_iter{}.pkl".format(config.TEST.test_iter)) if os.path.exists(pose_err_file) and not ignore_cache and not vis: with open(pose_err_file, "rb") as fid: if six.PY3: [all_rot_err, all_trans_err, all_poses_est, all_poses_gt] = cPickle.load(fid, encoding="latin1") else: [all_rot_err, all_trans_err, all_poses_est, all_poses_gt] = cPickle.load(fid) imdb_test.evaluate_pose(config, all_poses_est, all_poses_gt) pose_add_plots_dir = os.path.join(imdb_test.result_path, "add_plots") mkdir_p(pose_add_plots_dir) imdb_test.evaluate_pose_add(config, all_poses_est, all_poses_gt, output_dir=pose_add_plots_dir) pose_arp2d_plots_dir = os.path.join(imdb_test.result_path, "arp_2d_plots") mkdir_p(pose_arp2d_plots_dir) imdb_test.evaluate_pose_arp_2d(config, all_poses_est, all_poses_gt, output_dir=pose_arp2d_plots_dir) return assert vis or not test_data.shuffle assert config.TEST.BATCH_PAIRS == 1 if not isinstance(test_data, PrefetchingIter): test_data = PrefetchingIter(test_data) num_pairs = len(pairdb) height = 480 width = 640 data_time, net_time, post_time = 0.0, 0.0, 0.0 sum_EPE_all = 0.0 num_inst_all = 0.0 sum_EPE_viz = 0.0 num_inst_viz = 0.0 sum_EPE_vizbg = 0.0 num_inst_vizbg = 0.0 sum_PoseErr = [ np.zeros((len(imdb_test.classes) + 1, 2)) for batch_idx in range(config.TEST.test_iter) ] all_rot_err = [[[] for j in range(config.TEST.test_iter)] for batch_idx in range(len(imdb_test.classes)) ] # num_cls x test_iter all_trans_err = [[[] for j in range(config.TEST.test_iter)] for batch_idx in range(len(imdb_test.classes))] all_poses_est = [[[] for j in range(config.TEST.test_iter)] for batch_idx in range(len(imdb_test.classes))] all_poses_gt = [[[] for j in range(config.TEST.test_iter)] for batch_idx in range(len(imdb_test.classes))] num_inst = np.zeros(len(imdb_test.classes) + 1) K = config.dataset.INTRINSIC_MATRIX if (config.TEST.test_iter > 1 or config.TEST.VISUALIZE) and True: print( "************* start setup render_glumpy environment... ******************" ) if config.dataset.dataset.startswith("ModelNet"): from lib.render_glumpy.render_py_light_modelnet_multi import Render_Py_Light_ModelNet_Multi modelnet_root = config.modelnet_root texture_path = os.path.join(modelnet_root, "gray_texture.png") model_path_list = [ os.path.join(config.dataset.model_dir, "{}.obj".format(model_name)) for model_name in config.dataset.class_name ] render_machine = Render_Py_Light_ModelNet_Multi( model_path_list, texture_path, K, width, height, config.dataset.ZNEAR, config.dataset.ZFAR, brightness_ratios=[0.7], ) else: render_machine = Render_Py( config.dataset.model_dir, config.dataset.class_name, K, width, height, config.dataset.ZNEAR, config.dataset.ZFAR, ) def render(render_machine, pose, cls_idx, K=None): if config.dataset.dataset.startswith("ModelNet"): idx = 2 # generate random light_position if idx % 6 == 0: light_position = [1, 0, 1] elif idx % 6 == 1: light_position = [1, 1, 1] elif idx % 6 == 2: light_position = [0, 1, 1] elif idx % 6 == 3: light_position = [-1, 1, 1] elif idx % 6 == 4: light_position = [-1, 0, 1] elif idx % 6 == 5: light_position = [0, 0, 1] else: raise Exception("???") light_position = np.array(light_position) * 0.5 # inverse yz light_position[0] += pose[0, 3] light_position[1] -= pose[1, 3] light_position[2] -= pose[2, 3] colors = np.array([1, 1, 1]) # white light intensity = np.random.uniform(0.9, 1.1, size=(3, )) colors_randk = 0 light_intensity = colors[colors_randk] * intensity # randomly choose a render machine rm_randk = 0 # random.randint(0, len(brightness_ratios) - 1) rgb_gl, depth_gl = render_machine.render( cls_idx, pose[:3, :3], pose[:3, 3], light_position, light_intensity, brightness_k=rm_randk, r_type="mat", ) rgb_gl = rgb_gl.astype("uint8") else: rgb_gl, depth_gl = render_machine.render(cls_idx, pose[:3, :3], pose[:, 3], r_type="mat", K=K) rgb_gl = rgb_gl.astype("uint8") return rgb_gl, depth_gl print( "***************setup render_glumpy environment succeed ******************" ) if config.TEST.PRECOMPUTED_ICP: print("precomputed_ICP") config.TEST.test_iter = 1 all_rot_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_poses_est = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_poses_gt = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] xy_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] z_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] for idx in range(len(pairdb)): pose_path = pairdb[idx]["depth_rendered"][:-10] + "-pose_icp.txt" pose_rendered_update = np.loadtxt(pose_path, skiprows=1) pose_observed = pairdb[idx]["pose_observed"] r_dist_est, t_dist_est = calc_rt_dist_m(pose_rendered_update, pose_observed) xy_dist = np.linalg.norm(pose_rendered_update[:2, -1] - pose_observed[:2, -1]) z_dist = np.linalg.norm(pose_rendered_update[-1, -1] - pose_observed[-1, -1]) print( "{}: r_dist_est: {}, t_dist_est: {}, xy_dist: {}, z_dist: {}". format(idx, r_dist_est, t_dist_est, xy_dist, z_dist)) class_id = imdb_test.classes.index(pairdb[idx]["gt_class"]) # store poses estimation and gt all_poses_est[class_id][0].append(pose_rendered_update) all_poses_gt[class_id][0].append(pairdb[idx]["pose_observed"]) all_rot_err[class_id][0].append(r_dist_est) all_trans_err[class_id][0].append(t_dist_est) xy_trans_err[class_id][0].append(xy_dist) z_trans_err[class_id][0].append(z_dist) all_rot_err = np.array(all_rot_err) all_trans_err = np.array(all_trans_err) print("rot = {} +/- {}".format(np.mean(all_rot_err[class_id][0]), np.std(all_rot_err[class_id][0]))) print("trans = {} +/- {}".format(np.mean(all_trans_err[class_id][0]), np.std(all_trans_err[class_id][0]))) num_list = all_trans_err[class_id][0] print("xyz: {:.2f} +/- {:.2f}".format( np.mean(num_list) * 100, np.std(num_list) * 100)) num_list = xy_trans_err[class_id][0] print("xy: {:.2f} +/- {:.2f}".format( np.mean(num_list) * 100, np.std(num_list) * 100)) num_list = z_trans_err[class_id][0] print("z: {:.2f} +/- {:.2f}".format( np.mean(num_list) * 100, np.std(num_list) * 100)) imdb_test.evaluate_pose(config, all_poses_est, all_poses_gt) pose_add_plots_dir = os.path.join(imdb_test.result_path, "add_plots_precomputed_ICP") mkdir_p(pose_add_plots_dir) imdb_test.evaluate_pose_add(config, all_poses_est, all_poses_gt, output_dir=pose_add_plots_dir) pose_arp2d_plots_dir = os.path.join(imdb_test.result_path, "arp_2d_plots_precomputed_ICP") mkdir_p(pose_arp2d_plots_dir) imdb_test.evaluate_pose_arp_2d(config, all_poses_est, all_poses_gt, output_dir=pose_arp2d_plots_dir) return if config.TEST.BEFORE_ICP: print("before_ICP") config.TEST.test_iter = 1 all_rot_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_poses_est = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] all_poses_gt = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] xy_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] z_trans_err = [[[] for j in range(1)] for batch_idx in range(len(imdb_test.classes))] for idx in range(len(pairdb)): pose_path = pairdb[idx]["depth_rendered"][:-10] + "-pose.txt" pose_rendered_update = np.loadtxt(pose_path, skiprows=1) pose_observed = pairdb[idx]["pose_observed"] r_dist_est, t_dist_est = calc_rt_dist_m(pose_rendered_update, pose_observed) xy_dist = np.linalg.norm(pose_rendered_update[:2, -1] - pose_observed[:2, -1]) z_dist = np.linalg.norm(pose_rendered_update[-1, -1] - pose_observed[-1, -1]) class_id = imdb_test.classes.index(pairdb[idx]["gt_class"]) # store poses estimation and gt all_poses_est[class_id][0].append(pose_rendered_update) all_poses_gt[class_id][0].append(pairdb[idx]["pose_observed"]) all_rot_err[class_id][0].append(r_dist_est) all_trans_err[class_id][0].append(t_dist_est) xy_trans_err[class_id][0].append(xy_dist) z_trans_err[class_id][0].append(z_dist) all_trans_err = np.array(all_trans_err) imdb_test.evaluate_pose(config, all_poses_est, all_poses_gt) pose_add_plots_dir = os.path.join(imdb_test.result_path, "add_plots_before_ICP") mkdir_p(pose_add_plots_dir) imdb_test.evaluate_pose_add(config, all_poses_est, all_poses_gt, output_dir=pose_add_plots_dir) pose_arp2d_plots_dir = os.path.join(imdb_test.result_path, "arp_2d_plots_before_ICP") mkdir_p(pose_arp2d_plots_dir) imdb_test.evaluate_pose_arp_2d(config, all_poses_est, all_poses_gt, output_dir=pose_arp2d_plots_dir) return # ------------------------------------------------------------------------------ t_start = time.time() t = time.time() for idx, data_batch in enumerate(test_data): if np.sum(pairdb[idx] ["pose_rendered"]) == -12: # NO POINT VALID IN INIT POSE print(idx) class_id = imdb_test.classes.index(pairdb[idx]["gt_class"]) for pose_iter_idx in range(config.TEST.test_iter): all_poses_est[class_id][pose_iter_idx].append( pairdb[idx]["pose_rendered"]) all_poses_gt[class_id][pose_iter_idx].append( pairdb[idx]["pose_observed"]) r_dist = 1000 t_dist = 1000 all_rot_err[class_id][pose_iter_idx].append(r_dist) all_trans_err[class_id][pose_iter_idx].append(t_dist) sum_PoseErr[pose_iter_idx][class_id, :] += np.array( [r_dist, t_dist]) sum_PoseErr[pose_iter_idx][-1, :] += np.array([r_dist, t_dist]) # post process if idx % 50 == 0: logger.info( "testing {}/{} data {:.4f}s net {:.4f}s calc_gt {:.4f}s". format( (idx + 1), num_pairs, data_time / ((idx + 1) * test_data.batch_size), net_time / ((idx + 1) * test_data.batch_size), post_time / ((idx + 1) * test_data.batch_size), )) print("in test: NO POINT_VALID IN rendered") continue data_time += time.time() - t t = time.time() pose_rendered = pairdb[idx]["pose_rendered"] if np.sum(pose_rendered) == -12: print(idx) class_id = imdb_test.classes.index(pairdb[idx]["gt_class"]) num_inst[class_id] += 1 num_inst[-1] += 1 for pose_iter_idx in range(config.TEST.test_iter): all_poses_est[class_id][pose_iter_idx].append(pose_rendered) all_poses_gt[class_id][pose_iter_idx].append( pairdb[idx]["pose_observed"]) # post process if idx % 50 == 0: logger.info( "testing {}/{} data {:.4f}s net {:.4f}s calc_gt {:.4f}s". format( (idx + 1), num_pairs, data_time / ((idx + 1) * test_data.batch_size), net_time / ((idx + 1) * test_data.batch_size), post_time / ((idx + 1) * test_data.batch_size), )) t = time.time() continue output_all = predictor.predict(data_batch) net_time += time.time() - t t = time.time() rst_iter = [] for output in output_all: cur_rst = {} cur_rst["se3"] = np.squeeze( output["se3_output"].asnumpy()).astype("float32") if not config.TEST.FAST_TEST and config.network.PRED_FLOW: cur_rst["flow"] = np.squeeze( output["flow_est_crop_output"].asnumpy().transpose( (2, 3, 1, 0))).astype("float16") else: cur_rst["flow"] = None if config.network.PRED_MASK and config.TEST.UPDATE_MASK not in [ "init", "box_rendered" ]: mask_pred = np.squeeze( output["mask_observed_pred_output"].asnumpy()).astype( "float32") cur_rst["mask_pred"] = mask_pred rst_iter.append(cur_rst) post_time += time.time() - t # sample_ratio = 1 # 0.01 for batch_idx in range(0, test_data.batch_size): # if config.TEST.VISUALIZE and not (r_dist>15 and t_dist>0.05): # continue # 3388, 5326 # calculate the flow error -------------------------------------------- t = time.time() if config.network.PRED_FLOW and not config.TEST.FAST_TEST: # evaluate optical flow flow_gt = par_generate_gt(config, pairdb[idx]) if config.network.PRED_FLOW: all_diff = calc_EPE_one_pair(rst_iter[batch_idx], flow_gt, "flow") sum_EPE_all += all_diff["epe_all"] num_inst_all += all_diff["num_all"] sum_EPE_viz += all_diff["epe_viz"] num_inst_viz += all_diff["num_viz"] sum_EPE_vizbg += all_diff["epe_vizbg"] num_inst_vizbg += all_diff["num_vizbg"] # calculate the se3 error --------------------------------------------- # evaluate se3 estimation pose_rendered = pairdb[idx]["pose_rendered"] class_id = imdb_test.classes.index(pairdb[idx]["gt_class"]) num_inst[class_id] += 1 num_inst[-1] += 1 post_time += time.time() - t # iterative refine se3 estimation -------------------------------------------------- for pose_iter_idx in range(config.TEST.test_iter): t = time.time() pose_rendered_update = RT_transform( pose_rendered, rst_iter[0]["se3"][:-3], rst_iter[0]["se3"][-3:], config.dataset.trans_means, config.dataset.trans_stds, config.network.ROT_COORD, ) # calculate error r_dist, t_dist = calc_rt_dist_m(pose_rendered_update, pairdb[idx]["pose_observed"]) # store poses estimation and gt all_poses_est[class_id][pose_iter_idx].append( pose_rendered_update) all_poses_gt[class_id][pose_iter_idx].append( pairdb[idx]["pose_observed"]) all_rot_err[class_id][pose_iter_idx].append(r_dist) all_trans_err[class_id][pose_iter_idx].append(t_dist) sum_PoseErr[pose_iter_idx][class_id, :] += np.array( [r_dist, t_dist]) sum_PoseErr[pose_iter_idx][-1, :] += np.array([r_dist, t_dist]) if config.TEST.VISUALIZE: print("idx {}, iter {}: rError: {}, tError: {}".format( idx + batch_idx, pose_iter_idx + 1, r_dist, t_dist)) post_time += time.time() - t # # if more than one iteration if pose_iter_idx < (config.TEST.test_iter - 1) or config.TEST.VISUALIZE: t = time.time() # get refined image K_path = pairdb[idx]["image_observed"][:-10] + "-K.txt" if os.path.exists(K_path): K = np.loadtxt(K_path) image_refined, depth_refined = render( render_machine, pose_rendered_update, config.dataset.class_name.index( pairdb[idx]["gt_class"]), K=K, ) image_refined = image_refined[:, :, :3] # update minibatch update_package = [{ "image_rendered": image_refined, "src_pose": pose_rendered_update }] if config.network.INPUT_DEPTH: update_package[0]["depth_rendered"] = depth_refined if config.network.INPUT_MASK: mask_rendered_refined = np.zeros(depth_refined.shape) mask_rendered_refined[depth_refined > 0.2] = 1 update_package[0][ "mask_rendered"] = mask_rendered_refined if config.network.PRED_MASK: # init, box_rendered, mask_rendered, box_observed, mask_observed if config.TEST.UPDATE_MASK == "box_rendered": input_names = [ blob_name[0] for blob_name in data_batch.provide_data[0] ] update_package[0][ "mask_observed"] = np.squeeze( data_batch.data[0][input_names.index( "mask_rendered")].asnumpy() [batch_idx]) # noqa elif config.TEST.UPDATE_MASK == "init": pass else: raise Exception( "Unknown UPDATE_MASK type: {}".format( config.network.UPDATE_MASK)) pose_rendered = pose_rendered_update data_batch = update_data_batch(config, data_batch, update_package) data_time += time.time() - t # forward and get rst if pose_iter_idx < config.TEST.test_iter - 1: t = time.time() output_all = predictor.predict(data_batch) net_time += time.time() - t t = time.time() rst_iter = [] for output in output_all: cur_rst = {} if config.network.REGRESSOR_NUM == 1: cur_rst["se3"] = np.squeeze( output["se3_output"].asnumpy()).astype( "float32") if not config.TEST.FAST_TEST and config.network.PRED_FLOW: cur_rst["flow"] = np.squeeze( output["flow_est_crop_output"].asnumpy(). transpose((2, 3, 1, 0))).astype("float16") else: cur_rst["flow"] = None if config.network.PRED_MASK and config.TEST.UPDATE_MASK not in [ "init", "box_rendered" ]: mask_pred = np.squeeze( output["mask_observed_pred_output"]. asnumpy()).astype("float32") cur_rst["mask_pred"] = mask_pred rst_iter.append(cur_rst) post_time += time.time() - t # post process if idx % 50 == 0: logger.info( "testing {}/{} data {:.4f}s net {:.4f}s calc_gt {:.4f}s". format( (idx + 1), num_pairs, data_time / ((idx + 1) * test_data.batch_size), net_time / ((idx + 1) * test_data.batch_size), post_time / ((idx + 1) * test_data.batch_size), )) t = time.time() all_rot_err = np.array(all_rot_err) all_trans_err = np.array(all_trans_err) # save inference results if not config.TEST.VISUALIZE: with open(pose_err_file, "wb") as f: logger.info("saving result cache to {}".format(pose_err_file)) cPickle.dump( [all_rot_err, all_trans_err, all_poses_est, all_poses_gt], f, protocol=2) logger.info("done") if config.network.PRED_FLOW: logger.info("evaluate flow:") logger.info("EPE all: {}".format(sum_EPE_all / max(num_inst_all, 1.0))) logger.info("EPE ignore unvisible: {}".format( sum_EPE_vizbg / max(num_inst_vizbg, 1.0))) logger.info("EPE visible: {}".format(sum_EPE_viz / max(num_inst_viz, 1.0))) logger.info("evaluate pose:") imdb_test.evaluate_pose(config, all_poses_est, all_poses_gt) # evaluate pose add pose_add_plots_dir = os.path.join(imdb_test.result_path, "add_plots") mkdir_p(pose_add_plots_dir) imdb_test.evaluate_pose_add(config, all_poses_est, all_poses_gt, output_dir=pose_add_plots_dir) pose_arp2d_plots_dir = os.path.join(imdb_test.result_path, "arp_2d_plots") mkdir_p(pose_arp2d_plots_dir) imdb_test.evaluate_pose_arp_2d(config, all_poses_est, all_poses_gt, output_dir=pose_arp2d_plots_dir) logger.info("using {} seconds in total".format(time.time() - t_start))