def evaluate_uncertainty(self,
mean_pts2d,
covar,
pose_targets,
class_type,
intri_type='blender',
vote_type=VotingType.BB8,
intri_matrix=None):
points_3d = VotingType.get_pts_3d(vote_type, class_type)
begin = time.time()
# full
cov_invs = []
for vi in range(covar.shape[0]):
if covar[vi, 0, 0] < 1e-6 or np.sum(np.isnan(covar)[vi]) > 0:
cov_invs.append(np.zeros([2, 2]).astype(np.float32))
continue
cov_inv = np.linalg.inv(scipy.linalg.sqrtm(covar[vi]))
cov_invs.append(cov_inv)
cov_invs = np.asarray(cov_invs) # pn,2,2
weights = cov_invs.reshape([-1, 4])
weights = weights[:, (0, 1, 3)]
if intri_type == 'use_intrinsic' and intri_matrix is not None:
K = intri_matrix
else:
K = self.projector.intrinsic_matrix[intri_type]
pose_pred = uncertainty_pnp(mean_pts2d, weights, points_3d, K)
model = self.linemod_db.get_ply_model(class_type)
diameter = self.linemod_db.get_diameter(class_type)
self.uncertainty_pnp_cost.append(time.time() - begin)
if class_type in ['eggbox', 'glue']:
self.add_metric_sym(pose_pred, pose_targets, model, diameter)
else:
self.add_metric(pose_pred, pose_targets, model, diameter)
self.projection_2d(pose_pred, pose_targets, model, K)
self.cm_degree_5_metric(pose_pred, pose_targets)
return pose_pred
def __update_ax__(self, ax, index):
print('asasdasdasdasd')
index = index % self.len
# Clear axes
ax.clear()
ax.set_title("Viewpoint {}".format(index))
# Load image
imgPath = os.path.join(self.imgDir, str(index+1).zfill(self.formats['rgbNLeadingZeros'])+'.'+self.formats['rgbFormat'])
img = self.read_img_np(imgPath)
height, width, _ = img.shape
ax.imshow(img)
# Scatter points and project poses
x = self.points[index]
if x is not None:
nInstances = x.shape[0]
colorCycler = pltColorCycler()
for iInstance in range(nInstances):
color = next(colorCycler)
ax.scatter(x[iInstance,:,0], x[iInstance,:,1], c=color, edgecolors='black')
covar = self.covariance[index][iInstance]
if covar is not None:
print(covar)
weights = covar_to_weight(covar)
pose = uncertainty_pnp(x[iInstance], weights, self.keypoints3D, self.K)
poseBajs = pnp(self.keypoints3D, x[iInstance], self.K)
print(pose)
bbProj = project(self.bbCorners.T, self.K@pose)
bbProjBajs = project(self.bbCorners.T, self.K@poseBajs)
#ax.scatter(bbProj[:,0], bbProj[:,1], marker='*', c=color)
ax.plot(bbProj[self.cornerOrder, 0], bbProj[self.cornerOrder, 1], c=color)
ax.plot(bbProjBajs[self.cornerOrder, 0], bbProjBajs[self.cornerOrder, 1], c='brown')
for iKeypoint in range(covar.shape[0]):
thisCov = covar[iKeypoint]
(eigVal, eigVec) = np.linalg.eig(thisCov)
for iDir in range(2):
ax.arrow(x[iInstance,iKeypoint,0], x[iInstance,iKeypoint,1], np.sqrt(eigVal[iDir])*eigVec[0,iDir], np.sqrt(eigVal[iDir])*eigVec[1,iDir])
ax.set_xlim(0, width)
ax.set_ylim(height, 0)
def demo():
net = Resnet18_8s(ver_dim=vote_num * 2, seg_dim=2)
net = NetWrapper(net).cuda()
net = DataParallel(net)
optimizer = optim.Adam(net.parameters(), lr=train_cfg['lr'])
model_dir = os.path.join(cfg.MODEL_DIR, 'cat_demo')
load_model(net.module.net, optimizer, model_dir, args.load_epoch)
data, points_3d, bb8_3d = read_data()
image, mask, vertex, vertex_weights, pose, corner_target = [
d.unsqueeze(0).cuda() for d in data
]
# Run the net
seg_pred, vertex_pred, loss_seg, loss_vertex, precision, recall = net(
image, mask, vertex, vertex_weights)
print('vertex_pred.shape')
print(vertex_pred.shape)
print(' ')
print('vertex_pred[0]')
print(vertex_pred)
print(' ')
# Various visualizations
#visualize_vertex_field(vertex_pred,vertex_weights, keypointIdx=3)
print('asdasdsadas')
print(seg_pred.shape, mask.shape)
visualize_mask(np.squeeze(seg_pred.cpu().detach().numpy()),
mask.cpu().detach().numpy())
rgb = Image.open('data/demo/cat.jpg')
img = np.array(rgb)
#visualize_overlap_mask(img, np.squeeze(seg_pred.cpu().detach().numpy()), None)
# Run the ransac voting
eval_net = DataParallel(EvalWrapper2().cuda())
#corner_pred = eval_net(seg_pred, vertex_pred).cpu().detach().numpy()[0]
corner_pred, covar = [
x.cpu().detach().numpy()[0] for x in eval_net(seg_pred, vertex_pred)
]
print('Keypoint predictions:')
print(corner_pred)
print(' ')
print('covar: ', covar)
print(' ')
camera_matrix = np.array([[572.4114, 0., 325.2611],
[0., 573.57043, 242.04899], [0., 0., 1.]])
# Fit pose to points
#pose_pred = pnp(points_3d, corner_pred, camera_matrix)
#evaluator = Evaluator()
#pose_pred = evaluator.evaluate_uncertainty(corner_pred,covar,pose,'cat',intri_matrix=camera_matrix)
def getWeights(covar):
cov_invs = []
for vi in range(covar.shape[0]): # For every keypoint
if covar[vi, 0, 0] < 1e-6 or np.sum(np.isnan(covar)[vi]) > 0:
cov_invs.append(np.zeros([2, 2]).astype(np.float32))
continue
cov_inv = np.linalg.inv(scipy.linalg.sqrtm(covar[vi]))
cov_invs.append(cov_inv)
cov_invs = np.asarray(cov_invs) # pn,2,2
weights = cov_invs.reshape([-1, 4])
weights = weights[:, (0, 1, 3)]
return weights
weights = getWeights(covar)
pose_pred = uncertainty_pnp(corner_pred, weights, points_3d, camera_matrix)
print('Predicted pose: \n', pose_pred)
print('Ground truth pose: \n', pose[0].detach().cpu().numpy())
print(' ')
projector = Projector()
bb8_2d_pred = projector.project(bb8_3d, pose_pred, 'linemod')
bb8_2d_gt = projector.project(bb8_3d, pose[0].detach().cpu().numpy(),
'linemod')
image = imagenet_to_uint8(image.detach().cpu().numpy())[0]
visualize_points(image[None, ...],
corner_target.detach().cpu().numpy(),
pts_pred=corner_pred[None, :, :])
visualize_bounding_box(image[None, ...], bb8_2d_pred[None, None, ...],
bb8_2d_gt[None, None, ...])
