def get_grasp_pose(pc, grids, contact_index, grid_len, grid_num): pc_, grids_, contact_index_ = torch.tensor(pc).unsqueeze(0), torch.tensor( grids), torch.tensor(contact_index) pc1, grids1, contact_index1 = pc_.float().cuda(1), grids_.float().cuda( 1), contact_index_.long().cuda(1) pc, grids, contact_index = pc_.float().cuda(0), grids_.float().cuda( 0), contact_index_.long().cuda(0) del (pc_, grids_, contact_index_) radius = grid_len / grid_num * np.sqrt(3) print('proposal input shape ', pc1.shape, grids1.shape, contact_index1.shape) pairs_all_, local_points_ = getTestProposalsV3(pc1, grids1, contact_index1, grid_th=0.075) del (pc1, grids1, contact_index1) pairs_all, local_points = pairs_all_.cuda(0), local_points_.cuda(0) del (local_points_, pairs_all_) st = time.time() print('net input: pc & local point & pairs all shape: ', pc.shape, local_points.shape, pairs_all.shape) with torch.no_grad(): prop_score, pred_score, pred_offset, pred_angle, prop_posi_idx = net( pc, local_points, pairs_all, scale=radius) print('\nforward time: ', time.time() - st) print('net output: pred score & offset & angle shape: ', pred_score.shape, pred_offset.shape, pred_angle.shape) posi_idx = torch.nonzero(pred_score.view(-1) >= 0.5).view(-1) print('pred score and positive index: ', pred_score, posi_idx) posi_scores = pred_score.view(-1)[posi_idx] prop_score = prop_score.view(-1)[posi_idx] prop_posi_idx = prop_posi_idx[posi_idx] pred_offset = pred_offset[0, posi_idx] pred_angle = pred_angle[0, posi_idx] pred_pairs = pairs_all[0, prop_posi_idx] print('pred pairs, offset, angles shape fed into get7dof: ', pred_pairs.shape) centers, widths, quaternions = get7dofPoses(pred_pairs, pred_offset, pred_angle, scale=radius) z = centers[:, 2] select = torch.nonzero((widths < 0.085) * (z > 0)).view(-1) centers, widths, quaternions = centers[select], widths[ select], quaternions[select] select_pred_pairs = pred_pairs[select] posi_scores = posi_scores[select] # prop_score = prop_score[select] # pred_angle = pred_angle[select].view(-1) # posi_contacts = select_pred_pairs[:, 0] # posi_contacts_cpu = posi_contacts.cpu().numpy() posi_scores_cpu = posi_scores.cpu().numpy() centers = centers.cpu().numpy() widths = widths.cpu().numpy() quaternions = quaternions.cpu().numpy() # prop_score_cpu = prop_score.cpu().numpy() # pred_angle_cpu = pred_angle.cpu().numpy() print('posi grasp num:', posi_scores.size(0)) keep = nms2(centers, quaternions, posi_scores_cpu, cent_th=0.04, ang_th=np.pi / 3) keep = np.array(keep, dtype=np.int32) centers = centers[keep] widths = widths[keep] quaternions = quaternions[keep] print(centers.shape, widths.shape, quaternions.shape) return centers, widths, quaternions
def main(): pred_data_path = opt.data_path outputdir = os.path.dirname(pred_data_path) if opt.posi_only: if opt.anti_score: LOG_FOUT = open( os.path.join( outputdir, 'test_top10_poses_%s_anti_score_posi_only.txt' % (os.path.basename(pred_data_path))), 'w') log_all = open( os.path.join( outputdir, 'test_all_poses_%s_anti_score_posi_only.txt' % (os.path.basename(pred_data_path))), 'w') else: LOG_FOUT = open( os.path.join( outputdir, 'test_top10_poses_%s_posi_only.txt' % (os.path.basename(pred_data_path))), 'w') log_all = open( os.path.join( outputdir, 'test_all_poses_%s_posi_only.txt' % (os.path.basename(pred_data_path))), 'w') else: if opt.anti_score: LOG_FOUT = open( os.path.join( outputdir, 'test_top10_poses_%s_anti_score.txt' % (os.path.basename(pred_data_path))), 'w') log_all = open( os.path.join( outputdir, 'test_all_poses_%s_anti_score.txt' % (os.path.basename(pred_data_path))), 'w') else: LOG_FOUT = open( os.path.join( outputdir, 'test_top10_poses_%s.txt' % (os.path.basename(pred_data_path))), 'w') log_all = open( os.path.join( outputdir, 'test_all_poses_%s.txt' % (os.path.basename(pred_data_path))), 'w') def log_string(out_str): LOG_FOUT.write(out_str + '\n') LOG_FOUT.flush() print(out_str) keep_num = [] pp = [] rr = [] n = 21 th = 0.03 q_th = np.pi / 4 files = os.listdir(opt.data_path) for i, f in enumerate(files): shape = f.split('.')[0] # if shape == '64d97464f86b591caf17412e945e52f4': # continue print(shape, i) # seen_num += 1 if not os.path.exists(os.path.join(pred_data_path, shape + '.npz')): continue f = np.load(os.path.join(pred_data_path, shape + '.npz')) pred_cent = f['centers'] pred_quat = f['quaternions'] # widths = f['widths'] scores = f['scores'] if opt.anti_score: anti_scores = f['anti_scores'] print('antipodal scores max-min :', anti_scores.max(), anti_scores.min()) print('grasp scores max-min :', scores.max(), scores.min()) if opt.anti_score: idx = anti_scores > 0.5 if idx.sum() == 0: idx = scores > 0 scores = scores[idx] pred_cent = pred_cent[idx] pred_quat = pred_quat[idx] if opt.posi_only: idx = scores > 0.5 if idx.sum() == 0: idx = scores > 0 scores = scores[idx] pred_cent = pred_cent[idx] pred_quat = pred_quat[idx] keep = nms2(pred_cent, pred_quat, scores, cent_th=0.04, ang_th=np.pi / 3) keep = np.array(keep, dtype=np.int32) pred_cent = pred_cent[keep] pred_quat = pred_quat[keep] log_all.write(shape + '\n') for i in range(pred_cent.shape[0]): c = pred_cent[i] q = pred_quat[i] log_all.write('%f,%f,%f,%f,%f,%f,%f\n' % (c[0], c[1], c[2], q[0], q[1], q[2], q[3])) log_all.flush() keep_num = keep.shape[0] print('keep_num: ', keep_num) if keep_num < 10: idx = np.concatenate([ np.arange(keep_num), np.random.choice(keep_num, 10 - keep_num, replace=True) ], 0) pred_cent = pred_cent[idx] pred_quat = pred_quat[idx] log_string(shape) for i in range(10): c = pred_cent[i] q = pred_quat[i] log_string('%f,%f,%f,%f,%f,%f,%f' % (c[0], c[1], c[2], q[0], q[1], q[2], q[3]))
def main(): data_path = opt.gt_data dataset = GraspData(data_path, split='test') dataLoader = torch.utils.data.DataLoader(dataset, batch_size=1, shuffle=False, num_workers=2, pin_memory=False) pred_data_path = opt.pred_data isdir = os.path.isdir(pred_data_path) if not isdir: shape_poses = readShapePoses(pred_data_path) keep_num = [] pp = [] rr = [] n = 21 th = 0.025 q_th = np.pi / 6 all_coverage = [] all_precision = [] all_number = [] for i, data in enumerate(dataLoader): gt_grasps, shape = data shape = shape[0] # if shape == '5db63af675507081118ddfdb81cc6068': # continue print(shape, i) gt_grasps = gt_grasps[0].float() # seen_num += 1 if isdir: if not os.path.exists(os.path.join(pred_data_path, shape + '.npz')): continue f = np.load(os.path.join(pred_data_path, shape + '.npz')) pred_cent = f['centers'] pred_quat = f['quaternions'] # widths = f['widths'] scores = f['scores'] # pred_correct = f['pred_label'] posi_idx = scores > 0.5 pred_cent = pred_cent[posi_idx] pred_quat = pred_quat[posi_idx] scores = scores[posi_idx] keep = nms2(pred_cent, pred_quat, scores, cent_th=0.04, ang_th=np.pi / 3) keep = np.array(keep, dtype=np.int32) keep_num.append(keep.shape[0]) print('keep_num', keep.shape) pred_cent = pred_cent[keep] pred_quat = pred_quat[keep] # pred_correct = pred_correct[keep] else: if shape not in shape_poses: continue grasp = shape_poses[shape] grasp = np.concatenate(grasp, 0) pred_cent = grasp[:, :3] pred_quat = grasp[:, 3:] print(gt_grasps.size(0), pred_cent.shape[0]) # if gt_grasps.size(0) < 800: # continue p = coverage_vs_precision(gt_grasps.numpy()) coverage = np.zeros(4) precision = np.zeros(4) number = np.zeros(4) for i, per in enumerate([0.1, 0.3, 0.5, 1.0]): s, c, k = p.precision_and_recall_at_k_percent(pred_cent, pred_quat, per, th, q_th, gpu=True) coverage[i] = c precision[i] = s number[i] = k all_coverage.append(coverage) all_precision.append(precision) all_number.append(number) all_coverage = np.array(all_coverage) all_precision = np.array(all_precision) all_number = np.array(all_number) print(np.mean(all_coverage, 0)) print(np.mean(all_precision, 0)) print(np.mean(all_number, 0)) print(len(dataset))
def main(): # opt.manualSeed = random.randint(1, 10000) opt.manualSeed = 1 random.seed(opt.manualSeed) torch.manual_seed(opt.manualSeed) print(opt) outputdir = os.path.join(os.path.dirname(opt.resume), 'test') grid_len = opt.grid_len / 100 grid_num = opt.grid_num dataset = GraspData(opt.dataset_root, split='test', view=opt.view, grid_len=grid_len, grid_num=grid_num) dataLoader = torch.utils.data.DataLoader(dataset, batch_size=1, shuffle=False, num_workers=opt.workers, pin_memory=True) net = GraspPoseNet(tanh=opt.tanh, grid=opt.grid, training=False, bn=False, use_angle=True).cuda() if os.path.isfile(opt.resume): print("=> loading checkpoint '{}'".format(opt.resume)) checkpoint = torch.load(opt.resume) pretrained_dict = checkpoint['state_dict'] net.load_state_dict(pretrained_dict) # lr = checkpoint['lr'] epoch = checkpoint['epoch'] print("\n=> loaded checkpoint '{}' (epoch {})".format( opt.resume, checkpoint['epoch'])) del checkpoint else: assert False, 'WRONG RESUME PATH!' all_grasps_dir = os.path.join(outputdir, 'epoch%d' % epoch) if opt.save_all: all_grasps_dir = all_grasps_dir + '_all' outputdir = all_grasps_dir all_grasps_dir = os.path.join(all_grasps_dir, 'view%d' % (opt.view)) if not os.path.exists(all_grasps_dir): os.makedirs(all_grasps_dir) log = open(os.path.join(outputdir, 'log.txt'), 'a') log.write('view%d\n' % opt.view) log_all = open( os.path.join(outputdir, 'nms_poses_view%s.txt' % (opt.view)), 'w') net.eval() time_list = [] with torch.no_grad(): for i, data in enumerate(dataLoader): st = time.time() pc_, grids_, contact_index_, shape = data shape = shape[0] print(shape, i) print(grids_.size(), contact_index_.size(), pc_.size()) pc1, grids1, contact_index1 = pc_.float().cuda( 1), grids_.float().cuda(1), contact_index_.long().cuda(1) pc, grids, contact_index = pc_.float().cuda( 0), grids_.float().cuda(0), contact_index_.long().cuda(0) data_index = torch.arange(contact_index_.size(1)).long().cuda() radius = grid_len / grid_num * np.sqrt(3) # pairs_all_, local_points_ = getTestProposals(pc1, grids1, contact_index1) pairs_all_, local_points_ = getTestProposalsV3(pc1, grids1, contact_index1, grid_th=opt.grid_th) print(pairs_all_.size()) del (pc1, grids1, contact_index1) pairs_all, local_points = pairs_all_.cuda(0), local_points_.cuda(0) del (pc_, grids_, contact_index_, local_points_, pairs_all_) prop_score, pred_score, pred_offset, pred_angle, prop_posi_idx \ = net(pc, local_points, pairs_all, scale=radius) print('\nforward time: ', time.time() - st) print('pred_score: ', pred_score.max().item(), pred_score.min().item()) if opt.save_all: posi_idx = torch.nonzero(pred_score.view(-1) >= 0.).view(-1) else: posi_idx = torch.nonzero(pred_score.view(-1) >= 0.5).view(-1) if posi_idx.size(0) == 0: continue posi_scores = pred_score.view(-1)[posi_idx] prop_score = prop_score.view(-1)[posi_idx] prop_posi_idx = prop_posi_idx[posi_idx] pred_offset = pred_offset[0, posi_idx] pred_angle = pred_angle[0, posi_idx] pred_pairs = pairs_all[0, prop_posi_idx] centers, widths, quaternions = get7dofPoses(pred_pairs, pred_offset, pred_angle, scale=radius) time_list.append(time.time() - st) z = centers[:, 2] select = torch.nonzero((widths < 0.085) * (z > 0)).view(-1) if select.size(0) == 0: continue centers, widths, quaternions = centers[select], widths[ select], quaternions[select] select_pred_pairs = pred_pairs[select] posi_scores = posi_scores[select] prop_score = prop_score[select] pred_angle = pred_angle[select].view(-1) posi_contacts = select_pred_pairs[:, 0] posi_contacts_cpu = posi_contacts.cpu().numpy() posi_scores_cpu = posi_scores.cpu().numpy() centers = centers.cpu().numpy() widths = widths.cpu().numpy() quaternions = quaternions.cpu().numpy() prop_score_cpu = prop_score.cpu().numpy() pred_angle_cpu = pred_angle.cpu().numpy() assert pred_angle_cpu.shape[0] == centers.shape[0] assert posi_contacts_cpu.shape[0] == centers.shape[0] print('posi grasp num:', posi_scores.size(0)) all_grasps_path = os.path.join(all_grasps_dir, shape + '.npz') np.savez(all_grasps_path, widths=widths, centers=centers, quaternions=quaternions, scores=posi_scores_cpu, contacts=posi_contacts_cpu, angles=pred_angle_cpu) st = time.time() keep = nms2(centers, quaternions, posi_scores_cpu, cent_th=0.04, ang_th=np.pi / 3) keep = np.array(keep, dtype=np.int32) print('nms time: ', time.time() - st) centers = centers[keep] widths = widths[keep] quaternions = quaternions[keep] log_all.write(shape + '\n') for i in range(centers.shape[0]): w = widths[i] c = centers[i] q = quaternions[i] log_all.write('%f,%f,%f,%f,%f,%f,%f\n' % (c[0], c[1], c[2], q[0], q[1], q[2], q[3])) log_all.flush() print(max(time_list), min(time_list), np.mean(time_list)) log.write('min time%f\n' % (min(time_list))) log.write('max time%f\n' % (max(time_list))) log.write('mean time%f\n' % (np.mean(time_list))) log.write(str(time_list) + '\n')