def eval_epoch(self, d_loader, is_test=False):
self.model.eval()
eval_dict = {}
total_loss = 0.0
count = 1.0
for i, data in tqdm.tqdm(enumerate(d_loader), leave=False, desc="val"):
while not rospy.is_shutdown():
self.optimizer.zero_grad()
preds, loss, eval_res = self.model_fn(self.model,
data,
is_eval=True,
is_test=is_test)
pcld = preds[4].cpu().numpy()[0]
pcld = np.array(pcld, dtype=np.float32)
rgb_img = preds[6].cpu().numpy()[0].transpose()
rgb_img = rgb_img.transpose(1, 0, 2)
pose = np.array(preds[3])
gt_pose = preds[5].cpu().numpy()
pred_kps = preds[7] #.cpu().numpy()[0]
#print(pred_kps)
_, classes_rgbd = torch.max(preds[1], -1)
#classes_rgbd = preds[1]
classes_rgbd = classes_rgbd.cpu().numpy()
rgb_img = np.array(rgb_img, dtype=np.uint8)
'''
print('gt_pose '+str(gt_pose.shape))
print('pose '+str(np.array(pose).shape))
print('kps '+ str(np.array(pred_kps).shape))
print('classes rgbd '+str(classes_rgbd.shape))
print('unique labels '+str(np.unique(classes_rgbd)))
print('')'''
print('pose ' + str(np.array(pose).shape))
#print('pose[0] '+str(pose[0,:,:]) )
rgb_labeled = rgb_img.copy()
pose_msg = []
pose_msg2 = []
mask = np.zeros((1), dtype=np.uint32)
rgb = np.zeros((pose.shape[0], 3), dtype=np.uint32)
rgb_mask = []
for cls in range(pose.shape[0]):
rot = R.from_dcm(pose[cls, 0:3, 0:3])
quat = rot.as_quat()
rot_gt = R.from_dcm(gt_pose[0, cls, 0:3, 0:3])
quat_gt = rot_gt.as_quat()
registered_corners = np.matmul(pose[cls, 0:3, 0:3],
corners_t[cls, :, :].T)
registered_corners += pose[cls, :3, 3].reshape(3, 1)
# Image with all the Bounding boxes - Takes longer per iteration
corners_2d = bs_utl.project_p3d(
registered_corners.T,
1,
K=config.intrinsic_matrix['custom'])
rgb_labeled_bb = bs_utl.draw_bounding_box(
rgb_labeled, corners_2d)
'''
pose_msg.append(Pose(position = Point(x=pose[cls,0,3],y=pose[cls,1,3],z=pose[cls, 2,3]), orientation = Quaternion(x=quat[0],y=quat[1],z=quat[2],w=quat[3])))
pose_msg2.append(Pose(position = Point(x=gt_pose[0,cls,0,3],y=gt_pose[0,cls,1,3],z=gt_pose[0,cls,2,3]), orientation = Quaternion(x=quat_gt[0],y=quat_gt[1],z=quat_gt[2],w=quat_gt[3])))
'''
this_mask = np.where(classes_rgbd[0, :] == cls + 1)[0]
rgb_mask.append(this_mask.shape[0])
this_rgb = bs_utl.get_label_color(cls_id=cls + 1)
rgb[cls, :] = np.array(
[this_rgb[0], this_rgb[1], this_rgb[2]])
mask = np.concatenate((mask, this_mask), axis=0)
cls = 0
if pose.shape[0] > cls:
rot = R.from_dcm(pose[cls, 0:3, 0:3])
quat = rot.as_quat()
rot_gt = R.from_dcm(gt_pose[0, cls, 0:3, 0:3])
quat_gt = rot_gt.as_quat()
pose_msg.append(
Pose(position=Point(x=pose[cls, 0, 3],
y=pose[cls, 1, 3],
z=pose[cls, 2, 3]),
orientation=Quaternion(x=quat[0],
y=quat[1],
z=quat[2],
w=quat[3])))
pose_msg2.append(
Pose(position=Point(x=gt_pose[0, cls, 0, 3],
y=gt_pose[0, cls, 1, 3],
z=gt_pose[0, cls, 2, 3]),
orientation=Quaternion(x=quat_gt[0],
y=quat_gt[1],
z=quat_gt[2],
w=quat_gt[3])))
rgb = np.repeat(rgb, rgb_mask, axis=0)
p3d_rgbs = np.concatenate((pcld[mask[1:], 0:3], rgb),
axis=1)
#p2ds = bs_utl.project_p3d(pcld[mask,0:3], 1 , K = config.intrinsic_matrix['openDR'] )
p2ds_rgb = bs_utl.project_p3d(
p3d_rgbs, 1, K=config.intrinsic_matrix['CrankSlider'])
#print(p2ds.shape)
r = rgb[0, 0] #np.random.randint(0,255)
g = rgb[1, 0] #np.random.randint(0,255)
b = rgb[2, 0] #np.random.randint(0,255)
#registered_pts = (np.matmul(pose[:3,:3],mesh_pts_t.T)+pose[:,3].reshape(3,1)).T
'''
# Image with single bounding box of the selected the cls - for faster visualization
registered_corners = np.matmul(pose[cls, 0:3,0:3], corners_t[cls,:,:].T)
registered_corners+= pose[cls,:3,3].reshape(3,1)
corners_2d = bs_utl.project_p3d(registered_corners.T, 1 , K = config.intrinsic_matrix['custom'] )'''
#mesh_p2ds = bs_utl.project_p3d(registered_pts, 1 , K = config.intrinsic_matrix['custom'] )
kps_2d = bs_utl.project_p3d(
pred_kps[cls], 1, K=config.intrinsic_matrix['custom'])
## Projects 2D points from all labels together with their respective colors
rgb_labeled = bs_utl.draw_p2ds(rgb_labeled, p2ds_rgb,
(r, g, b), 1)
#rgb_labeled_kps = bs_utl.draw_p2ds(rgb_labeled, kps_2d, (0, 255, 0), 2)
#rgb_labeled_bb = bs_utl.draw_bounding_box(rgb_labeled_kps, corners_2d)
#rgb_labeled_axis = bs_utl.draw_axis(rgb_labeled, gt_pose[0:3,0:3], gt_pose[:,3], config.intrinsic_matrix['custom'])
#rgb_labeled = rgb_img
#rgb_labeled_kps = bs_utl.draw_p2ds( rgb_labeled ,kps_2d, (255,0,0),2 )
#print(rgb_labeled.max())
#print(rgb_labeled.min())
#print(pcld.shape)
#rgb_labeled.dtype = np.uint16
#print(rgb_labeled.shape)
cloud = pcl.PointCloud()
kps = pcl.PointCloud()
kps.from_array(pred_kps[cls])
cloud.from_array(pcld[:, 0:3])
#colors = 200*np.ones((pcld.shape[0],3))
#colors[mask[1:],:] = p3d_rgbs[:,3:6]
colors = pcld[:, 3:6]
#colors[mask,:]= [r,g,b]
cloud_out = pcl_helper.XYZ_to_XYZRGB(
cloud, colors.tolist())
cloud_msg = pcl_helper.pcl_to_ros(cloud_out, 'world')
kp_cld_msg = pcl_helper.pcl_to_ros(kps, 'world')
#cld_registered = pcl.PointCloud()
#cld_registered.from_array(np.array(registered_pts, dtype=np.float32) )
pub.publish(cloud_msg)
pub2.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id='world'),
poses=pose_msg))
pub3.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id='world'),
poses=pose_msg2))
pub4.publish(
ros_numpy.msgify(Image, rgb_labeled, encoding='rgb8'))
pub5.publish(kp_cld_msg)
#pub6.publish(pcl_helper.pcl_to_ros(cld_registered, 'world'))
'''
try:
usrIn = input('Enter to continue to next Test Image...')
except:
usrIn = None'''
break
if 'loss_target' in eval_res.keys():
total_loss += eval_res['loss_target']
else:
total_loss += loss.item()
count += 1
for k, v in eval_res.items():
if v is not None:
eval_dict[k] = eval_dict.get(k, []) + [v]
if is_test:
self.model_fn(self.model,
data,
is_eval=True,
is_test=is_test,
finish_test=True)
return total_loss / count, eval_dict
cls_ids = args.cls_id
if args.dataset == 'openDR':
cls_ids = [1,2,3,4,5,6,7,8,9,10]
elif args.dataset == 'CrankSlider':
cls_ids = [1,2,3,4,5,6,7,8]
for cls_id in cls_ids:
print('Writing kps for class '+str(cls_id))
pcd = pcl.load('./datasets/'+args.dataset+'/'+dataset_folder[args.dataset]+'/models/obj_'+str(cls_id)+'.pcd')
pts = np.array(pcd)
fps_idx = bs_utl.farthestPointSampling(pts, args.n_kps)
colors = 100*np.ones((pts.shape[0],3))
colors[fps_idx] = np.array([255, 0 , 0])
colors2 = 100*np.ones((pts[fps_idx].shape[0],3))
#colors[:] = np.array([255, 0 , 0])
colors = colors.tolist()
#cloud_kps = pch.XYZ_to_XYZRGB(pts[fps_idx], colors2)
cloud_kps = pch.XYZ_to_XYZRGB(pts, colors)
np.savetxt('./datasets/'+args.dataset+'/'+kps_folder[args.dataset]+'/'+str(cls_id)+'/farthest_'+str(args.n_kps)+'.txt', pts[fps_idx]) #Keypoints
pcd = pcl.save(cloud_kps,'./datasets/'+args.dataset+'/'+kps_folder[args.dataset]+'/'+str(cls_id)+'/farthest_'+str(args.n_kps)+'.pcd') #PCD file with keypoints added to the actual cloud
if (j == 0):
y = np.min(pts[:, 1])
else:
y = np.max(pts[:, 1])
for k in range(0, 2):
if (k == 0):
z = np.min(pts[:, 2])
else:
z = np.max(pts[:, 2])
corners.append([x, y, z])
corners = np.array(corners)
#print(corners.shape)
pts = np.vstack((pts, corners))
colors = 100 * np.ones((pts.shape[0], 3))
colors[-8:-1, :] = np.array([255, 0, 0])
colors = colors.tolist()
cloud_corners = pch.XYZ_to_XYZRGB(pts, colors)
np.savetxt('./datasets/' + args.dataset + '/' + kps_folder[args.dataset] +
'/' + str(cls_id) + '/corners.txt', corners) #corners
pcd = pcl.save(
cloud_corners, './datasets/' + args.dataset + '/' +
kps_folder[args.dataset] + '/' + str(cls_id) +
'/corners.pcd') ##PCD file with corners added to the actual cloud
def publishAll(self, rgb_img, cls_ids, poses, pcld, kps):
registered_pts = np.zeros((1,3))
rgb_labeled_bb = rgb_img.copy()
poses_msg = []
try:
## Iterate and publish for all the available cls_ids in a frame ##
for i, cls_id in enumerate(np.unique( cls_ids[cls_ids.nonzero()] )):
mask = np.where(cls_ids[0,:] == cls_id)[0]
pose = poses[i]
'''
#This is to rectify the mistake during training - The following labels are symetric objects in dataset which was not
# indicated during the training, hence their detected poses are subject to rotational ambiguity along z-axis i.e., because
# of symmetry they look similar after every 90 degrees of rotation along z-axis
if int(cls_id) in [1,2,3,5,7]:
print('Symmetric class detected. Wrapping yaw...')
pose_np = pose.copy()#ros_numpy.numpify(pose)
pose_eul = R.from_dcm(pose_np[:3, :3]).as_euler('zyx', degrees=True)
print('detected yaw: '+str(pose_eul[0]))
if pose_eul[0] > 90:
pose_eul[0] = 180 - pose_eul[0]
elif pose_eul[0] < -90:
pose_eul[0] = 180 + pose_eul[0]
print('yaw rectified '+str(pose_eul[0]))
pose_np[:3, :3] = R.from_euler('zyx', pose_eul, degrees=True).as_dcm()
pose = pose_np#ros_numpy.msgify(Pose, pose_np)'''
kp = kps[i]
p2ds = bs_utils.project_p3d(pcld[mask,0:3], 1 , K = config.intrinsic_matrix['custom'] )
registered_pts = (np.matmul(pose[:3,:3],self.mesh_pts_t.T)+pose[:,3].reshape(3,1)).T
registered_corners = (np.matmul(pose[:3,:3],self.corners[str(cls_id)].T)+pose[:,3].reshape(3,1)).T
mesh_p2ds = bs_utils.project_p3d(registered_pts, 1 , K = config.intrinsic_matrix['custom'] )
kps_2d = bs_utils.project_p3d(kp, 1 , K = config.intrinsic_matrix['custom'] )
corners_2d = bs_utils.project_p3d(registered_corners, 1 , config.intrinsic_matrix['custom'] )
rgb_labeled = bs_utils.draw_p2ds(rgb_labeled , p2ds, bs_utils.get_label_color( cls_id=cls_id), 1)
rgb_kps = bs_utils.draw_p2ds(rgb_kps, kps_2d, (255, 0, 0), 2)
rgb_labeled_bb = bs_utils.draw_bounding_box(rgb_labeled_bb, corners_2d)
optical2cam = R.from_euler('zyx',[1.57, 0, 1.57]).as_dcm()
optical2cam = np.hstack(( optical2cam, np.array([0,0,0]).reshape(3,1) ))
R_tf = np.matmul(optical2cam.as_dcm(), pose[:3,:3])
t_tf = pose[:,3]
t_tf = np.array([ -t_tf[1], -t_tf[2], t_tf[0] ]).reshape(3,1)
pose_tf = np.matmul( pose.T, optical2cam).T
pose_tf = np.concatenate((R_tf, t_tf), axis=1)
pose_tf = pose
rot = R.from_dcm(pose[0:3,0:3])
quat = rot.as_quat()
registered_mesh = (np.matmul(pose_tf[:3,:3],self.mesh[str(cls_id)].T)+pose_tf[:,3].reshape(3,1)).T
registered_pts = np.concatenate((registered_pts, registered_mesh), axis = 0)
poses_msg.append(Pose(position = Point(x=pose[0,3],y=pose[1,3],z=pose[2,3]), orientation = Quaternion(x=quat[0],y=quat[1],z=quat[2],w=quat[3])))
cld_registered = pcl.PointCloud()
cld_registered.from_array(np.array(registered_pts, dtype=np.float32) )
cloud = pcl.PointCloud()
kps = pcl.PointCloud()
kps.from_array(pred_kps[cls])
cloud.from_array(pcld[:,0:3])
#colors = 200*np.ones((pcld.shape[0],3))
colors = pcld[:,3:6]
#colors[mask[1:],:] = p3d_rgbs[:,3:6]
#colors[mask,:]= [r,g,b]
cloud_out = pcl_helper.XYZ_to_XYZRGB(cloud, colors.tolist())
cloud_msg = pcl_helper.pcl_to_ros(cloud_out, 'camera_optical_link')
#kp_cld_msg = pcl_helper.pcl_to_ros(kps, 'camera_optical_link')
print('Publishing All...')
self.pub1.publish(cloud_msg)
self.pub2.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.cam_frame), poses = poses_msg))
self.pub4.publish(ros_numpy.msgify(Image, rgb_labeled_bb,encoding='rgb8'))
self.pub5.publish(ros_numpy.msgify(Image, rgb_kps,encoding='rgb8'))
self.pub6.publish(pcl_helper.pcl_to_ros(cld_registered, 'camera_optical_link'))
print('All published...')
except Exception as inst:
exc_type, exc_obj, exc_tb = sys.exc_info()
print('exception: '+str(inst)+' in '+ str(exc_tb.tb_lineno))
return None