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