def check_objects_centred():
"""
The hand is supposed to be aimed at a point on the object, so central
pixel(s) should be on object, otherwise probably a bug (e.g. something
wrong with how mesh imported into V-REP).
"""
image_dir = '../../grasp-conv/data/obj_depths/'
im_width = 80
camera_offset=.45
near_clip=.25
far_clip=.8
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
import scipy
finger_path = finger_path_template(45.*np.pi/180., im_width, camera_offset)
bad = []
background_threshold = 254
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
image = scipy.misc.imread(image_filename)
if np.min(image[39:41,39:41]) >= background_threshold:
bad.append(f)
print(f)
print(len(bad))
def calculate_metrics(perspectives, im_width=80, fov=45.0, camera_offset=.45):
"""
:param perspectives: numpy array of depth images of object from gripper perspective
"""
asymmetry_scale = 13.0 #TODO: calculate from camera params (13 pixels is ~5cm with default params)
from heuristic import finger_path_template, calculate_grip_metrics
finger_path = finger_path_template(fov * np.pi / 180., im_width,
camera_offset)
collision_template = np.zeros_like(finger_path)
collision_template[finger_path > 0] = camera_offset + 0.033
# print(np.max(collision_template))
# print(np.max(finger_path))
# plt.imshow(collision_template)
# plt.show()
metrics = []
collisions = []
for perspective in perspectives:
intersections, qualities = calculate_grip_metrics(
perspective, finger_path)
q1 = qualities[0]
q2 = qualities[1]
q3 = qualities[2]
if intersections[0] is None or intersections[2] is None:
a1 = 1
else:
a1 = ((intersections[0] - intersections[2]) / asymmetry_scale)**2
if intersections[1] is None or intersections[2] is None:
a2 = 1
else:
a2 = ((intersections[1] - intersections[2]) / asymmetry_scale)**2
m = np.minimum(
(q1 + q2) / 1.5, q3) / (1 + (q1 * a1 + q2 * a2) / (q1 + q2 + 1e-6))
collision = np.max(collision_template - perspective) > 0
collisions.append(collision)
# if collision:
# m = 0
metrics.append(m)
# plt.subplot(1,2,1)
# plt.imshow(perspective)
# plt.subplot(1,2,2)
# plt.imshow(np.maximum(0, finger_path-perspective))
# print(collision)
# print((a1,a2))
# print(intersections)
# print(qualities)
# print('metric: ' + str(m))
# plt.show()
# print((intersections, qualities))
return metrics, collisions
def calculate_overlap_for_directory(image_dir, dest_dir, im_width=80,
camera_offset=.45, far_clip=.8):
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
finger_path = finger_path_template(45.*np.pi/180., im_width, camera_offset)
c = 0
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
if c % 1000 == 0:
print('Processing ' + image_filename)
c += 1
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
overlap = np.maximum(0, finger_path - distance)
imfile = dest_dir + f[:-4] + '-overlap' + '.png'
# we divide by 15.4cm because it's the max overlap due to gripper geometry
Image.fromarray((255.0/.154*overlap).astype('uint8')).save(imfile)
def calculate_overlap_for_directory(image_dir,
dest_dir,
im_width=80,
camera_offset=.45,
far_clip=.8):
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
finger_path = finger_path_template(45. * np.pi / 180., im_width,
camera_offset)
c = 0
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
if c % 1000 == 0:
print('Processing ' + image_filename)
c += 1
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
overlap = np.maximum(0, finger_path - distance)
imfile = dest_dir + f[:-4] + '-overlap' + '.png'
# we divide by 15.4cm because it's the max overlap due to gripper geometry
Image.fromarray(
(255.0 / .154 * overlap).astype('uint8')).save(imfile)
def calculate_grasp_metric_maps_for_directory(image_dir,
dest_dir,
im_width=80,
camera_offset=.45,
near_clip=.25,
far_clip=.8):
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
finger_path = finger_path_template(45. * np.pi / 180., im_width,
camera_offset)
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
print('Processing ' + image_filename)
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
mm = calculate_metric_map(distance, finger_path, 1)
imfile = dest_dir + f[:-4] + '-map' + '.png'
Image.fromarray((255.0 * mm).astype('uint8')).save(imfile)
def check_objects_centred():
"""
The hand is supposed to be aimed at a point on the object, so central
pixel(s) should be on object, otherwise probably a bug (e.g. something
wrong with how mesh imported into V-REP).
"""
image_dir = '../../grasp-conv/data/obj_depths/'
im_width = 80
camera_offset = .45
near_clip = .25
far_clip = .8
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
import scipy
finger_path = finger_path_template(45. * np.pi / 180., im_width,
camera_offset)
bad = []
background_threshold = 254
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
image = scipy.misc.imread(image_filename)
if np.min(image[39:41, 39:41]) >= background_threshold:
bad.append(f)
print(f)
print(len(bad))
def calculate_grasp_metrics_for_directory(image_dir,
im_width=80,
camera_offset=.45,
near_clip=.25,
far_clip=.8):
from os import listdir
from os.path import isfile, join
import time
from heuristic import finger_path_template
from heuristic import calculate_grip_metrics
template = finger_path_template(45. * np.pi / 180., im_width,
camera_offset)
all_intersections = []
all_qualities = []
all_files = []
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
# print('Processing ' + image_filename)
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
# from mpl_toolkits.mplot3d import axes3d, Axes3D
# X = np.arange(0, im_width)
# Y = np.arange(0, im_width)
# X, Y = np.meshgrid(X, Y)
# fig = plt.figure()
# distance[distance > camera_offset + .3] = None
# template[template < camera_offset] = None
# ax = fig.add_subplot(1,1,1,projection='3d')
# ax.plot_wireframe(X, Y, distance)
# ax.plot_wireframe(X, Y, template, color='r')
# ax.set_xlabel('x')
# plt.show()
intersections, qualities = calculate_grip_metrics(
distance, template)
# print(intersections)
# print(qualities)
all_intersections.append(intersections)
all_qualities.append(qualities)
all_files.append(f)
return all_intersections, all_qualities, all_files
def calculate_grasp_metric_maps_for_directory(image_dir, dest_dir, im_width=80,
camera_offset=.45, near_clip=.25, far_clip=.8):
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
finger_path = finger_path_template(45.*np.pi/180., im_width, camera_offset)
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
print('Processing ' + image_filename)
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
mm = calculate_metric_map(distance, finger_path, 1)
imfile = dest_dir + f[:-4] + '-map' + '.png'
Image.fromarray((255.0*mm).astype('uint8')).save(imfile)
def calculate_grasp_metrics_for_directory(image_dir, im_width=80,
camera_offset=.45, near_clip=.25, far_clip=.8):
from os import listdir
from os.path import isfile, join
import time
from heuristic import finger_path_template
from heuristic import calculate_grip_metrics
template = finger_path_template(45.*np.pi/180., im_width, camera_offset)
all_intersections = []
all_qualities = []
all_files = []
for f in listdir(image_dir):
image_filename = join(image_dir, f)
if isfile(image_filename) and f.endswith('.png'):
# print('Processing ' + image_filename)
image = scipy.misc.imread(image_filename)
rescaled_distance = image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
# from mpl_toolkits.mplot3d import axes3d, Axes3D
# X = np.arange(0, im_width)
# Y = np.arange(0, im_width)
# X, Y = np.meshgrid(X, Y)
# fig = plt.figure()
# distance[distance > camera_offset + .3] = None
# template[template < camera_offset] = None
# ax = fig.add_subplot(1,1,1,projection='3d')
# ax.plot_wireframe(X, Y, distance)
# ax.plot_wireframe(X, Y, template, color='r')
# ax.set_xlabel('x')
# plt.show()
intersections, qualities = calculate_grip_metrics(distance, template)
# print(intersections)
# print(qualities)
all_intersections.append(intersections)
all_qualities.append(qualities)
all_files.append(f)
return all_intersections, all_qualities, all_files
def check_overlap_range(image_dir='../../grasp-conv/data/obj_depths', im_width=80, camera_offset=.45, far_clip=.8):
from data import load_all_params
import scipy
objects, gripper_pos, gripper_orient, labels = load_all_params('../../grasp-conv/data/output_data.csv')
seq_nums = np.arange(len(objects)) % 1000
from os import listdir
from os.path import isfile, join
from heuristic import finger_path_template
finger_path = finger_path_template(45.*np.pi/180., im_width, camera_offset)
max_overlaps = []
for object, seq_num in zip(objects, seq_nums):
filename = join(image_dir, object[:-4] + '-' + str(seq_num) + '.png')
image = scipy.misc.imread(filename)
rescaled_distance = image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
max_overlaps.append(np.max(finger_path - distance))
return max_overlaps, labels
rescaled_distance = image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
max_overlaps.append(np.max(finger_path - distance))
return max_overlaps, labels
if __name__ == '__main__':
camera_offset = .45
near_clip = .25
far_clip = .8
fov = 45. * np.pi / 180.
im_width = 80
template = finger_path_template(fov, im_width, camera_offset)
plt.imshow(template)
plt.show()
# import scipy
# image = scipy.misc.imread('../../grasp-conv/data/obj_depths/1_Coffeecup_final-03-Mar-2016-18-50-40-1.png')
# rescaled_distance = image / 255.0
# distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
#
# finger_path = finger_path_template(45.*np.pi/180., 80, camera_offset)
# import time
# start_time = time.time()
# for i in range(100):
# mm = calculate_metric_map(distance, finger_path, 1)
# print('elapsed: ' + str(time.time() - start_time))
#
def get_correlates():
"""
Create a CSV file with a bunch of measures that may be correlates of grasp success.
"""
from heuristic import finger_path_template, calculate_grip_metrics
data_file = '../../grasp-conv/data/output_data.csv'
objects, gripper_pos, gripper_orient, labels, power_pinch = load_all_params(data_file, return_power_pinch=True)
seq_nums = np.arange(len(objects)) % 1000
labels = np.array(labels)
power_pinch = np.array(power_pinch)
camera_offset=.45
far_clip=.8
fov = 45.*np.pi/180.
im_width=80
finger_path = finger_path_template(fov, im_width, camera_offset)
background_threshold = far_clip - .005
result = []
for i in range(len(labels)):
data = [labels[i], power_pinch[i]]
image_filename = join('../../grasp-conv/data/obj_depths', objects[i][:-4] + '-' + str(seq_nums[i]) + '.png')
distance_image = scipy.misc.imread(image_filename)
rescaled_distance = distance_image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
intersections, qualities = calculate_grip_metrics(distance, finger_path)
regions = regions_at_intersections(distance, intersections)
for region in regions:
slope = surface_slope(region, background_threshold)
data.extend(slope[:2])
image_filename = join('../../grasp-conv/data/obj_overlap', objects[i][:-4] + '-' + str(seq_nums[i]) + '-overlap.png')
overlap_image = scipy.misc.imread(image_filename)
regions = regions_at_intersections(overlap_image, intersections, length=3)
for region in regions:
data.append(np.sum(region > 0) / float(region.size))
data.append(angle_from_vertical(gripper_orient[i]))
data.append(object_area(distance, background_threshold))
data.extend(object_centre(distance, background_threshold))
# intersections and quality metrics
intersections = np.where(intersections == np.array(None), im_width, intersections).astype(float)
data.extend(intersections)
data.extend(qualities)
# symmetry
rel_int = intersections / (im_width/2.)
data.append((rel_int[0]-rel_int[2])**2)
data.append((rel_int[1]-rel_int[2])**2)
# intersections with support box
image_filename = join('../../grasp-conv/data/support_depths', objects[i][:-4] + '-' + str(seq_nums[i]) + '.png')
distance_image = scipy.misc.imread(image_filename)
rescaled_distance = distance_image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
support_intersections, support_qualities = calculate_grip_metrics(distance, finger_path)
support_intersections = np.where(support_intersections == np.array(None), im_width, support_intersections).astype(float)
data.extend(support_intersections)
result.append(data)
# fig = plt.figure(figsize=(5,10))
# fig.add_subplot(2,1,1)
# plt.hist(power_pinch[labels<.5], range=(0, .20), bins=100)
# fig.add_subplot(2,1,2)
# plt.hist(power_pinch[labels>.5], range=(0, .20), bins=100)
# plt.show()
return np.array(result)
image = scipy.misc.imread(filename)
rescaled_distance = image / 255.0
distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
max_overlaps.append(np.max(finger_path - distance))
return max_overlaps, labels
if __name__ == '__main__':
camera_offset=.45
near_clip=.25
far_clip=.8
fov = 45.*np.pi/180.
im_width=80
template = finger_path_template(fov, im_width, camera_offset)
plt.imshow(template)
plt.show()
# import scipy
# image = scipy.misc.imread('../../grasp-conv/data/obj_depths/1_Coffeecup_final-03-Mar-2016-18-50-40-1.png')
# rescaled_distance = image / 255.0
# distance = rescaled_distance*(far_clip-camera_offset)+camera_offset
#
# finger_path = finger_path_template(45.*np.pi/180., 80, camera_offset)
# import time
# start_time = time.time()
# for i in range(100):
# mm = calculate_metric_map(distance, finger_path, 1)
# print('elapsed: ' + str(time.time() - start_time))
from mpl_toolkits.mplot3d import axes3d, Axes3D
camera_offset = .5 #distance of camera behind hand
im_width = 40
d = Display(imsize=(im_width,im_width))
rot = rot_matrix(gripper_orient[0], gripper_orient[1], gripper_orient[2])
d.set_camera_position(gripper_pos, rot, camera_offset)
d.set_mesh(verts, faces)
depth = d.read_depth()
d.close()
distance = get_distance(depth, .2, 1.0)
from heuristic import finger_path_template
template = finger_path_template(45.*np.pi/180., im_width, camera_offset)
X = np.arange(0, im_width)
Y = np.arange(0, im_width)
X, Y = np.meshgrid(X, Y)
distance = distance[:,::-1] # x flipped to match V-REP
from heuristic import calculate_grip_metrics
intersections, qualities = calculate_grip_metrics(distance, template)
print(intersections)
print(qualities)
fig = plt.figure()
distance[distance > camera_offset + .3] = None
template[template < camera_offset] = None
from mpl_toolkits.mplot3d import axes3d, Axes3D
camera_offset = .5 #distance of camera behind hand
im_width = 40
d = Display(imsize=(im_width, im_width))
rot = rot_matrix(gripper_orient[0], gripper_orient[1], gripper_orient[2])
d.set_camera_position(gripper_pos, rot, camera_offset)
d.set_mesh(verts, faces)
depth = d.read_depth()
d.close()
distance = get_distance(depth, .2, 1.0)
from heuristic import finger_path_template
template = finger_path_template(45. * np.pi / 180., im_width,
camera_offset)
X = np.arange(0, im_width)
Y = np.arange(0, im_width)
X, Y = np.meshgrid(X, Y)
distance = distance[:, ::-1] # x flipped to match V-REP
from heuristic import calculate_grip_metrics
intersections, qualities = calculate_grip_metrics(distance, template)
print(intersections)
print(qualities)
fig = plt.figure()
distance[distance > camera_offset + .3] = None
template[template < camera_offset] = None
def get_correlates():
"""
Create a CSV file with a bunch of measures that may be correlates of grasp success.
"""
from heuristic import finger_path_template, calculate_grip_metrics
data_file = '../../grasp-conv/data/output_data.csv'
objects, gripper_pos, gripper_orient, labels, power_pinch = load_all_params(
data_file, return_power_pinch=True)
seq_nums = np.arange(len(objects)) % 1000
labels = np.array(labels)
power_pinch = np.array(power_pinch)
camera_offset = .45
far_clip = .8
fov = 45. * np.pi / 180.
im_width = 80
finger_path = finger_path_template(fov, im_width, camera_offset)
background_threshold = far_clip - .005
result = []
for i in range(len(labels)):
data = [labels[i], power_pinch[i]]
image_filename = join(
'../../grasp-conv/data/obj_depths',
objects[i][:-4] + '-' + str(seq_nums[i]) + '.png')
distance_image = scipy.misc.imread(image_filename)
rescaled_distance = distance_image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
intersections, qualities = calculate_grip_metrics(
distance, finger_path)
regions = regions_at_intersections(distance, intersections)
for region in regions:
slope = surface_slope(region, background_threshold)
data.extend(slope[:2])
image_filename = join(
'../../grasp-conv/data/obj_overlap',
objects[i][:-4] + '-' + str(seq_nums[i]) + '-overlap.png')
overlap_image = scipy.misc.imread(image_filename)
regions = regions_at_intersections(overlap_image,
intersections,
length=3)
for region in regions:
data.append(np.sum(region > 0) / float(region.size))
data.append(angle_from_vertical(gripper_orient[i]))
data.append(object_area(distance, background_threshold))
data.extend(object_centre(distance, background_threshold))
# intersections and quality metrics
intersections = np.where(intersections == np.array(None), im_width,
intersections).astype(float)
data.extend(intersections)
data.extend(qualities)
# symmetry
rel_int = intersections / (im_width / 2.)
data.append((rel_int[0] - rel_int[2])**2)
data.append((rel_int[1] - rel_int[2])**2)
# intersections with support box
image_filename = join(
'../../grasp-conv/data/support_depths',
objects[i][:-4] + '-' + str(seq_nums[i]) + '.png')
distance_image = scipy.misc.imread(image_filename)
rescaled_distance = distance_image / 255.0
distance = rescaled_distance * (far_clip -
camera_offset) + camera_offset
support_intersections, support_qualities = calculate_grip_metrics(
distance, finger_path)
support_intersections = np.where(
support_intersections == np.array(None), im_width,
support_intersections).astype(float)
data.extend(support_intersections)
result.append(data)
# fig = plt.figure(figsize=(5,10))
# fig.add_subplot(2,1,1)
# plt.hist(power_pinch[labels<.5], range=(0, .20), bins=100)
# fig.add_subplot(2,1,2)
# plt.hist(power_pinch[labels>.5], range=(0, .20), bins=100)
# plt.show()
return np.array(result)