'-p',
type=int,
help='0: TP FP FN dx dy dz angle '
'1: TP FP FN dx dy dz dl angle '
'2: TP FP FN dl angle ',
default=0)
args = parser.parse_args()
print_mode = args.print_mode
# gan
base_dir = args.gan_diff_directory
paths = list(sorted(Path(base_dir).glob('*.json')))
path = paths[1]
data = load_json(str(path))
result = {}
# only for hanging
two_labels_category = ['037_scissors', '048_hammer']
one_tp_par_one_data = True
total = {
'tp': 0,
'fp': 0,
'fn': 0,
'pos_diff': [],
'distance': [],
'angle': []
'--input', '-i', type=str,
help='input annotation directory which has pcd and json.',
default='/media/kosuke55/SANDISK/meshdata/ycb_hanging_object/real_ycb_annotation') # noqa
args = parser.parse_args()
annotation_dir = args.input
pcd_paths = list(sorted(Path(annotation_dir).glob('*.pcd')))
pcd_path = pcd_paths[0]
for pcd_path in pcd_paths:
pcd = o3d.io.read_point_cloud(str(pcd_path))
paths = list(sorted(Path('./') .glob('*.json')))
pose_path = pcd_path.with_suffix('.json')
contact_points_dict = load_json(str(pose_path))
contact_points = contact_points_dict['contact_points']
contact_point_sphere_list = []
for i, cp in enumerate(contact_points):
contact_point_sphere = skrobot.model.Axis(0.003, 0.05)
contact_point_sphere.newcoords(
skrobot.coordinates.Coordinates(pos=cp[0], rot=cp[1:]))
contact_point_sphere_list.append(contact_point_sphere)
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 640))
trimesh_pc = trimesh.PointCloud(
np.asarray(pcd.points), np.asarray(pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--input-dir', '-i', type=str,
help='input urdf',
default='/media/kosuke55/SANDISK-2/meshdata/ycb_eval/019_pitcher_base/pocky-2020-10-17-06-01-16-481902-45682') # noqa
parser.add_argument(
'--idx', type=int,
help='data idx',
default=0)
parser.add_argument(
'--large-axis', '-la', action='store_true',
help='use large axis as visualizing marker')
args = parser.parse_args()
base_dir = args.input_dir
start_idx = args.idx
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
for idx in range(start_idx, 100000):
print(idx)
if idx != start_idx:
viewer.delete(pc) # noqa
for c in contact_point_marker_list: # noqa
viewer.delete(c)
annotation_path = osp.join(
base_dir, 'annotation', '{:06}.json'.format(idx))
annotation_data = load_json(annotation_path)
color_path = osp.join(base_dir, 'color', '{:06}.png'.format(idx))
color = o3d.io.read_image(color_path)
depth_path = osp.join(base_dir, 'depth', '{:06}.npy'.format(idx))
depth = np.load(depth_path)
depth = o3d.geometry.Image(depth)
camera_info_path = osp.join(
base_dir, 'camera_info', '{:06}.yaml'.format(idx))
cameramodel = cameramodels.PinholeCameraModel.from_yaml_file(
camera_info_path)
intrinsics = cameramodel.open3d_intrinsic
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd, intrinsics)
contact_point_marker_list = []
# for manual annotaion which have labels
if 'label' in annotation_data[0]:
labels = [a['label'] for a in annotation_data]
color_map = label_colormap(max(labels) + 1)
for annotation in annotation_data:
cx = annotation['xy'][0]
cy = annotation['xy'][1]
q = np.array(annotation['quaternion'])
dep = annotation['depth']
color = [255, 0, 0]
if 'label' in annotation:
label = annotation['label']
color = color_map[label]
print(cx, cy)
pos = np.array(
cameramodel.project_pixel_to_3d_ray([cx, cy]))
length = dep * 0.001 / pos[2]
pos = pos * length
if args.large_axis:
contact_point_marker = skrobot.model.Axis(0.003, 0.05)
else:
contact_point_marker = skrobot.model.Sphere(0.003, color=color)
contact_point_marker.newcoords(
skrobot.coordinates.Coordinates(pos=pos, rot=q))
viewer.add(contact_point_marker)
contact_point_marker_list.append(contact_point_marker)
trimesh_pc = trimesh.PointCloud(
np.asarray(
pcd.points), np.asarray(
pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
viewer.add(pc)
if idx == start_idx:
viewer.show()
input('Next data?: [ENTER]')
first = True
try:
for color_path in color_paths:
if gui or save_3d_image:
if not first:
viewer.delete(pc)
for c in contact_point_marker_list:
viewer.delete(c)
if visualize_gt:
for gt_c in gt_contact_point_marker_list:
viewer.delete(gt_c)
if is_sim_data:
annotation_path = color_path.parent.parent / \
'annotation' / color_path.with_suffix('.json').name
annotation_data = load_json(str(annotation_path))
camera_info_path = color_path.parent.parent / \
'camera_info' / color_path.with_suffix('.yaml').name
depth_path = color_path.parent.parent / \
'depth' / color_path.with_suffix('.npy').name
camera_model = cameramodels.PinholeCameraModel.from_yaml_file(
str(camera_info_path))
camera_model.target_size = target_size
intrinsics = camera_model.open3d_intrinsic
cv_bgr = cv2.imread(str(color_path))
cv_bgr = cv2.resize(cv_bgr, target_size)
cv_rgb = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2RGB)
color = o3d.geometry.Image(cv_rgb)
interpolation=cv2.INTER_NEAREST)
depth = o3d.geometry.Image(cv_depth)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsics)
trimesh_pc = trimesh.PointCloud(np.asarray(pcd.points),
np.asarray(pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
idx = int(regex.findall(color_path.stem)[0])
if is_sim_data:
category = color_path.parent.parent.parent.name
annotation_data = load_json(str(annotation_path))
contact_points = []
for annotation in annotation_data:
cx = annotation['xy'][0]
cy = annotation['xy'][1]
q = np.array(annotation['quaternion'])
dep = annotation['depth']
pos = np.array(camera_model.project_pixel_to_3d_ray([cx, cy]))
length = dep * 0.001 / pos[2]
pos = pos * length
coords = skrobot.coordinates.Coordinates(pos=pos, rot=q)
contact_point = np.concatenate(
[coords.T()[:3, 3][None, :],
coords.T()[:3, :3]]).tolist()
contact_points.append(contact_point)
else:
def __getitem__(self, idx):
depth_filepath = self.file_paths[idx]
depth = np.load(depth_filepath).astype(np.float32)
if self.test:
depth = resize_np_img(depth, self.data_shape, Image.NEAREST)
else:
depth = depth_edges_erase(depth)
depth = self.aug_seq.augment_image(depth)
if self.data_augmentation:
nonzero_depth = depth.copy()
nonzero_depth[nonzero_depth == 0] = depth.max()
depth_eraser = get_random_eraser(p=0.9,
s_l=0.1,
s_h=0.5,
v_l=nonzero_depth.min(),
v_h=depth.max(),
pixel_level=True)
depth = depth_eraser(depth)
# r = np.random.randint(20)
# kernel = np.ones((r, r), np.uint8)
# depth = cv2.dilate(depth, kernel, iterations=1)
# r = np.random.randint(20)
# r = r if np.mod(r, 2) else r + 1
# depth = cv2.GaussianBlur(depth, (r, r), 10)
camera_info_path = str(depth_filepath.parent.parent / 'camera_info' /
depth_filepath.with_suffix('.yaml').name)
if self.use_bgr:
depth_bgr = colorize_depth(depth.copy(), self.depth_range[0],
self.depth_range[1])
color = cv2.imread(
str(depth_filepath.parent.parent / 'color' /
depth_filepath.with_suffix('.png').name), cv2.IMREAD_COLOR)
if self.test:
color = resize_np_img(color, self.data_shape)
# else:
# color = self.aug_seq.augment_image(color)
if self.use_bgr2gray:
# 0~1
gray = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY)[...,
None] / 255.
gray = gray.astype(np.float32)
normalized_depth = normalize_depth(depth, self.depth_range[0],
self.depth_range[1])[...,
None]
# -1~1
# gray = gray * 2 - 1
# depth = depth * 2 - 1
# 2 channels
in_feature = np.concatenate((normalized_depth, gray), axis=2)
else:
# 6 channels
in_feature = np.concatenate((depth_bgr, color), axis=2)
else:
in_feature = depth * 0.001
if self.test:
if self.transform:
in_feature = self.transform(in_feature)
depth = self.transform(depth)
return in_feature, depth, camera_info_path, 'dummy', 'dummy'
annotation_data = load_json(
str(depth_filepath.parent.parent / 'annotation' /
depth_filepath.with_suffix('.json').name))
confidence = np.zeros(self.data_shape, dtype=np.uint32)
# depth_map = DepthMap(
# self.data_shape[1],
# self.data_shape[0],
# circular=True)
# rotation_map = RotationMap(self.data_shape[1], self.data_shape[0])
for i, annotation in enumerate(annotation_data):
px = annotation['xy'][0]
py = annotation['xy'][1]
annotation_data[i]['depth'] = normalize_depth(
annotation_data[i]['depth'], self.depth_range[0],
self.depth_range[1])
# depth_value = annotation['depth']
# quaternion = np.array(annotation['quaternion'])
create_gradient_circle(confidence, py, px)
# rotation_map.add_quaternion(
# px, py, quaternion)
# depth_map.add_depth(px, py, depth_value)
confidence = (confidence / confidence.max()).astype(np.float32)
# rotations = rotation_map.rotations.astype(np.float32)
# hanging_point_depth \
# = depth_map.on_depth_image(depth).astype(np.float32)
# clip_info = np.load(
# depth_filepath.parent.parent / 'clip_info' / depth_filepath.name)
# confidence = cv2.imread(
# str(depth_filepath.parent.parent / 'heatmap' /
# depth_filepath.with_suffix('.png').name),
# cv2.IMREAD_GRAYSCALE).astype(np.float32)
# confidence /= 255.
# hanging_point_depth = np.load(
# depth_filepath.parent.parent / 'hanging_points_depth' /
# depth_filepath.name).astype(np.float32)
# hanging_point_depth = normalize_depth(
# hanging_point_depth, self.depth_range[0], self.depth_range[1])
# rotations = np.load(
# depth_filepath.parent.parent / 'rotations' /
# depth_filepath.name).astype(np.float32)
# ground_truth = np.concatenate(
# [confidence[..., None],
# hanging_point_depth[..., None],
# rotations], axis=2)
ground_truth = confidence[..., None]
if self.transform:
in_feature = self.transform(in_feature)
depth = self.transform(depth)
ground_truth = self.transform(ground_truth)
return in_feature, depth, camera_info_path, ground_truth, annotation_data