def eval_one_frame_pose_lm(item):
pcld, mask, ctr_of, pred_kp_of, RTs, cls_ids, use_ctr, n_cls, \
min_cnt, use_ctr_clus_flter, label, epoch, ibs, obj_id = item
n_kps, n_pts, _ = pred_kp_of.size()
pred_ctr = pcld - ctr_of[0]
pred_kp = pcld.view(1, n_pts, 3).repeat(n_kps, 1, 1) - pred_kp_of
radius = 0.08
if use_ctr:
cls_kps = torch.zeros(n_cls, n_kps + 1, 3).cuda()
else:
cls_kps = torch.zeros(n_cls, n_kps, 3).cuda()
pred_pose_lst = []
cls_id = 1
cls_msk = mask == cls_id
if cls_msk.sum() < 1:
pred_pose_lst.append(np.identity(4)[:3, :])
else:
cls_voted_kps = pred_kp[:, cls_msk, :]
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
if ctr_labels.sum() < 1:
ctr_labels[0] = 1
if use_ctr:
cls_kps[cls_id, n_kps, :] = ctr
if use_ctr_clus_flter:
in_pred_kp = cls_voted_kps[:, ctr_labels, :]
else:
in_pred_kp = cls_voted_kps
for ikp, kps3d in enumerate(in_pred_kp):
cls_kps[cls_id, ikp, :], _ = ms.fit(kps3d)
mesh_kps = bs_utils_lm.get_kps(obj_id, ds_type="linemod")
if use_ctr:
mesh_ctr = bs_utils_lm.get_ctr(obj_id,
ds_type="linemod").reshape(1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
mesh_kps = torch.from_numpy(mesh_kps.astype(np.float32)).cuda()
pred_RT = bs_utils_lm.best_fit_transform(
mesh_kps.contiguous().cpu().numpy(),
cls_kps[cls_id].squeeze().contiguous().cpu().numpy())
pred_pose_lst.append(pred_RT)
cls_add_dis, cls_adds_dis = eval_metric_lm(cls_ids, pred_pose_lst, RTs,
mask, label, obj_id)
return (cls_add_dis, cls_adds_dis)
def eval_one_frame_pose(item):
pcld, mask, ctr_of, pred_kp_of, RTs, cls_ids, use_ctr, n_cls, \
min_cnt, use_ctr_clus_flter, label, epoch, ibs, ds = item
ds = str(ds)
print("we are looking at ds : ", str(ds))
n_kps, n_pts, _ = pred_kp_of.size()
pred_ctr = pcld - ctr_of[0]
pred_kp = pcld.view(1, n_pts, 3).repeat(n_kps, 1, 1) - pred_kp_of
radius = 0.08
if use_ctr:
cls_kps = torch.zeros(n_cls, n_kps + 1, 3).cuda()
else:
cls_kps = torch.zeros(n_cls, n_kps, 3).cuda()
pred_cls_ids = np.unique(mask[mask > 0].contiguous().cpu().numpy())
print('pred_cls_ids ' + str(pred_cls_ids))
if use_ctr_clus_flter:
ctrs = []
for icls, cls_id in enumerate(pred_cls_ids):
cls_msk = (mask == cls_id)
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
ctrs.append(ctr.detach().contiguous().cpu().numpy())
ctrs = torch.from_numpy(np.array(ctrs).astype(np.float32)).cuda()
n_ctrs, _ = ctrs.size()
pred_ctr_rp = pred_ctr.view(n_pts, 1, 3).repeat(1, n_ctrs, 1)
ctrs_rp = ctrs.view(1, n_ctrs, 3).repeat(n_pts, 1, 1)
ctr_dis = torch.norm((pred_ctr_rp - ctrs_rp), dim=2)
min_dis, min_idx = torch.min(ctr_dis, dim=1)
msk_closest_ctr = torch.LongTensor(pred_cls_ids).cuda()[min_idx]
new_msk = mask.clone()
for cls_id in pred_cls_ids:
if cls_id == 0:
break
if ds == 'ycb':
min_msk = min_dis < config.ycb_r_lst[cls_id - 1] * 0.8
else:
min_msk = min_dis < config_cs.CrankSlider_r_lst[
cls_id - 1] * 0.8 #Changed to CrankSlider
update_msk = (mask > 0) & (msk_closest_ctr == cls_id) & min_msk
new_msk[update_msk] = msk_closest_ctr[update_msk]
mask = new_msk
pred_pose_lst = []
pred_kps_lst = []
for icls, cls_id in enumerate(pred_cls_ids):
if cls_id == 0:
break
cls_msk = mask == cls_id
if cls_msk.sum() < 1:
pred_pose_lst.append(np.identity(4)[:3, :])
continue
cls_voted_kps = pred_kp[:, cls_msk, :]
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
if ctr_labels.sum() < 1:
ctr_labels[0] = 1
if use_ctr:
cls_kps[cls_id, n_kps, :] = ctr
if use_ctr_clus_flter:
in_pred_kp = cls_voted_kps[:, ctr_labels, :]
else:
in_pred_kp = cls_voted_kps
for ikp, kps3d in enumerate(in_pred_kp):
cls_kps[cls_id, ikp, :], _ = ms.fit(kps3d)
if ds == 'ycb':
mesh_kps = bs_utils.get_kps(cls_lst[cls_id - 1])
if use_ctr:
mesh_ctr = bs_utils.get_ctr(cls_lst[cls_id - 1]).reshape(1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
else:
mesh_kps = bs_utils_cs.get_kps(
int(cls_id - 1),
kp_type='farthest_' + str(config_cs.n_keypoints),
ds_type='CrankSlider') #changed to CrankSlider
if use_ctr:
mesh_ctr = bs_utils_cs.get_ctr(int(cls_id - 1),
ds_type='CrankSlider').reshape(
1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
mesh_kps = torch.from_numpy(mesh_kps.astype(np.float32)).cuda()
pred_RT = bs_utils.best_fit_transform(
mesh_kps.contiguous().cpu().numpy(),
cls_kps[cls_id].squeeze().contiguous().cpu().numpy())
pred_pose_lst.append(pred_RT)
pred_kps_lst.append(
cls_kps[cls_id].squeeze().contiguous().cpu().numpy())
cls_add_dis, cls_adds_dis = eval_metric(cls_ids, pred_pose_lst,
pred_cls_ids, RTs, mask, label, ds)
'''
cls_add_dis = []
cls_adds_dis = []'''
return (cls_add_dis, cls_adds_dis, pred_pose_lst, pred_kps_lst)
def cal_frame_poses(pcld, mask, ctr_of, pred_kp_of, use_ctr, n_cls,
use_ctr_clus_flter, ds):
try:
n_kps, n_pts, _ = pred_kp_of.size()
pred_ctr = pcld - ctr_of[0]
pred_kp = pcld.view(1, n_pts, 3).repeat(n_kps, 1, 1) - pred_kp_of
radius = 0.08
if use_ctr:
cls_kps = torch.zeros(n_cls, n_kps + 1, 3).cuda()
else:
cls_kps = torch.zeros(n_cls, n_kps, 3).cuda()
pred_cls_ids = np.unique(mask[mask > 0].contiguous().cpu().numpy())
if use_ctr_clus_flter:
ctrs = []
for icls, cls_id in enumerate(pred_cls_ids):
cls_msk = (mask == cls_id)
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
ctrs.append(ctr.detach().contiguous().cpu().numpy())
ctrs = torch.from_numpy(np.array(ctrs).astype(np.float32)).cuda()
n_ctrs, _ = ctrs.size()
pred_ctr_rp = pred_ctr.view(n_pts, 1, 3).repeat(1, n_ctrs, 1)
ctrs_rp = ctrs.view(1, n_ctrs, 3).repeat(n_pts, 1, 1)
ctr_dis = torch.norm((pred_ctr_rp - ctrs_rp), dim=2)
min_dis, min_idx = torch.min(ctr_dis, dim=1)
msk_closest_ctr = torch.LongTensor(pred_cls_ids).cuda()[min_idx]
new_msk = mask.clone()
for cls_id in pred_cls_ids:
if cls_id == 0:
break
if ds == 'ycb':
min_msk = min_dis < config.ycb_r_lst[cls_id - 1] * 0.8
else:
min_msk = min_dis < config_cs.CrankSlider_r_lst[
cls_id - 1] * 0.8 #Changed to CrankSlider
#min_msk = min_dis < config.ycb_r_lst[cls_id-1] * 0.8
update_msk = (mask > 0) & (msk_closest_ctr == cls_id) & min_msk
new_msk[update_msk] = msk_closest_ctr[update_msk]
mask = new_msk
pred_pose_lst = []
pred_kps_lst = []
for icls, cls_id in enumerate(pred_cls_ids):
if cls_id == 0:
break
cls_msk = mask == cls_id
if cls_msk.sum() < 1:
pred_pose_lst.append(np.identity(4)[:3, :])
continue
cls_voted_kps = pred_kp[:, cls_msk, :]
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
if ctr_labels.sum() < 1:
ctr_labels[0] = 1
if use_ctr:
cls_kps[cls_id, n_kps, :] = ctr
if use_ctr_clus_flter:
in_pred_kp = cls_voted_kps[:, ctr_labels, :]
else:
in_pred_kp = cls_voted_kps
for ikp, kps3d in enumerate(in_pred_kp):
cls_kps[cls_id, ikp, :], _ = ms.fit(kps3d)
if ds == 'ycb':
mesh_kps = bs_utils.get_kps(cls_lst[cls_id - 1])
if use_ctr:
mesh_ctr = bs_utils.get_ctr(cls_lst[cls_id - 1]).reshape(
1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
else:
mesh_kps = bs_utils_od.get_kps(int(cls_id - 1),
kp_type='farthest_' +
str(config_cs.n_keypoints),
ds_type='CrankSlider') #changed
if use_ctr:
mesh_ctr = bs_utils_od.get_ctr(
int(cls_id - 1), ds_type='CrankSlider').reshape(1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
mesh_kps = torch.from_numpy(mesh_kps.astype(np.float32)).cuda()
pred_RT = bs_utils.best_fit_transform(
mesh_kps.contiguous().cpu().numpy(),
cls_kps[cls_id].squeeze().contiguous().cpu().numpy())
pred_pose_lst.append(pred_RT)
pred_kps_lst.append(
cls_kps[cls_id].squeeze().contiguous().cpu().numpy())
return (pred_cls_ids, pred_pose_lst, pred_kps_lst)
except Exception as inst:
exc_type, exc_obj, exc_tb = sys.exc_info()
print('Cal_frame_poses: exception: ' + str(inst) + ' in ' +
str(exc_tb.tb_lineno))
def cal_frame_poses(pcld, mask, ctr_of, pred_kp_of, use_ctr, n_cls, use_ctr_clus_flter):
n_kps, n_pts, _ = pred_kp_of.size()
pred_ctr = pcld - ctr_of[0]
pred_kp = pcld.view(1, n_pts, 3).repeat(n_kps, 1, 1) - pred_kp_of
radius = 0.08
if use_ctr:
cls_kps = torch.zeros(n_cls, n_kps + 1, 3).cuda()
else:
cls_kps = torch.zeros(n_cls, n_kps, 3).cuda()
pred_cls_ids = np.unique(mask[mask > 0].contiguous().cpu().numpy())
if use_ctr_clus_flter:
ctrs = []
for icls, cls_id in enumerate(pred_cls_ids):
cls_msk = (mask == cls_id)
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
ctrs.append(ctr.detach().contiguous().cpu().numpy())
ctrs = torch.from_numpy(np.array(ctrs).astype(np.float32)).cuda()
n_ctrs, _ = ctrs.size()
pred_ctr_rp = pred_ctr.view(n_pts, 1, 3).repeat(1, n_ctrs, 1)
ctrs_rp = ctrs.view(1, n_ctrs, 3).repeat(n_pts, 1, 1)
ctr_dis = torch.norm((pred_ctr_rp - ctrs_rp), dim=2)
min_dis, min_idx = torch.min(ctr_dis, dim=1)
msk_closest_ctr = torch.LongTensor(pred_cls_ids).cuda()[min_idx]
new_msk = mask.clone()
for cls_id in pred_cls_ids:
if cls_id == 0:
break
min_msk = min_dis < config.ycb_r_lst[cls_id - 1] * 0.8
update_msk = (mask > 0) & (msk_closest_ctr == cls_id) & min_msk
new_msk[update_msk] = msk_closest_ctr[update_msk]
mask = new_msk
pred_pose_lst = []
for icls, cls_id in enumerate(pred_cls_ids):
if cls_id == 0:
break
cls_msk = mask == cls_id
if cls_msk.sum() < 1:
pred_pose_lst.append(np.identity(4)[:3, :])
continue
cls_voted_kps = pred_kp[:, cls_msk, :]
ms = MeanShiftTorch(bandwidth=radius)
ctr, ctr_labels = ms.fit(pred_ctr[cls_msk, :])
if ctr_labels.sum() < 1:
ctr_labels[0] = 1
if use_ctr:
cls_kps[cls_id, n_kps, :] = ctr
if use_ctr_clus_flter:
in_pred_kp = cls_voted_kps[:, ctr_labels, :]
else:
in_pred_kp = cls_voted_kps
for ikp, kps3d in enumerate(in_pred_kp):
cls_kps[cls_id, ikp, :], _ = ms.fit(kps3d)
mesh_kps = bs_utils.get_kps(cls_lst[cls_id - 1])
if use_ctr:
mesh_ctr = bs_utils.get_ctr(cls_lst[cls_id - 1]).reshape(1, 3)
mesh_kps = np.concatenate((mesh_kps, mesh_ctr), axis=0)
mesh_kps = torch.from_numpy(mesh_kps.astype(np.float32)).cuda()
pred_RT = bs_utils.best_fit_transform(
mesh_kps.contiguous().cpu().numpy(),
cls_kps[cls_id].squeeze().contiguous().cpu().numpy()
)
pred_pose_lst.append(pred_RT)
return (pred_cls_ids, pred_pose_lst)