def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, contact_points=None, contact_normals=None): self.center = center self.angle = angle self.depth = depth self.width = width # If `camera_intr` is none use default primesense camera intrinsics. if not camera_intr: self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr self.contact_points = contact_points self.contact_normals = contact_normals frame = "image" if camera_intr is not None: frame = camera_intr.frame if isinstance(center, np.ndarray): self.center = Point(center, frame=frame)
def __init__(self, path_to_images, frame=None): """Create a new virtualized Primesense sensor. This requires a directory containing a specific set of files. First, the directory must contain a set of images, where each image has three files: - color_{#}.png - depth_{#}.npy - ir_{#}.npy In these, the {#} is replaced with the integer index for the image. These indices should start at zero and increase consecutively. Second, the directory must contain CameraIntrisnics files for the color and ir cameras: - {frame}_color.intr - {frame}_ir.intr In these, the {frame} is replaced with the reference frame name that is passed as a parameter to this function. Parameters ---------- path_to_images : :obj:`str` The path to a directory containing images that the virtualized sensor will return. frame : :obj:`str` The name of the frame of reference in which the sensor resides. If None, this will be discovered from the files in the directory. """ self._running = False self._path_to_images = path_to_images self._im_index = 0 self._num_images = 0 self._frame = frame filenames = os.listdir(self._path_to_images) # get number of images for filename in filenames: if filename.find('depth') != -1 and filename.endswith('.npy'): self._num_images += 1 # set the frame dynamically if self._frame is None: for filename in filenames: file_root, file_ext = os.path.splitext(filename) color_ind = file_root.rfind('color') if file_ext == INTR_EXTENSION and color_ind != -1: self._frame = file_root[:color_ind-1] self._color_frame = file_root self._ir_frame = file_root break # load color intrinsics color_intr_filename = os.path.join(self._path_to_images, '%s_color.intr' %(self._frame)) self._color_intr = CameraIntrinsics.load(color_intr_filename) ir_intr_filename = os.path.join(self._path_to_images, '%s_ir.intr' %(self._frame)) self._ir_intr = CameraIntrinsics.load(ir_intr_filename)
def __init__(self, center, axis=None, depth=1.0, camera_intr=None): if axis is None: axis = np.array([0, 0, 1]) self.center = center self.axis = axis frame = "image" if camera_intr is not None: frame = camera_intr.frame if isinstance(center, np.ndarray): self.center = Point(center, frame=frame) if isinstance(axis, list): self.axis = np.array(axis) if np.abs(np.linalg.norm(self.axis) - 1.0) > 1e-3: raise ValueError("Illegal axis. Must be norm 1.") self.depth = depth # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr
def generate_images(self, salient_edge_set, n_samples=1): """Generate depth image, normal image, and binary edge mask tuples. Parameters ---------- salient_edge_set : SalientEdgeSet A salient edge set to generate images of. n_samples : int The number of samples to generate. Returns ------- depth_ims : (n,) list of perception.DepthImage Randomly-rendered depth images of object. normal_ims : (n,) list of perception.PointCloudImage Normals for the given image edge_masks : (n,) list of perception.BinaryImage Masks for pixels on the salient edges of the object. """ # Compute stable poses of mesh mesh = salient_edge_set.mesh stp_pose_tfs, probs = mesh.compute_stable_poses() probs = probs / sum(probs) # Generate n renders depth_ims, normal_ims, edge_masks = [], [], [] scene = Scene() so = SceneObject(mesh, RigidTransform(from_frame='obj', to_frame='world')) scene.add_object('object', so) for i in range(n_samples): # Sample random stable pose. tf_id = np.random.choice(np.arange(len(probs)), p=probs) tf = stp_pose_tfs[tf_id] T_obj_world = RigidTransform(tf[:3,:3], tf[:3,3], from_frame='obj', to_frame='world') so.T_obj_world = T_obj_world # Create the random uniform workspace sampler ws_cfg = self._config['worksurface_rv_config'] uvs = UniformPlanarWorksurfaceImageRandomVariable('object', scene, [RenderMode.DEPTH], frame='camera', config=ws_cfg) # Sample and extract the depth image, camera intrinsics, and T_obj_camera sample = uvs.sample() depth_im = sample.renders[RenderMode.DEPTH] cs = sample.camera ci = CameraIntrinsics(frame='camera', fx=cs.focal, fy=cs.focal, cx=cs.cx, cy=cs.cy, skew=0.0, height=ws_cfg['im_height'], width=ws_cfg['im_width']) T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world) edge_mask = self._compute_edge_mask(salient_edge_set, depth_im, ci, T_obj_camera) point_cloud_im = ci.deproject_to_image(depth_im) normal_im = point_cloud_im.normal_cloud_im() depth_ims.append(depth_im) normal_ims.append(normal_im) edge_masks.append(edge_mask) return depth_ims, normal_ims, edge_masks
def __init__(self, center, angle, depth, width=0.0, camera_intr=None): self.center = center self.angle = angle self.depth = depth self.width = width # if camera_intr is none use default primesense camera intrinsics if not camera_intr: self.camera_intr = CameraIntrinsics('primesense_overhead', fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr
def render_depth_maps(mesh_filename, intrinsic, extrinsics, mesh_scale=1.0): scene = mr.Scene() # setup camera ci = CameraIntrinsics(frame='camera', fx=intrinsic.get_focal_length()[0], fy=intrinsic.get_focal_length()[1], cx=intrinsic.get_principal_point()[0], cy=intrinsic.get_principal_point()[1], skew=0, height=intrinsic.height, width=intrinsic.width) cp = RigidTransform(from_frame='camera', to_frame='world') camera = mr.VirtualCamera(ci, cp) scene.camera = camera obj = trimesh.load_mesh(mesh_filename) obj.apply_scale(mesh_scale) dmaps = [] for T in extrinsics: obj_pose = RigidTransform(rotation=T[:3, :3], translation=T[:3, 3], from_frame='obj', to_frame='camera') sobj = mr.SceneObject(obj, obj_pose) scene.add_object('obj', sobj) dmap = scene.render(render_color=False) dmap = np.uint16(dmap * 1000.0) dmaps.append(dmap) scene.remove_object('obj') return np.stack(dmaps)
def ir_intrinsics(self): """:obj:`CameraIntrinsics` : The camera intrinsics for the primesense IR camera. """ return CameraIntrinsics(self._ir_frame, PrimesenseSensor.FOCAL_X, PrimesenseSensor.FOCAL_Y, PrimesenseSensor.CENTER_X, PrimesenseSensor.CENTER_Y, height=PrimesenseSensor.DEPTH_IM_HEIGHT, width=PrimesenseSensor.DEPTH_IM_WIDTH)
def load(save_dir): if not os.path.exists(save_dir): raise ValueError('Directory %s does not exist!' % (save_dir)) color_image_filename = os.path.join(save_dir, 'color.png') depth_image_filename = os.path.join(save_dir, 'depth.npy') camera_intr_filename = os.path.join(save_dir, 'camera.intr') segmask_filename = os.path.join(save_dir, 'segmask.npy') obj_segmask_filename = os.path.join(save_dir, 'obj_segmask.npy') state_filename = os.path.join(save_dir, 'state.pkl') camera_intr = CameraIntrinsics.load(camera_intr_filename) color = ColorImage.open(color_image_filename, frame=camera_intr.frame) depth = DepthImage.open(depth_image_filename, frame=camera_intr.frame) segmask = None if os.path.exists(segmask_filename): segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame) obj_segmask = None if os.path.exists(obj_segmask_filename): obj_segmask = SegmentationImage.open(obj_segmask_filename, frame=camera_intr.frame) fully_observed = None if os.path.exists(state_filename): fully_observed = pkl.load(open(state_filename, 'rb')) return RgbdImageState(RgbdImage.from_color_and_depth(color, depth), camera_intr, segmask=segmask, obj_segmask=obj_segmask, fully_observed=fully_observed)
def resize(self, new_width, new_height): """Reset the camera intrinsics for a new width and height viewing window. Parameters ---------- new_width : int The new window width, in pixels. new_height : int The new window height, in pixels. """ x_scale = float(new_width) / self.intrinsics.width y_scale = float(new_height) / self.intrinsics.height center_x = float(self.intrinsics.width-1)/2 center_y = float(self.intrinsics.height-1)/2 orig_cx_diff = self.intrinsics.cx - center_x orig_cy_diff = self.intrinsics.cy - center_y scaled_center_x = float(new_width-1) / 2 scaled_center_y = float(new_height-1) / 2 cx = scaled_center_x + x_scale * orig_cx_diff cy = scaled_center_y + y_scale * orig_cy_diff fx = self.intrinsics.fx * x_scale fy = self.intrinsics.fy * x_scale scaled_intrinsics = CameraIntrinsics(frame=self.intrinsics.frame, fx=fx, fy=fy, skew=self.intrinsics.skew, cx=cx, cy=cy, height=new_height, width=new_width) self._intrinsics = scaled_intrinsics
def plan_grasp(self, depth, rgb, resetting=False, camera_intr=None, segmask=None): """ Computes possible grasps. Parameters ---------- depth: type `numpy` depth image rgb: type `numpy` rgb image camera_intr: type `perception.CameraIntrinsics` Intrinsic camera object. segmask: type `perception.BinaryImage` Binary segmask of detected object Returns ------- type `GQCNNGrasp` Computed optimal grasp. """ if "FC" in self.model: assert not (segmask is None), "Fully-Convolutional policy expects a segmask." if camera_intr is None: camera_intr_filename = os.path.join( os.path.dirname(os.path.realpath(__file__)), "gqcnn/data/calib/primesense/primesense.intr") camera_intr = CameraIntrinsics.load(camera_intr_filename) depth_im = DepthImage(depth, frame=camera_intr.frame) color_im = ColorImage(rgb, frame=camera_intr.frame) valid_px_mask = depth_im.invalid_pixel_mask().inverse() if segmask is None: segmask = valid_px_mask else: segmask = segmask.mask_binary(valid_px_mask) # Inpaint. depth_im = depth_im.inpaint( rescale_factor=self.config["inpaint_rescale_factor"]) # Aggregate color and depth images into a single # BerkeleyAutomation/perception `RgbdImage`. self.rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) # Create an `RgbdImageState` with the `RgbdImage` and `CameraIntrinsics`. state = RgbdImageState(self.rgbd_im, camera_intr, segmask=segmask) # Set input sizes for fully-convolutional policy. if "FC" in self.model: self.policy_config["metric"]["fully_conv_gqcnn_config"][ "im_height"] = depth_im.shape[0] self.policy_config["metric"]["fully_conv_gqcnn_config"][ "im_width"] = depth_im.shape[1] return self.execute_policy(state, resetting)
def camera_intrinsics(self, T_camera_world, f, cx, cy): """Generate shifted camera intrinsics to simulate cropping. """ # form intrinsics camera_intr = CameraIntrinsics(self.frame, fx=f, fy=f, cx=cx, cy=cy, skew=0.0, height=self.im_height, width=self.im_width) return camera_intr
def __init__(self, pose, camera_intr=None): self._pose = pose frame = 'image' if camera_intr is not None: frame = camera_intr.frame # if camera_intr is none use default primesense camera intrinsics if not camera_intr: self.camera_intr = CameraIntrinsics('primesense_overhead', fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr
def __init__(self, pose, camera_intr=None): self._pose = pose # TODO(vsatish): Confirm that this is really not needed. # frame = "image" # if camera_intr is not None: # frame = camera_intr.frame # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr
def read_images(self, req): """Reads images from a ROS service request. Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service. """ # Get the raw depth and color images as ROS `Image` objects. raw_color = req.color_image raw_depth = req.depth_image # Get the raw camera info as ROS `CameraInfo`. raw_camera_info = req.camera_info # Wrap the camera info in a BerkeleyAutomation/perception # `CameraIntrinsics` object. camera_intr = CameraIntrinsics( raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) # Create wrapped BerkeleyAutomation/perception RGB and depth images by # unpacking the ROS images using ROS `CvBridge` try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intr.frame) depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough"), frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # Check image sizes. if color_im.height != depth_im.height or \ color_im.width != depth_im.width: msg = ("Color image and depth image must be the same shape! Color" " is %d x %d but depth is %d x %d") % ( color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if (color_im.height < self.min_height or color_im.width < self.min_width): msg = ("Color image is too small! Must be at least %d x %d" " resolution but the requested image is only %d x %d") % ( self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr
def camera_intrinsics(self, T_camera_obj, f, cx, cy): """ Generate shifted camera intrinsics to simulate cropping """ # form intrinsics camera_intr = CameraIntrinsics(self.frame, fx=f, fy=f, cx=cx, cy=cy, skew=0.0, height=self.im_height, width=self.im_width) # compute new camera center by projecting object 0,0,0 into the camera center_obj_obj = Point(np.zeros(3), frame='obj') center_obj_camera = T_camera_obj * center_obj_obj u_center_obj = camera_intr.project(center_obj_camera) camera_shifted_intr = copy.deepcopy(camera_intr) camera_shifted_intr.cx = 2 * camera_intr.cx - float(u_center_obj.x) camera_shifted_intr.cy = 2 * camera_intr.cy - float(u_center_obj.y) return camera_shifted_intr
def color_intrinsics(self): """:obj:`CameraIntrinsics` : The camera intrinsics for the RealSense color camera. """ return CameraIntrinsics( self._frame, self._intrinsics[0, 0], self._intrinsics[1, 1], self._intrinsics[0, 2], self._intrinsics[1, 2], height=RealSenseSensor.COLOR_IM_HEIGHT, width=RealSenseSensor.COLOR_IM_WIDTH, )
def read_images(self, req): """ Reads images from a ROS service request Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service """ # get the raw depth and color images as ROS Image objects raw_color = req.color_image raw_depth = req.depth_image # get the raw camera info as ROS CameraInfo object raw_camera_info = req.camera_info # wrap the camera info in a perception CameraIntrinsics object camera_intr = CameraIntrinsics( raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) # create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ### try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intr.frame) depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough"), frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # check image sizes if color_im.height != depth_im.height or \ color_im.width != depth_im.width: msg = 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' % ( color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if color_im.height < self.min_height or color_im.width < self.min_width: msg = 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' % ( self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr
def sample(self, size=1): """Sample random variables from the model. Parameters ---------- size : int number of sample to take Returns ------- :obj:`list` of :obj:`CameraSample` sampled camera intrinsics and poses """ samples = [] for i in range(size): # sample camera params focal = self.focal_rv.rvs(size=1)[0] cx = self.cx_rv.rvs(size=1)[0] cy = self.cy_rv.rvs(size=1)[0] # sample viewsphere params radius = self.rad_rv.rvs(size=1)[0] elev = self.elev_rv.rvs(size=1)[0] az = self.az_rv.rvs(size=1)[0] roll = self.roll_rv.rvs(size=1)[0] # sample plane translation tx = self.tx_rv.rvs(size=1)[0] ty = self.ty_rv.rvs(size=1)[0] # convert to pose and intrinsics pose = self.camera_to_world_pose(radius, elev, az, roll, tx, ty) intrinsics = CameraIntrinsics(self.frame, fx=focal, fy=focal, cx=cx, cy=cy, skew=0.0, height=self.im_height, width=self.im_width) # convert to camera pose samples.append((pose, intrinsics)) # not a list if only 1 sample if size == 1: return samples[0] return samples
def largest_planar_surface(filename, cam_int_file): # Load the image as a numpy array and the camera intrinsics image = np.load(filename) ci = CameraIntrinsics.load(cam_int_file) # Create and deproject a depth image of the data using the camera intrinsics di = DepthImage(image, frame=ci.frame) di = di.inpaint() pc = ci.deproject(di) # Make a PCL type point cloud from the image p = pcl.PointCloud(pc.data.T.astype(np.float32)) # Make a segmenter and segment the point cloud. seg = p.make_segmenter() seg.set_model_type(pcl.SACMODEL_PARALLEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) seg.set_distance_threshold(0.005) indices, model = seg.segment() return indices, model, image, pc
def __init__(self): DATASET_DIR = pt.abspath('.') OUTPUT_DIR = pt.abspath('./data') self.sampler = ModelSampler(DATASET_DIR) self.scene = Scene() self.local_scene = Scene() self.grip_scene = Scene() self.dataset_dir = DATASET_DIR self.output_dir = OUTPUT_DIR self.image_dir = 'color-input-synth' self.depth_dir = 'depth-input-synth' self.seg_dir = 'label-synth' clear_dir(pt.join(self.output_dir, self.image_dir)) clear_dir(pt.join(self.output_dir, self.depth_dir)) clear_dir(pt.join(self.output_dir, self.seg_dir)) ci = CameraIntrinsics(frame='camera', fx=617.0, fy=617.0, cx=320.0, cy=240.0, skew=0.0, height=480, width=640) # Set up the camera pose (z axis faces away from scene, x to right, y up) cp1 = RigidTransform(rotation=trimesh.transformations.rotation_matrix( np.deg2rad(-30), [1, 0, 0])[:3, :3] @ trimesh.transformations.rotation_matrix( np.deg2rad(180), [0, 1, 0])[:3, :3], translation=np.array([0.0, 0.75, 1.0]), from_frame='camera', to_frame='world') cp2 = RigidTransform(rotation=trimesh.transformations.rotation_matrix( np.deg2rad(37), [1, 0, 0])[:3, :3], translation=np.array([0.0, 0.0, 1.0]), from_frame='camera', to_frame='world') camera1 = VirtualCamera(ci, cp1) camera2 = VirtualCamera(ci, cp2) # Add the camera to the scene self.scene.camera = camera1 self.local_scene.camera = camera1 self.grip_scene.camera = camera1
def find_clutter(pc, indices, cam_int_file): """plane_pc = pc.data.T[indices] using pc from largest_planar_surface""" plane_pc_data = pc.data.T[indices] ci = CameraIntrinsics.load(cam_int_file) plane_pc = PointCloud(plane_pc_data.T, pc.frame) di = ci.project_to_image(plane_pc) vis2d.figure() vis2d.imshow(di) vis2d.show() inv_pixel_mask = di.invalid_pixel_mask() vis2d.figure() vis2d.imshow(inv_pixel_mask) vis2d.show() #print(inv_pixel_mask.data[inv_pixel_mask.data.shape[0]//2][inv_pixel_mask.data.shape[1]//2]) clutter = [] for r in range(len(inv_pixel_mask.data)): for c in range(len(inv_pixel_mask.data[r])): if inv_pixel_mask.data[r][c] > 0: i = r * len(inv_pixel_mask.data[r]) + c clutter.append(i) return pc.data.T[clutter]
def get_depth_image_from_3d_model(scene, camera_intrinsics, image_height, image_width, camera_pose, z_near, z_far, point_light_strength, ambient_strength): # Add a point light source to the scene pointlight = PointLight(location=camera_pose[:3, 3], color=np.array([1.0, 1.0, 1.0]), strength=point_light_strength) scene.add_light('point_light', pointlight) # Add lighting to the scene # Create an ambient light ambient = AmbientLight(color=np.array([1.0, 1.0, 1.0]), strength=ambient_strength) # Add the lights to the scene scene.ambient_light = ambient # only one ambient light per scene # Add a camera to the scene # Set up camera intrinsics ci = CameraIntrinsics(frame='camera', fx=camera_intrinsics[0, 0], fy=camera_intrinsics[1, 1], cx=camera_intrinsics[0, 2], cy=camera_intrinsics[1, 2], skew=0.0, height=image_height, width=image_width) # Set up the camera pose (z axis faces away from scene, x to right, y up) cp = RigidTransform(rotation=camera_pose[:3, :3], translation=camera_pose[:3, 3], from_frame='camera', to_frame='world') # Create a VirtualCamera camera = VirtualCamera(ci, cp, z_near=z_near, z_far=z_far) # Add the camera to the scene scene.camera = camera # Render raw numpy arrays containing color and depth color_image_raw, depth_image_raw = scene.render(render_color=True, front_and_back=True) return color_image_raw, depth_image_raw
def _reset_view(self): """Reset the view to a good initial state. The view is initially along the positive x-axis a sufficient distance from the scene. """ # Compute scene bounds and scale bounds = self._compute_scene_bounds() centroid = np.mean(bounds, axis=0) extents = np.diff(bounds, axis=0).reshape(-1) scale = (extents**2).sum()**.5 width, height = self._size # Set up reasonable camera intrinsics fov = np.pi / 6.0 fl = height / (2.0 * np.tan(fov / 2)) ci = CameraIntrinsics(frame='camera', fx=fl, fy=fl, cx=width / 2.0, cy=height / 2.0, skew=0.0, height=height, width=width) # Move centroid if needed if self._target_object and self._target_object in self.scene.objects: obj = self.scene.objects[self._target_object] if isinstance(obj, InstancedSceneObject): centroid = np.mean(obj.raw_pose_data[3::4, :3], axis=0) else: centroid = np.mean(obj.mesh.bounds, axis=0) centroid = obj.T_obj_world.matrix.dot(np.hstack( (centroid, 1.0)))[:3] scale = (obj.mesh.extents**2).sum()**.5 # Set up the camera pose (z axis faces towards scene, x to right, y down) s2 = 1.0 / np.sqrt(2.0) cp = RigidTransform( rotation=np.array([[0.0, s2, -s2], [1.0, 0.0, 0.0], [0.0, -s2, -s2]]), translation=np.sqrt(2.0) * np.array([scale, 0.0, scale]) + centroid, from_frame='camera', to_frame='world') if self._starting_camera_pose is not None: cp = self._starting_camera_pose # Create a VirtualCamera self._camera = VirtualCamera(ci, cp, z_near=scale / 100.0, z_far=scale * 100.0) # Create a trackball self._trackball = Trackball( self._camera.T_camera_world, self._size, scale, target=centroid, )
# Read config. config = YamlConfig(config_filename) inpaint_rescale_factor = config["inpaint_rescale_factor"] policy_config = config["policy"] # Make relative paths absolute. if "gqcnn_model" in policy_config["metric"]: policy_config["metric"]["gqcnn_model"] = model_path if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): policy_config["metric"]["gqcnn_model"] = os.path.join( os.path.dirname(os.path.realpath(__file__)), "..", policy_config["metric"]["gqcnn_model"]) # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) # Read images. depth_data = np.load(depth_im_filename) depth_im = DepthImage(depth_data, frame=camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=camera_intr.frame) # Optionally read a segmask. segmask = None if segmask_filename is not None: segmask = BinaryImage.open(segmask_filename) valid_px_mask = depth_im.invalid_pixel_mask().inverse() if segmask is None: segmask = valid_px_mask
scene.ambient_light = ambient # only one ambient light per scene dl = DirectionalLight(direction=np.array([0.0, 0.0, -1.0]), color=np.array([1.0, 1.0, 1.0]), strength=2.0) scene.add_light('direc', dl) #==================================== # Add a camera to the scene #==================================== # Set up camera intrinsics ci = CameraIntrinsics(frame='camera', fx=525.0, fy=525.0, cx=320.0, cy=240.0, skew=0.0, height=480, width=640) # Set up the camera pose (z axis faces away from scene, x to right, y up) cp = RigidTransform(rotation=np.array([[0.0, 0.0, 1.0], [0.0, -1.0, 0.0], [1.0, 0.0, 0.0]]), translation=np.array([-0.3, 0.0, 0.0]), from_frame='camera', to_frame='world') # Create a VirtualCamera camera = VirtualCamera(ci, cp) # Add the camera to the scene
def main(): # ==================================== # parse arguments # ==================================== parser = argparse.ArgumentParser( description='Generate segmentation images and updated json files.') parser.add_argument('-d', '--data-dir', default=os.getcwd(), help='directory containing images and json files') parser.add_argument( '-o', '--object-settings', help= 'object_settings file where the fixed_model_transform corresponds to the poses' 'in the frame annotation jsons.' 'default: <data_dir>/_object_settings.json') parser.add_argument( '-t', '--target-object-settings', help= 'if given, transform all poses into the fixed_model_transform from this file.' ) args = parser.parse_args() if not args.object_settings: args.object_settings = os.path.join(args.data_dir, '_object_settings.json') if not args.target_object_settings: args.target_object_settings = args.object_settings # ===================== # parse object_settings # ===================== with open(args.object_settings, 'r') as f: object_settings_json = json.load(f) with open(args.target_object_settings, 'r') as f: target_object_settings_json = json.load(f) if not len(object_settings_json['exported_objects']) == len( target_object_settings_json['exported_objects']): print "FATAL: object_settings and target_object_settings do not match!" sys.exit(-1) models = {} for model_json, target_model_json in zip( object_settings_json['exported_objects'], target_object_settings_json['exported_objects']): class_name = model_json['class'] if not class_name == target_model_json['class']: print "FATAL: object_settings and target_object_settings do not match!" sys.exit(-1) segmentation_class_id = model_json['segmentation_class_id'] cuboid_dimensions = np.array(model_json['cuboid_dimensions']) # calculate model_transform fixed_model_transform_mat = np.transpose( np.array(model_json['fixed_model_transform'])) fixed_model_transform = RigidTransform( rotation=fixed_model_transform_mat[:3, :3], translation=fixed_model_transform_mat[:3, 3], from_frame='ycb_model', to_frame='source_model') target_fixed_model_transform_mat = np.transpose( np.array(target_model_json['fixed_model_transform'])) target_fixed_model_transform = RigidTransform( rotation=target_fixed_model_transform_mat[:3, :3], translation=target_fixed_model_transform_mat[:3, 3], from_frame='ycb_model', to_frame='target_model_original') model_transform = fixed_model_transform.dot( target_fixed_model_transform.inverse()) models[class_name] = ModelConfig(class_name, segmentation_class_id, model_transform, cuboid_dimensions) # ================================================== # parse camera_settings and set up camera intrinsics # ================================================== with open(os.path.join(args.data_dir, '_camera_settings.json'), 'r') as f: camera_settings_json = json.load(f)['camera_settings'][0] camera_intrinsics = CameraIntrinsics( frame='camera', fx=camera_settings_json['intrinsic_settings']['fx'], fy=camera_settings_json['intrinsic_settings']['fy'], cx=camera_settings_json['intrinsic_settings']['cx'], cy=camera_settings_json['intrinsic_settings']['cy'], skew=camera_settings_json['intrinsic_settings']['s'], height=camera_settings_json['captured_image_size']['height'], width=camera_settings_json['captured_image_size']['width']) # ==================================== # process each frame # ==================================== pattern = re.compile(r'\d{3,}.json') json_files = sorted([ os.path.join(args.data_dir, f) for f in os.listdir(args.data_dir) if pattern.match(f) ]) for json_file in json_files: filename_prefix = json_file[:-len('json')] print '\n---------------------- {}*'.format(filename_prefix) with open(json_file, 'r') as f: frame_json = json.load(f) updated_frame_json = process_frame(frame_json, camera_intrinsics, models) with open(filename_prefix + 'flipped.json', 'w') as f: json.dump(updated_frame_json, f, indent=2, sort_keys=True)
def generate_examples(self, salient_edge_set_filename, n_samples=1): """Generate RegistrationExamples for evaluating the algorithm. Parameters ---------- salient_edge_set_filename : str A file containing the salient edge set to generate images of. n_samples : int The number of samples to generate. Returns ------- list of RegistrationExample A list of RegistrationExamples. """ # Compute stable poses of mesh salient_edge_set = SalientEdgeSet.load(salient_edge_set_filename) mesh = salient_edge_set.mesh stp_pose_tfs, probs = mesh.compute_stable_poses() probs = probs / sum(probs) # Generate n renders examples = [] scene = Scene() so = SceneObject(mesh, RigidTransform(from_frame='obj', to_frame='world')) scene.add_object('object', so) for i in range(n_samples): # Sample random stable pose. tf_id = np.random.choice(np.arange(len(probs)), p=probs) tf = stp_pose_tfs[tf_id] T_obj_world = RigidTransform(tf[:3, :3], tf[:3, 3], from_frame='obj', to_frame='world') so.T_obj_world = T_obj_world # Create the random uniform workspace sampler ws_cfg = self._config['worksurface_rv_config'] uvs = UniformPlanarWorksurfaceImageRandomVariable( 'object', scene, [RenderMode.DEPTH], frame='camera', config=ws_cfg) # Sample and extract the depth image, camera intrinsics, and T_obj_camera sample = uvs.sample() depth_im = sample.renders[RenderMode.DEPTH] cs = sample.camera ci = CameraIntrinsics(frame='camera', fx=cs.focal, fy=cs.focal, cx=cs.cx, cy=cs.cy, skew=0.0, height=ws_cfg['im_height'], width=ws_cfg['im_width']) T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world) examples.append( RegistrationExample(salient_edge_set_filename, depth_im, ci, T_obj_camera)) return examples
#import libry as ry #%% rosco = RosComm() rospy.init_node('z') #%% rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/depth/image_rect_raw/') #rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/depth/color/points') #rosco.subscribe_synced_rgbd('/camera/color/image_raw/', '/camera/aligned_depth_to_color/image_raw/') #%% intr = rosco.get_camera_intrinsics('/camera/color/camera_info') #%% intr #rosco.threaded_synced_rgbd_cb('/camera/color/image_raw/', '/camera/depth/image_rect_raw/') #%% camera_int = CameraIntrinsics(frame='pcl', fx=intr['fx'], fy=intr['fy'], cx=intr['cx'], cy=intr['cy'], height=intr['height'], width=intr['width']) #%% cfg = YamlConfig('cfg/gqcnn_pj_dbg.yaml') #%% grasp_policy = CrossEntropyRobustGraspingPolicy(cfg['policy']) # grasp_policy = RobustGraspingPolicy(cfg['policy']) #%% img = rosco.rgb d = rosco.depth #%% plt.imshow(img) #%% plt.imshow(d) #print(img) #print(d) #%%
def read_images(self, req): """Reads images from a ROS service request. Parameters --------- req: :obj:`ROS ServiceRequest` ROS ServiceRequest for grasp planner service. """ # Get the raw depth and color images as ROS `Image` objects. raw_color = req.color_image raw_depth = req.depth_image # Get the raw camera info as ROS `CameraInfo`. raw_camera_info = req.camera_info # Wrap the camera info in a BerkeleyAutomation/perception # `CameraIntrinsics` object. camera_intr = CameraIntrinsics( frame=raw_camera_info.header.frame_id, fx=raw_camera_info.K[0], fy=raw_camera_info.K[4], cx=raw_camera_info.K[2], cy=raw_camera_info.K[5], width=raw_camera_info.width, height=raw_camera_info.height, ) # Create wrapped BerkeleyAutomation/perception RGB and depth images by # unpacking the ROS images using ROS `CvBridge` try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( raw_color, "rgb8"), frame=camera_intr.frame) cv2_depth = self.cv_bridge.imgmsg_to_cv2( raw_depth, desired_encoding="passthrough") cv2_depth = np.array(cv2_depth, dtype=np.float32) cv2_depth *= 0.001 nan_idxs = np.isnan(cv2_depth) cv2_depth[nan_idxs] = 1000.0 depth_im = DepthImage(cv2_depth, frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: print("except CvBridgeError") rospy.logerr(cv_bridge_exception) # Check image sizes. if color_im.height != depth_im.height or color_im.width != depth_im.width: msg = ("Color image and depth image must be the same shape! Color" " is %d x %d but depth is %d x %d") % ( color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if (color_im.height < self.min_height or color_im.width < self.min_width): msg = ("Color image is too small! Must be at least %d x %d" " resolution but the requested image is only %d x %d") % ( self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) # --- create CameraData struct --- # camera_data = CameraData() camera_data.rgb_img = color_im camera_data.depth_img = depth_im camera_data.intrinsic_params = camera_intr return camera_data
class MultiSuctionPoint2D(object): """Multi-Cup Suction grasp in image space. Equivalent to projecting a 6D pose to image space. Attributes ---------- pose : :obj:`autolab_core.RigidTransform` Pose in 3D camera space. camera_intr : :obj:`perception.CameraIntrinsics` Frame of reference for camera that the suction point corresponds to. """ def __init__(self, pose, camera_intr=None): self._pose = pose # TODO(vsatish): Confirm that this is really not needed. # frame = "image" # if camera_intr is not None: # frame = camera_intr.frame # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr def pose(self): return self._pose @property def frame(self): """The name of the frame of reference for the grasp.""" if self.camera_intr is None: raise ValueError("Must specify camera intrinsics") return self.camera_intr.frame @property def center(self): center_camera = Point(self._pose.translation, frame=self.camera_intr.frame) center_px = self.camera_intr.project(center_camera) return center_px @property def axis(self): return self._pose.x_axis @property def approach_axis(self): return self.axis @property def approach_angle(self): """The angle between the grasp approach axis and camera optical axis. """ dot = max(min(self.axis.dot(np.array([0, 0, 1])), 1.0), -1.0) return np.arccos(dot) @property def angle(self): g_axis = self._pose.y_axis g_axis_im = np.array([g_axis[0], g_axis[1], 0]) if np.linalg.norm(g_axis_im) == 0: return 0 theta = np.arctan2(g_axis[1], g_axis[0]) return theta @property def depth(self): return self._pose.translation[2] @property def orientation(self): x_axis = self.axis y_axis = np.array([x_axis[1], -x_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 delta_R = R.T.dot(self._pose.rotation) orientation = np.arccos(delta_R[1, 1]) if delta_R[1, 2] > 0: orientation = 2 * np.pi - orientation return orientation @property def feature_vec(self): """Returns the feature vector for the suction point. Note ---- `v = [center, axis, depth]` """ return np.r_[self.center.data, np.cos(self.orientation), np.sin(self.orientation)] @staticmethod def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): """Creates a `SuctionPoint2D` instance from a feature vector and additional parameters. Parameters ---------- v : :obj:`numpy.ndarray` Feature vector, see `Grasp2D.feature_vec`. camera_intr : :obj:`perception.CameraIntrinsics` Frame of reference for camera that the grasp corresponds to. depth : float Hard-set the depth for the suction grasp. axis : :obj:`numpy.ndarray` Normalized 3-vector specifying the approach direction. """ # Read feature vec. center_px = v[:2] grasp_angle = 0 if v.shape[0] > 2 and angle is None: # grasp_angle = v[2] grasp_vec = v[2:] grasp_vec = grasp_vec / np.linalg.norm(grasp_vec) grasp_angle = np.arctan2(grasp_vec[1], grasp_vec[0]) elif angle is not None: grasp_angle = angle grasp_axis = np.array([1, 0, 0]) if axis is not None: grasp_axis = axis grasp_depth = 0.5 if depth is not None: grasp_depth = depth x_axis = grasp_axis y_axis = np.array([grasp_axis[1], -grasp_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 = R.dot(RigidTransform.x_axis_rotation(grasp_angle)) t = camera_intr.deproject_pixel( grasp_depth, Point(center_px, frame=camera_intr.frame)).data T = RigidTransform(rotation=R, translation=t, from_frame="grasp", to_frame=camera_intr.frame) # Compute center and angle. return MultiSuctionPoint2D(T, camera_intr=camera_intr) @staticmethod def image_dist(g1, g2, alpha=1.0): """Computes the distance between grasps in image space. Uses Euclidean distance with alpha weighting of angles. Parameters ---------- g1 : :obj:`SuctionPoint2D` First suction point. g2 : :obj:`SuctionPoint2D` Second suction point. alpha : float Weight of angle distance (rad to meters). Returns ------- float Distance between grasps. """ # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) # Axis distances. dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0) axis_dist = np.arccos(dot) return point_dist + alpha * axis_dist