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