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
