def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
depth = point_cloud_im[center.y, center.x][2]
# Update number of tries.
k += 1
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Perturb depth.
delta_depth = self._depth_rv.rvs(size=1)[0]
depth = depth + delta_depth
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Create candidate grasp.
candidate = SuctionPoint2D(center,
axis,
depth,
camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
List of 2D suction point candidates.
"""
# Compute edge pixels.
filter_start = time()
if self._depth_gaussian_sigma > 0:
depth_im_mask = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_gaussian_sigma)
else:
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
self._logger.debug("Filtering took %.3f sec" % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# Project to get the point cloud.
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
self._logger.debug("Normal cloud took %.3f sec" %
(time() - cloud_start))
# Randomly sample points and add to image.
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# Sample a point uniformly at random.
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
# depth = point_cloud_im[center.y, center.x][2]
orientation = 2 * np.pi * np.random.rand()
# Update number of tries.
k += 1
# Skip bad axes.
if np.linalg.norm(axis) == 0:
continue
# Rotation matrix.
x_axis = axis
y_axis = np.array([axis[1], -axis[0], 0])
if np.linalg.norm(y_axis) == 0:
y_axis = np.array([1, 0, 0])
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.cross(x_axis, y_axis)
R = np.array([x_axis, y_axis, z_axis]).T
# R_orig = np.copy(R)
R = R.dot(RigidTransform.x_axis_rotation(orientation))
t = point_cloud_im[center.y, center.x]
pose = RigidTransform(rotation=R,
translation=t,
from_frame="grasp",
to_frame=camera_intr.frame)
# Check center px dist from boundary.
if (center_px[0] < self._min_dist_from_boundary
or center_px[1] < self._min_dist_from_boundary
or center_px[1] >
depth_im.height - self._min_dist_from_boundary
or center_px[0] >
depth_im.width - self._min_dist_from_boundary):
continue
# Keep if the angle between the camera optical axis and the suction
# direction is less than a threshold.
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# Check distance to ensure sample diversity.
candidate = MultiSuctionPoint2D(pose, camera_intr=camera_intr)
# Check constraint satisfaction.
if constraint_fn is None or constraint_fn(candidate):
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return suction_points
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D grasp candidates from a depth image by finding
depth edges, then uniformly sampling point pairs and keeping only
antipodal grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`autolab.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`Grasp2D`
List of 2D grasp candidates.
"""
# Compute edge pixels.
edge_start = time()
depth_im = depth_im.apply(
snf.
gaussian_filter, #Create a new image by applying a function to this image's data.
sigma=self._depth_grad_gaussian_sigma) #这里会造成失真，最好用copy
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(
scale_factor) #Resize the image.#这里会造成失真，最好用copy
depth_im_threshed = depth_im_downsampled.threshold_gradients( #Creates a new DepthImage by zeroing out all depths where the magnitude of the gradient at that point is greater than grad_thresh.
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels(
) #Return an array of the zero pixels.
edge_pixels = edge_pixels.astype(
np.int16) #找到所有的大于_depth_grad_thresh的像素点。
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# Re-threshold edges if there are too few.
if edge_pixels.shape[0] < self._min_num_edge_pixels:
self._logger.info("Too few edge pixels!")
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels(
) #Return an array of the zero pixels的坐标.
edge_pixels = edge_pixels.astype(
np.int16) #找到所有的大于_depth_grad_thresh的像素点。
depth_im_mask = depth_im.copy() # depth_im.resize(scale_factor)
if segmask is not None: #如果有segmask，就去除segmask外的edge_pixels，以及深度信息。
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0] #边缘像素的数量
self._logger.debug("Depth edge detection took %.3f sec" %
(time() - edge_start))
self._logger.debug("Found %d edge pixels" % (num_pixels))
# Compute point cloud.
point_cloud_im = camera_intr.deproject_to_image(
depth_im_mask) # Deprojects a DepthImage into a PointCloudImage.
# Compute_max_depth.
depth_data = depth_im_mask.data[
depth_im_mask.data > 0] #depth_im_mask是指经过mask后的深度图（如果有mask）
if depth_data.shape[0] == 0: #失败
return []
min_depth = np.min(
depth_data
) + self._min_depth_offset # Offset from the minimum depth at the grasp center pixel to use in depth sampling
max_depth = np.max(depth_data) + self._max_depth_offset
# Compute surface normals.
normal_start = time()
edge_normals = self._surface_normals(
depth_im, edge_pixels
) # Return an array of the surface normals at the edge pixels.
self._logger.debug("Normal computation took %.3f sec" %
(time() - normal_start))
if visualize:
edge_pixels = edge_pixels[::2, :]
edge_normals = edge_normals[::2, :]
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b")
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [3 * pix[1] for pix in edge_normals]
V = [-3 * pix[0] for pix in edge_normals]
plt.quiver(X,
Y,
U,
V,
units="x",
scale=0.25,
width=0.5,
zorder=2,
color="r")
vis.title("Edge pixels and normals")
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title("Edge map")
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title("Segmask")
vis.show()
# Exit if no edge pixels.
if num_pixels == 0:
return []
# Form set of valid candidate point pairs. #先找到一部分符合要求的抓取点。
pruning_start = time()
max_grasp_width_px = Grasp2D(
Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px #计算Returns the width in pixels.
normal_ip = edge_normals.dot(edge_normals.T) #ab cos（θ）是一个对称矩阵
dists = ssd.squareform(ssd.pdist(edge_pixels)) # 是一个对称矩阵
#ssd.pdist(edge_pixels)计算每个点距离其他点之间的距离。ssd.squareform 用来把一个向量格式的距离向量转换成一个方阵格式的距离矩阵，反之亦然。
#ssd.pdist先形成距离函数，再由squareform形成一个距离方阵
#https://blog.csdn.net/qq_20135597/article/details/94212816?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-3.control
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0)
) #两个点之间符合force closure，距离小于max_grasp_width，同时两点距离大于零。np.where对于二维方阵生成的是两个array，是两个配对点的索引
valid_indices = np.c_[valid_indices[0], valid_indices[
1]] #Translates slice objects to concatenation along the second axis.即将valid_indices[1]并到valid_indices[0]后面。
self._logger.debug("Normal pruning %.3f sec" %
(time() - pruning_start))
# Raise exception if no antipodal pairs.
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# Prune out grasps.
contact_points1 = edge_pixels[
valid_indices[:,
0], :] # valid_indices：[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cnx所对应的那一行 也就是第一个点的坐标
contact_points2 = edge_pixels[
valid_indices[:,
1], :] # valid_indices：[ [c1x,c1y] [c2x,c2y].. ] valid_indices所有的cny所对应的那一行 也就是第二个点的坐标
contact_normals1 = edge_normals[valid_indices[:, 0], :] #
contact_normals2 = edge_normals[valid_indices[:, 1], :] #
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# Raise exception if no antipodal pairs.
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples,
num_pairs) #防止采样个数过多造成计算负担，设置一个最大的采样量。
grasp_indices = np.random.choice(
antipodal_indices, #随机选择一部分抓取
size=sample_size,
replace=False)
self._logger.debug("Grasp comp took %.3f sec" %
(time() - pruning_start))
# Compute grasps.
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :] #第一个点的坐标
p2 = contact_points2[grasp_ind, :] #第二个点的坐标
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
# width = np.linalg.norm(p1 - p2)
k += 1
depth_p1 = depth_im[p1[0], p1[1]]
depth_p2 = depth_im[p2[0], p2[1]]
# Compute center and axis.
grasp_center = (p1 + p2) // 2 #一个抓取的中心
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan2(grasp_axis[0],
grasp_axis[1]) #一个抓取的轴角
grasp_center_pt = Point(np.array(
[grasp_center[1], grasp_center[0]]),
frame=camera_intr.frame)
# Compute grasp points in 3D.只是为了确定两点距离小于夹爪宽度
x1 = point_cloud_im[p1[0], p1[1]]
x2 = point_cloud_im[p2[0], p2[1]]
if np.linalg.norm(x2 - x1) > self._gripper_width:
continue
# Perturb.
if self._grasp_center_sigma > 0.0:
grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(
cov=self._grasp_center_sigma * np.diag(np.ones(2)))
if self._grasp_angle_sigma > 0.0:
grasp_theta = grasp_theta + ss.norm.rvs(
scale=self._grasp_angle_sigma)
# Check center px dist from boundary. 检查抓取中心是否在工作空间内
if (grasp_center[0] < self._min_dist_from_boundary
or grasp_center[1] < self._min_dist_from_boundary
or grasp_center[0] >
depth_im.height - self._min_dist_from_boundary
or grasp_center[1] >
depth_im.width - self._min_dist_from_boundary):
continue
depth_p1 = depth_im[p1[0], p1[1]]
depth_p2 = depth_im[p2[0], p2[1]]
depth_grasp = (depth_p1 + depth_p2) / 2
depth_grasp = depth_grasp[0]
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
depth_grasp,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
grasps.append(candidate_grasp)
'''
for i in range(self._depth_samples_per_grasp):
# Get depth in the neighborhood of the center pixel.
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] +
self._h, grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# Sample depth between the min and max.
min_depth = center_depth + self._min_depth_offset
max_depth = center_depth + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c="b", s=25)
vis.scatter(p2[1], p2[0], c="b", s=25)
vis.show()
grasps.append(candidate_grasp)
'''
# Return sampled grasps.
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return grasps
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False,
constraint_fn=None):
"""Sample a set of 2D grasp candidates from a depth image by finding
depth edges, then uniformly sampling point pairs and keeping only
antipodal grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:"perception.DepthImage"
Depth image to sample from.
camera_intr : :obj:`perception.CameraIntrinsics`
Intrinsics of the camera that captured the images.
num_samples : int
Number of grasps to sample.
segmask : :obj:`perception.BinaryImage`
Binary image segmenting out the object of interest.
visualize : bool
Whether or not to show intermediate samples (for debugging).
constraint_fn : :obj:`GraspConstraintFn`
Constraint function to apply to grasps.
Returns
-------
:obj:`list` of :obj:`Grasp2D`
List of 2D grasp candidates.
"""
# Compute edge pixels.
edge_start = time()
depth_im = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_grad_gaussian_sigma)
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(scale_factor)
depth_im_threshed = depth_im_downsampled.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# Re-threshold edges if there are too few.
if edge_pixels.shape[0] < self._min_num_edge_pixels:
self._logger.info("Too few edge pixels!")
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0]
self._logger.debug("Depth edge detection took %.3f sec" %
(time() - edge_start))
self._logger.debug("Found %d edge pixels" % (num_pixels))
# Compute point cloud.
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
# Compute_max_depth.
depth_data = depth_im_mask.data[depth_im_mask.data > 0]
if depth_data.shape[0] == 0:
return []
min_depth = np.min(depth_data) + self._min_depth_offset
max_depth = np.max(depth_data) + self._max_depth_offset
# Compute surface normals.
normal_start = time()
edge_normals = self._surface_normals(depth_im, edge_pixels)
self._logger.debug("Normal computation took %.3f sec" %
(time() - normal_start))
if visualize:
edge_pixels = edge_pixels[::2, :]
edge_normals = edge_normals[::2, :]
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b")
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [3 * pix[1] for pix in edge_normals]
V = [-3 * pix[0] for pix in edge_normals]
plt.quiver(X,
Y,
U,
V,
units="x",
scale=0.25,
width=0.5,
zorder=2,
color="r")
vis.title("Edge pixels and normals")
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title("Edge map")
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title("Segmask")
vis.show()
# Exit if no edge pixels.
if num_pixels == 0:
return []
# Form set of valid candidate point pairs.
pruning_start = time()
max_grasp_width_px = Grasp2D(Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px
normal_ip = edge_normals.dot(edge_normals.T)
dists = ssd.squareform(ssd.pdist(edge_pixels))
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0))
valid_indices = np.c_[valid_indices[0], valid_indices[1]]
self._logger.debug("Normal pruning %.3f sec" %
(time() - pruning_start))
# Raise exception if no antipodal pairs.
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# Prune out grasps.
contact_points1 = edge_pixels[valid_indices[:, 0], :]
contact_points2 = edge_pixels[valid_indices[:, 1], :]
contact_normals1 = edge_normals[valid_indices[:, 0], :]
contact_normals2 = edge_normals[valid_indices[:, 1], :]
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# Raise exception if no antipodal pairs.
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples, num_pairs)
grasp_indices = np.random.choice(antipodal_indices,
size=sample_size,
replace=False)
self._logger.debug("Grasp comp took %.3f sec" %
(time() - pruning_start))
# Compute grasps.
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :]
p2 = contact_points2[grasp_ind, :]
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
# width = np.linalg.norm(p1 - p2)
k += 1
# Compute center and axis.
grasp_center = (p1 + p2) // 2
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1])
grasp_center_pt = Point(np.array(
[grasp_center[1], grasp_center[0]]),
frame=camera_intr.frame)
# Compute grasp points in 3D.
x1 = point_cloud_im[p1[0], p1[1]]
x2 = point_cloud_im[p2[0], p2[1]]
if np.linalg.norm(x2 - x1) > self._gripper_width:
continue
# Perturb.
if self._grasp_center_sigma > 0.0:
grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(
cov=self._grasp_center_sigma * np.diag(np.ones(2)))
if self._grasp_angle_sigma > 0.0:
grasp_theta = grasp_theta + ss.norm.rvs(
scale=self._grasp_angle_sigma)
# Check center px dist from boundary.
if (grasp_center[0] < self._min_dist_from_boundary
or grasp_center[1] < self._min_dist_from_boundary
or grasp_center[0] >
depth_im.height - self._min_dist_from_boundary
or grasp_center[1] >
depth_im.width - self._min_dist_from_boundary):
continue
# Sample depths.
for i in range(self._depth_samples_per_grasp):
# Get depth in the neighborhood of the center pixel.
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] +
self._h, grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# Sample depth between the min and max.
min_depth = center_depth + self._min_depth_offset
max_depth = center_depth + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c="b", s=25)
vis.scatter(p2[1], p2[0], c="b", s=25)
vis.show()
grasps.append(candidate_grasp)
# Return sampled grasps.
self._logger.debug("Loop took %.3f sec" % (time() - sample_start))
return grasps
def quality(self, state, actions, params=None):
"""Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points.
Parameters
----------
state : :obj:`RgbdImageState`
An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed.
action: :obj:`SuctionPoint2D`
A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr.
params: dict
Stores params used in computing suction quality.
Returns
-------
:obj:`numpy.ndarray`
Array of the quality for each grasp
"""
qualities = []
# deproject points
point_cloud_image = state.camera_intr.deproject_to_image(
state.rgbd_im.depth)
# compute negative SSE from the best fit plane for each grasp
for i, action in enumerate(actions):
if not isinstance(action, SuctionPoint2D):
raise ValueError(
'This function can only be used to evaluate suction quality'
)
points = self._points_in_window(
point_cloud_image, action,
segmask=state.segmask) # x,y in matrix A and z is vector z.
A, b = self._points_to_matrices(points)
w = self._action_to_plane(
point_cloud_image,
action) # vector w w/ a bias term represents a best-fit plane.
sse = self._sum_of_squared_residuals(w, A, b)
if params is not None and params['vis']['plane']:
from visualization import Visualizer2D as vis2d
from visualization import Visualizer3D as vis3d
mid_i = A.shape[0] / 2
pred_z = A.dot(w)
p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]])
n = np.array([w[0], w[1], -1])
n = n / np.linalg.norm(n)
tx = np.array([n[1], -n[0], 0])
tx = tx / np.linalg.norm(tx)
ty = np.cross(n, tx)
R = np.array([tx, ty, n]).T
c = state.camera_intr.deproject_pixel(action.depth,
action.center)
d = Point(c.data - 0.01 * action.axis, frame=c.frame)
T_table_world = RigidTransform(rotation=R,
translation=p0,
from_frame='patch',
to_frame='world')
vis3d.figure()
vis3d.points(point_cloud_image.to_point_cloud(),
scale=0.0025,
subsample=10,
random=True,
color=(0, 0, 1))
vis3d.points(PointCloud(points.T),
scale=0.0025,
color=(1, 0, 0))
vis3d.points(c, scale=0.005, color=(1, 1, 0))
vis3d.points(d, scale=0.005, color=(1, 1, 0))
vis3d.table(T_table_world, dim=0.01)
vis3d.show()
vis2d.figure()
vis2d.imshow(state.rgbd_im.depth)
vis2d.scatter(action.center.x, action.center.y, s=50, c='b')
vis2d.show()
quality = np.exp(
-sse
) # evaluate how well best-fit plane describles all points in window.
qualities.append(quality)
return np.array(qualities)
def _sample_suction_points(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False):
"""
Sample a set of 2D suction point candidates from a depth image by
choosing points on an object surface uniformly at random
and then sampling around the surface normal
Parameters
----------
depth_im : :obj:'perception.DepthImage'
Depth image to sample from
camera_intr : :obj:`perception.CameraIntrinsics`
intrinsics of the camera that captured the images
num_samples : int
number of grasps to sample
segmask : :obj:`perception.BinaryImage`
binary image segmenting out the object of interest
visualize : bool
whether or not to show intermediate samples (for debugging)
Returns
-------
:obj:`list` of :obj:`SuctionPoint2D`
list of 2D suction point candidates
"""
# compute edge pixels
filter_start = time()
depth_im_mask = depth_im.copy()
if segmask is not None:
depth_im_mask = depth_im.mask_binary(segmask)
logging.debug('Filtering took %.3f sec' % (time() - filter_start))
if visualize:
vis.figure()
vis.subplot(1, 2, 1)
vis.imshow(depth_im)
vis.subplot(1, 2, 2)
vis.imshow(depth_im_mask)
vis.show()
# project to get the point cloud
cloud_start = time()
point_cloud_im = camera_intr.deproject_to_image(depth_im_mask)
normal_cloud_im = point_cloud_im.normal_cloud_im()
nonzero_px = depth_im_mask.nonzero_pixels()
num_nonzero_px = nonzero_px.shape[0]
if num_nonzero_px == 0:
return []
logging.debug('Normal cloud took %.3f sec' % (time() - cloud_start))
# randomly sample points and add to image
sample_start = time()
suction_points = []
k = 0
sample_size = min(self._max_num_samples, num_nonzero_px)
indices = np.random.choice(num_nonzero_px,
size=sample_size,
replace=False)
while k < sample_size and len(suction_points) < num_samples:
# sample a point uniformly at random
ind = indices[k]
center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]])
center = Point(center_px, frame=camera_intr.frame)
axis = -normal_cloud_im[center.y, center.x]
depth = point_cloud_im[center.y, center.x][2]
# update number of tries
k += 1
# check center px dist from boundary
if center_px[0] < self._min_dist_from_boundary or \
center_px[1] < self._min_dist_from_boundary or \
center_px[1] > depth_im.height - self._min_dist_from_boundary or \
center_px[0] > depth_im.width - self._min_dist_from_boundary:
continue
# perturb depth
delta_depth = self._depth_rv.rvs(size=1)[0]
depth = depth + delta_depth
# keep if the angle between the camera optical axis and the suction direction is less than a threshold
dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
psi = np.arccos(dot)
if psi < self._max_suction_dir_optical_axis_angle:
# check distance to ensure sample diversity
candidate = SuctionPoint2D(center,
axis,
depth,
camera_intr=camera_intr)
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.scatter(center.x, center.y)
vis.show()
suction_points.append(candidate)
logging.debug('Loop took %.3f sec' % (time() - sample_start))
return suction_points
def _sample_antipodal_grasps(self,
depth_im,
camera_intr,
num_samples,
segmask=None,
visualize=False):
"""
Sample a set of 2D grasp candidates from a depth image by finding depth
edges, then uniformly sampling point pairs and keeping only antipodal
grasps with width less than the maximum allowable.
Parameters
----------
depth_im : :obj:'perception.DepthImage'
Depth image to sample from
camera_intr : :obj:`perception.CameraIntrinsics`
intrinsics of the camera that captured the images
num_samples : int
number of grasps to sample
segmask : :obj:`perception.BinaryImage`
binary image segmenting out the object of interest
visualize : bool
whether or not to show intermediate samples (for debugging)
Returns
-------
:obj:`list` of :obj:`Grasp2D`
list of 2D grasp candidates
"""
# compute edge pixels
edge_start = time()
depth_im = depth_im.apply(snf.gaussian_filter,
sigma=self._depth_grad_gaussian_sigma)
scale_factor = self._rescale_factor
depth_im_downsampled = depth_im.resize(scale_factor)
depth_im_threshed = depth_im_downsampled.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array(
[p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)])
depth_im_mask = depth_im.mask_binary(segmask)
# re-threshold edges if there are too few
if edge_pixels.shape[0] < self._min_num_edge_pixels:
logging.info('Too few edge pixels!')
depth_im_threshed = depth_im.threshold_gradients(
self._depth_grad_thresh)
edge_pixels = depth_im_threshed.zero_pixels()
edge_pixels = edge_pixels.astype(np.int16)
depth_im_mask = depth_im.copy()
if segmask is not None:
edge_pixels = np.array([
p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)
])
depth_im_mask = depth_im.mask_binary(segmask)
num_pixels = edge_pixels.shape[0]
logging.debug('Depth edge detection took %.3f sec' %
(time() - edge_start))
logging.debug('Found %d edge pixels' % (num_pixels))
# exit if no edge pixels
if num_pixels == 0:
return []
# compute_max_depth
min_depth = np.min(depth_im_mask.data[
depth_im_mask.data > 0]) + self._min_depth_offset
max_depth = np.max(depth_im_mask.data[
depth_im_mask.data > 0]) + self._max_depth_offset
# compute surface normals
normal_start = time()
edge_normals = self._surface_normals(depth_im, edge_pixels)
logging.debug('Normal computation took %.3f sec' %
(time() - normal_start))
if visualize:
vis.figure()
vis.subplot(1, 3, 1)
vis.imshow(depth_im)
if num_pixels > 0:
vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=10, c='b')
X = [pix[1] for pix in edge_pixels]
Y = [pix[0] for pix in edge_pixels]
U = [10 * pix[1] for pix in edge_normals]
V = [-10 * pix[0] for pix in edge_normals]
plt.quiver(X, Y, U, V, units='x', scale=0.5, zorder=2, color='g')
vis.title('Edge pixels and normals')
vis.subplot(1, 3, 2)
vis.imshow(depth_im_threshed)
vis.title('Edge map')
vis.subplot(1, 3, 3)
vis.imshow(segmask)
vis.title('Segmask')
vis.show()
# form set of valid candidate point pairs
pruning_start = time()
max_grasp_width_px = Grasp2D(Point(np.zeros(2)),
0.0,
min_depth,
width=self._gripper_width,
camera_intr=camera_intr).width_px
normal_ip = edge_normals.dot(edge_normals.T)
dists = ssd.squareform(ssd.pdist(edge_pixels))
valid_indices = np.where(
(normal_ip < -np.cos(np.arctan(self._friction_coef)))
& (dists < max_grasp_width_px) & (dists > 0.0))
valid_indices = np.c_[valid_indices[0], valid_indices[1]]
logging.debug('Normal pruning %.3f sec' % (time() - pruning_start))
# raise exception if no antipodal pairs
num_pairs = valid_indices.shape[0]
if num_pairs == 0:
return []
# prune out grasps
contact_points1 = edge_pixels[valid_indices[:, 0], :]
contact_points2 = edge_pixels[valid_indices[:, 1], :]
contact_normals1 = edge_normals[valid_indices[:, 0], :]
contact_normals2 = edge_normals[valid_indices[:, 1], :]
v = contact_points1 - contact_points2
v_norm = np.linalg.norm(v, axis=1)
v = v / np.tile(v_norm[:, np.newaxis], [1, 2])
ip1 = np.sum(contact_normals1 * v, axis=1)
ip2 = np.sum(contact_normals2 * (-v), axis=1)
ip1[ip1 > 1.0] = 1.0
ip1[ip1 < -1.0] = -1.0
ip2[ip2 > 1.0] = 1.0
ip2[ip2 < -1.0] = -1.0
beta1 = np.arccos(ip1)
beta2 = np.arccos(ip2)
alpha = np.arctan(self._friction_coef)
antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0]
# raise exception if no antipodal pairs
num_pairs = antipodal_indices.shape[0]
if num_pairs == 0:
return []
sample_size = min(self._max_rejection_samples, num_pairs)
grasp_indices = np.random.choice(antipodal_indices,
size=sample_size,
replace=False)
logging.debug('Grasp comp took %.3f sec' % (time() - pruning_start))
# compute grasps
sample_start = time()
k = 0
grasps = []
while k < sample_size and len(grasps) < num_samples:
grasp_ind = grasp_indices[k]
p1 = contact_points1[grasp_ind, :]
p2 = contact_points2[grasp_ind, :]
n1 = contact_normals1[grasp_ind, :]
n2 = contact_normals2[grasp_ind, :]
width = np.linalg.norm(p1 - p2)
k += 1
# compute center and axis
grasp_center = (p1 + p2) / 2
grasp_axis = p2 - p1
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
grasp_theta = np.pi / 2
if grasp_axis[1] != 0:
grasp_theta = np.arctan(grasp_axis[0] / grasp_axis[1])
grasp_center_pt = Point(
np.array([grasp_center[1], grasp_center[0]]))
# check center px dist from boundary
if grasp_center[0] < self._min_dist_from_boundary or \
grasp_center[1] < self._min_dist_from_boundary or \
grasp_center[0] > depth_im.height - self._min_dist_from_boundary or \
grasp_center[1] > depth_im.width - self._min_dist_from_boundary:
continue
# sample depths
for i in range(self._depth_samples_per_grasp):
# get depth in the neighborhood of the center pixel
depth_win = depth_im.data[grasp_center[0] -
self._h:grasp_center[0] + self._h,
grasp_center[1] -
self._w:grasp_center[1] + self._w]
center_depth = np.min(depth_win)
if center_depth == 0 or np.isnan(center_depth):
continue
# sample depth between the min and max
min_depth = np.min(center_depth) + self._min_depth_offset
max_depth = np.max(center_depth) + self._max_depth_offset
sample_depth = min_depth + (max_depth -
min_depth) * np.random.rand()
candidate_grasp = Grasp2D(grasp_center_pt,
grasp_theta,
sample_depth,
width=self._gripper_width,
camera_intr=camera_intr,
contact_points=[p1, p2],
contact_normals=[n1, n2])
if visualize:
vis.figure()
vis.imshow(depth_im)
vis.grasp(candidate_grasp)
vis.scatter(p1[1], p1[0], c='b', s=25)
vis.scatter(p2[1], p2[0], c='b', s=25)
vis.show()
grasps.append(candidate_grasp)
# return sampled grasps
logging.debug('Loop took %.3f sec' % (time() - sample_start))
return grasps
