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 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 __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 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 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 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, 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 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 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 __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):
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 __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 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 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,
)
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 test_get_cuboid(self):
object_pose = RigidTransform(rotation=np.array(
[[9.94856600e-01, 9.59313538e-04, 1.01288816e-01],
[4.85117785e-02, 8.73304839e-01, -4.84752788e-01],
[-8.89210435e-02, 4.87173212e-01, 8.68766545e-01]]),
translation=np.array([
0.023264999389648438,
-0.050949001312255859,
1.3201980590820313
]),
from_frame='target_model',
to_frame='camera')
# Qtn: [0.96655677 0.25138875 0.0491978 0.01229945]
cuboid_dimensions = np.array(
[0.10240400314331055, 0.140177001953125, 0.10230899810791016])
camera_intrinsics = CameraIntrinsics(frame='camera',
fx=768.16058349609375,
fy=768.16058349609375,
cx=480,
cy=270,
skew=0,
height=540,
width=960)
expected_json = json.loads("""
{
"cuboid_centroid": [0.023264999389648438, -0.050949001312255859, 1.3201980590820313],
"cuboid": [
[0.079317998886108398, -0.13447099685668945, 1.32593994140625],
[-0.022558999061584473, -0.13944000244140625, 1.3350430297851563],
[-0.022427000999450684, -0.017024999856948853, 1.4033380126953125],
[0.079450998306274414, -0.012056000232696533, 1.3942340087890625],
[0.068956999778747559, -0.084874000549316406, 1.2370590209960938],
[-0.032920000553131104, -0.089841995239257813, 1.2461630249023438],
[-0.032788000106811523, 0.032572999000549316, 1.3144569396972656],
[0.069088997840881348, 0.037541000843048096, 1.3053529357910156]
],
"projected_cuboid_centroid": [493.53689575195313, 240.35519409179688],
"projected_cuboid": [
[525.9517822265625, 192.09669494628906],
[467.01998901367188, 189.76899719238281],
[467.72409057617188, 260.68121337890625],
[523.77362060546875, 263.3577880859375],
[522.81939697265625, 217.29719543457031],
[459.70730590820313, 214.6195068359375],
[460.8389892578125, 289.035400390625],
[520.6571044921875, 292.09201049804688]
]
}
""")
actual_json = get_cuboid(object_pose, cuboid_dimensions,
camera_intrinsics)
np.testing.assert_almost_equal(actual_json['cuboid_centroid'],
expected_json['cuboid_centroid'],
decimal=4)
np.testing.assert_almost_equal(actual_json['cuboid'],
expected_json['cuboid'],
decimal=4)
np.testing.assert_almost_equal(
actual_json['projected_cuboid_centroid'],
expected_json['projected_cuboid_centroid'],
decimal=2)
np.testing.assert_almost_equal(actual_json['projected_cuboid'],
expected_json['projected_cuboid'],
decimal=2)
def sample_grasp(self, env, mesh_obj, vis=True):
# Setup sensor.
#camera_intr = CameraIntrinsics.load(camera_intr_filename)
# Read images.
# get depth image from camera
inpaint_rescale_factor = 1.0
segmask_filename = None
_, center, extents, obj_xquat, bbox_xpos = self.get_bbox_properties(
env, mesh_obj)
camera_pos = copy.deepcopy(center)
camera_pos[2] += 0.5 * extents[2] + 0.2
env.set_camera(camera_pos,
np.array([0.7071068, 0, 0, -0.7071068]),
camera_name=f"ext_camera_0")
rgb, depth = env.render_from_camera(int(self.config.camera_img_height),
int(self.config.camera_img_width),
camera_name=f"ext_camera_0")
# enforce zoom out
from scipy.interpolate import interp2d
center_x = self.config.camera_img_height / 2 + 1
center_y = self.config.camera_img_width / 2 + 1
img_height = self.config.camera_img_height
img_width = self.config.camera_img_width
xdense = np.linspace(0, img_height - 1, img_height)
ydense = np.linspace(0, img_width - 1, img_width)
xintr = (xdense - center_x) * (1.0 / self.rescale_factor) + center_x
yintr = (ydense - center_y) * (1.0 / self.rescale_factor) + center_y
xintr[xintr < 0] = 0
xintr[xintr > (img_height - 1)] = img_height - 1
yintr[yintr < 0] = 0
yintr[yintr > (img_width - 1)] = img_width - 1
fr = interp2d(xdense, ydense, rgb[:, :, 0], kind="linear")
rgb_r_new = fr(xintr, yintr)
fg = interp2d(xdense, ydense, rgb[:, :, 1], kind="linear")
rgb_g_new = fg(xintr, yintr)
fb = interp2d(xdense, ydense, rgb[:, :, 2], kind="linear")
rgb_b_new = fb(xintr, yintr)
rgb_new = np.stack([rgb_r_new, rgb_g_new, rgb_b_new], axis=2)
fd = interp2d(xdense, ydense, depth, kind="linear")
depth_new = fd(xintr, yintr)
#from skimage.transform import resize
#rgb22, depth2 = env.render_from_camera(int(self.config.camera_img_height) , int(self.config.camera_img_width), camera_name=f"ext_camera_0")
#import ipdb; ipdb.set_trace()
# visualize the interpolation
#import imageio
#imageio.imwrite(f"tmp/rgb_{self.iter_id}.png", rgb)
#imageio.imwrite(f"tmp/rgb2_{self.iter_id}.png", rgb_new)
#imageio.imwrite(f"tmp/depth_{self.iter_id}.png", depth)
#imageio.imwrite(f"tmp/depth2_{self.iter_id}.png", depth_new)
#import ipdb; ipdb.set_trace()
rgb = rgb_new
depth = depth_new
depth = depth * self.rescale_factor
# rgb: 128 x 128 x 1
# depth: 128 x 128 x 1
scaled_camera_fov_y = self.config.camera_fov_y
aspect = 1
scaled_fovx = 2 * np.arctan(
np.tan(np.deg2rad(scaled_camera_fov_y) * 0.5) * aspect)
scaled_fovx = np.rad2deg(scaled_fovx)
scaled_fovy = scaled_camera_fov_y
cx = self.config.camera_img_width * 0.5
cy = self.config.camera_img_height * 0.5
scaled_fx = cx / np.tan(np.deg2rad(
scaled_fovx / 2.)) * (self.rescale_factor)
scaled_fy = cy / np.tan(np.deg2rad(
scaled_fovy / 2.)) * (self.rescale_factor)
camera_intr = CameraIntrinsics(frame='phoxi',
fx=scaled_fx,
fy=scaled_fy,
cx=self.config.camera_img_width * 0.5,
cy=self.config.camera_img_height * 0.5,
height=self.config.camera_img_height,
width=self.config.camera_img_width)
depth_im = DepthImage(depth, 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.
valid_px_mask = depth_im.invalid_pixel_mask().inverse()
segmask = valid_px_mask
# Inpaint.
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
# Create state.
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# Query policy.
policy_start = time.time()
action = self.policy(state)
print("Planning took %.3f sec" % (time.time() - policy_start))
# numpy array with 2 values
grasp_center = action.grasp.center._data[:, 0] #(width, depth)
grasp_depth = action.grasp.depth * (1 / self.rescale_factor)
grasp_angle = action.grasp.angle #np.pi*0.3
if self.config.data_collection_mode:
self.current_grasp = action.grasp
depth_im = state.rgbd_im.depth
scale = 1.0
depth_im_scaled = depth_im.resize(scale)
translation = scale * np.array([
depth_im.center[0] - grasp_center[1],
depth_im.center[1] - grasp_center[0]
])
im_tf = depth_im_scaled
im_tf = depth_im_scaled.transform(translation, grasp_angle)
im_tf = im_tf.crop(self.gqcnn_image_size, self.gqcnn_image_size)
# get the patch
self.current_patch = im_tf.raw_data
XYZ_origin, gripper_quat = self.compute_grasp_pts_from_grasp_sample(
grasp_center, grasp_depth, grasp_angle, env)
return XYZ_origin[:, 0], gripper_quat
# Vis final grasp.
if vis:
from visualization import Visualizer2D as vis
vis.figure(size=(10, 10))
vis.imshow(rgbd_im.depth,
vmin=self.policy_config["vis"]["vmin"],
vmax=self.policy_config["vis"]["vmax"])
vis.grasp(action.grasp,
scale=2.5,
show_center=False,
show_axis=True)
vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format(
action.grasp.depth, action.q_value))
vis.show(f"tmp/grasp2_{mesh_obj.name}_{self.iter_id}.png")
vis.figure(size=(10, 10))
vis.imshow(im_tf,
vmin=self.policy_config["vis"]["vmin"],
vmax=self.policy_config["vis"]["vmax"])
vis.show(f"tmp/cropd_{mesh_obj.name}_{self.iter_id}.png")
import ipdb
ipdb.set_trace()
return XYZ_origin[:, 0], gripper_quat
import ipdb
ipdb.set_trace()
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
def plan_grasp(self, req):
""" Grasp planner request handler .
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service
"""
rospy.loginfo('Planning Grasp')
# set min dimensions
pad = max(
math.ceil(
np.sqrt(2) *
(float(self.cfg['policy']['metric']['crop_width']) / 2)),
math.ceil(
np.sqrt(2) *
(float(self.cfg['policy']['metric']['crop_height']) / 2)))
min_width = 2 * pad + self.cfg['policy']['metric']['crop_width']
min_height = 2 * pad + self.cfg['policy']['metric']['crop_height']
# get the raw depth and color images as ROS Image objects
raw_color = req.color_image
raw_depth = req.depth_image
segmask = None
raw_segmask = req.segmask
# get the raw camera info as ROS CameraInfo object
raw_camera_info = req.camera_info
# get the bounding box as a custom ROS BoundingBox msg
bounding_box = req.bounding_box
# wrap the camera info in a perception CameraIntrinsics object
camera_intrinsics = 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_image = ColorImage(self.cv_bridge.imgmsg_to_cv2(
raw_color, "rgb8"),
frame=camera_intrinsics.frame)
depth_image = DepthImage(self.cv_bridge.imgmsg_to_cv2(
raw_depth, desired_encoding="passthrough"),
frame=camera_intrinsics.frame)
segmask = BinaryImage(self.cv_bridge.imgmsg_to_cv2(
raw_segmask, desired_encoding="passthrough"),
frame=camera_intrinsics.frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
# check image sizes
if color_image.height != depth_image.height or \
color_image.width != depth_image.width:
rospy.logerr(
'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d'
% (color_image.height, color_image.width, depth_image.height,
depth_image.width))
raise rospy.ServiceException(
'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d'
% (color_image.height, color_image.width, depth_image.height,
depth_image.width))
if color_image.height < min_height or color_image.width < min_width:
rospy.logerr(
'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d'
%
(min_height, min_width, color_image.height, color_image.width))
raise rospy.ServiceException(
'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d'
%
(min_height, min_width, color_image.height, color_image.width))
# inpaint images
color_image = color_image.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
depth_image = depth_image.inpaint(
rescale_factor=self.cfg['inpaint_rescale_factor'])
# visualize
if self.cfg['vis']['color_image']:
vis.imshow(color_image)
vis.show()
if self.cfg['vis']['depth_image']:
vis.imshow(depth_image)
vis.show()
if self.cfg['vis']['segmask'] and segmask is not None:
vis.imshow(segmask)
vis.show()
# aggregate color and depth images into a single perception rgbdimage
rgbd_image = RgbdImage.from_color_and_depth(color_image, depth_image)
# calc crop parameters
minX = bounding_box.minX - pad
minY = bounding_box.minY - pad
maxX = bounding_box.maxX + pad
maxY = bounding_box.maxY + pad
# contain box to image->don't let it exceed image height/width bounds
if minX < 0:
minX = 0
if minY < 0:
minY = 0
if maxX > rgbd_image.width:
maxX = rgbd_image.width
if maxY > rgbd_image.height:
maxY = rgbd_image.height
centroidX = (maxX + minX) / 2
centroidY = (maxY + minY) / 2
# compute width and height
width = maxX - minX
height = maxY - minY
# crop camera intrinsics and rgbd image
cropped_camera_intrinsics = camera_intrinsics.crop(
height, width, centroidY, centroidX)
cropped_rgbd_image = rgbd_image.crop(height, width, centroidY,
centroidX)
cropped_segmask = None
if segmask is not None:
cropped_segmask = segmask.crop(height, width, centroidY, centroidX)
# visualize
if self.cfg['vis']['cropped_rgbd_image']:
vis.imshow(cropped_rgbd_image)
vis.show()
# create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics
image_state = RgbdImageState(cropped_rgbd_image,
cropped_camera_intrinsics,
segmask=cropped_segmask)
# execute policy
try:
return self.execute_policy(image_state, self.grasping_policy,
self.grasp_pose_publisher,
cropped_camera_intrinsics.frame)
except NoValidGraspsException:
rospy.logerr(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
raise rospy.ServiceException(
'While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!'
)
if __name__ == "__main__":
# Get grasp planner using GQCNN
grasp_planner = GraspPlanner(model="GQCNN-2.0")
# Get large container empty detector
large_container_detector = container_detector(model='large_container_detector_model.pth')
# Get small container empty detector
small_container_detector = container_detector(model='small_container_detector_model.pth')
# Set Action Mode, See rlbench/action_modes.py for other action modes
action_mode = ActionMode(ArmActionMode.ABS_JOINT_POSITION)
# Create grasp controller with initialized environment and task
grasp_controller = GraspController(action_mode, static_positions=True)
# Reset task
descriptions, obs = grasp_controller.reset()
# The camera intrinsic in RLBench
camera_intr = CameraIntrinsics(fx=893.738, fy=893.738, cx=516, cy=386, frame='world', height=772, width=1032)
# The translation between camera and gripper
# TODO: Change the whole logic into detecting the object using GQCNN
object_initial_poses = {}
while True:
camera_to_gripper_translation = [0.022, 0, 0.095]
while True:
objs = grasp_controller.get_objects(add_noise=True)
# Go to home position
home_pose = np.copy(objs['waypoint0'][1])
home_pose[0] -= 0.022
path = grasp_controller.get_path(home_pose)
obs, reward, terminate = grasp_controller.execute_path(path, open_gripper=True)
# Scale the image and change the type to uint8 to fit the neural network
rgb = np.array(obs.wrist_rgb * 255, dtype='uint8')
#myBin = SegmentationImage(depth_im.to_binary(0.01).raw_data)
#segMask = myBin.segment_mask(10)
#segMask.save('my_mask.png')
#%%
#color_im = ColorImage.open('color_im.png')
#depth_im = DepthImage.open('depth_im.png')
#%%
print(color_im.shape)
print(depth_im.shape)
#%%
intr = cam.color_intrinsics
camera_int = CameraIntrinsics(frame=intr.frame,
fx=intr.fx,
fy=intr.fy,
cx=intr.cx,
cy=intr.cy,
height=intr.height,
width=intr.width)
#camera_int.save('mySensor.intr')
#%%
cfg = YamlConfig('cfg/examples/replication/dex-net_2.0.yaml')
grasp_policy = CrossEntropyRobustGraspingPolicy(cfg['policy'])
#%%
#depth_im.threshold(front_thresh=0.00, rear_thresh=75.0)
color_im = color_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
depth_im = depth_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
depth_im.save('my_im_heoo9.npy')
depth_im.save('my_im_heoo9.png')
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
def test_new_database_and_graspable(self):
# new database
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
database.close()
self.assertTrue(database is not None)
# new dataset
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
database.create_dataset(TEST_DS_NAME)
database.close()
self.assertTrue(database is not None)
# read existing dataset
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
dataset = database.dataset(TEST_DS_NAME)
self.assertTrue(database is not None and dataset is not None)
# create graspable
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
dataset.create_graspable(mesh_processor.key, mesh_processor.mesh,
mesh_processor.sdf,
mesh_processor.stable_poses,
mass=mass)
# read graspable and ensure data integrity
obj = dataset[mesh_processor.key]
self.assertTrue(obj.key == mesh_processor.key)
write_vertices = mesh_processor.mesh.vertices
write_triangles = mesh_processor.mesh.triangles
write_sdf_data = mesh_processor.sdf.data
write_stable_poses = mesh_processor.stable_poses
load_vertices = obj.mesh.vertices
load_triangles = obj.mesh.triangles
load_sdf_data = obj.sdf.data
load_stable_poses = dataset.stable_poses(obj.key)
self.assertTrue(np.allclose(write_vertices, load_vertices))
self.assertTrue(np.allclose(write_triangles, load_triangles))
self.assertTrue(np.allclose(write_sdf_data, load_sdf_data))
self.assertTrue(obj.mass == mass)
for wsp, lsp in zip(write_stable_poses, load_stable_poses):
self.assertTrue(np.allclose(wsp.r, lsp.r))
self.assertTrue(np.allclose(wsp.p, lsp.p))
self.assertTrue(database is not None and dataset is not None)
# test loop access
for obj in dataset:
key = obj.key
# test direct access
obj = dataset[key]
self.assertTrue(obj.key == key)
# read / write meshing
obj = dataset[dataset.object_keys[0]]
mesh_filename = dataset.obj_mesh_filename(obj.key, overwrite=True)
f = ObjFile(mesh_filename)
load_mesh = f.read()
write_vertices = np.array(obj.mesh.vertices)
write_triangles = np.array(obj.mesh.triangles)
load_vertices = np.array(load_mesh.vertices)
load_triangles = np.array(load_mesh.triangles)
self.assertTrue(np.allclose(write_vertices, load_vertices, atol=1e-5))
self.assertTrue(np.allclose(write_triangles, load_triangles, atol=1e-5))
# test rendering images
stable_poses = dataset.stable_poses(obj.key)
stable_pose = stable_poses[0]
# setup virtual camera
width = CONFIG['width']
height = CONFIG['height']
f = CONFIG['focal']
cx = float(width) / 2
cy = float(height) / 2
ci = CameraIntrinsics('camera', fx=f, fy=f, cx=cx, cy=cy,
height=height, width=width)
vp = ViewsphereDiscretizer(min_radius=CONFIG['min_radius'],
max_radius=CONFIG['max_radius'],
num_radii=CONFIG['num_radii'],
min_elev=CONFIG['min_elev']*np.pi,
max_elev=CONFIG['max_elev']*np.pi,
num_elev=CONFIG['num_elev'],
num_az=CONFIG['num_az'],
num_roll=CONFIG['num_roll'])
vc = VirtualCamera(ci)
# render segmasks and depth
render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH, RenderMode.SCALED_DEPTH]
for render_mode in render_modes:
rendered_images = vc.wrapped_images_viewsphere(obj.mesh, vp,
render_mode,
stable_pose)
pre_store_num_images = len(rendered_images)
dataset.store_rendered_images(obj.key, rendered_images,
stable_pose_id=stable_pose.id,
render_mode=render_mode)
rendered_images = dataset.rendered_images(obj.key,
stable_pose_id=stable_pose.id,
render_mode=render_mode)
post_store_num_images = len(rendered_images)
self.assertTrue(pre_store_num_images == post_store_num_images)
# test read / write grasp metrics
metric_name = CONFIG['metrics'].keys()[0]
metric_config = CONFIG['metrics'][metric_name]
dataset.create_metric(metric_name, metric_config)
load_metric_config = dataset.metric(metric_name)
self.assertTrue(dataset.has_metric(metric_name))
for key, value in metric_config.iteritems():
if isinstance(value, dict):
for k, v in value.iteritems():
self.assertTrue(load_metric_config[key][k] == v)
else:
self.assertTrue(load_metric_config[key] == value)
dataset.delete_metric(metric_name)
self.assertFalse(dataset.has_metric(metric_name))
# test read / write grasps
num_grasps = NUM_DB_GRASPS
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
dataset = database.dataset(TEST_DS_NAME)
key = dataset.object_keys[0]
grasps = []
grasp_metrics = {}
for i in range(num_grasps):
configuration = np.random.rand(9)
configuration[3:6] = configuration[3:6] / np.linalg.norm(configuration[3:6])
random_grasp = ParallelJawPtGrasp3D(configuration)
grasps.append(random_grasp)
dataset.store_grasps(key, grasps)
loaded_grasps = dataset.grasps(key)
for g in loaded_grasps:
grasp_metrics[g.id] = {}
grasp_metrics[g.id]['force_closure'] = 1 * (np.random.rand() > 0.5)
for g1, g2 in zip(grasps, loaded_grasps):
self.assertTrue(np.allclose(g1.configuration, g2.configuration))
self.assertTrue(dataset.has_grasps(key))
dataset.store_grasp_metrics(key, grasp_metrics)
loaded_grasp_metrics = dataset.grasp_metrics(key, loaded_grasps)
for i, metrics in loaded_grasp_metrics.iteritems():
self.assertTrue(metrics['force_closure'] == grasp_metrics[i]['force_closure'])
# remove grasps
dataset.delete_grasps(key)
self.assertFalse(dataset.has_grasps(key))
# remove rendered images
render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH, RenderMode.SCALED_DEPTH]
for render_mode in render_modes:
dataset.delete_rendered_images(key, stable_pose_id=stable_pose.id,
render_mode=render_mode)
rendered_images = dataset.rendered_images(key,
stable_pose_id=stable_pose.id,
render_mode=render_mode)
self.assertTrue(len(rendered_images) == 0)
# remove graspable
database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS)
dataset = database.dataset(TEST_DS_NAME)
key = dataset.object_keys[0]
dataset.delete_graspable(key)
obj_deleted = False
try:
obj = dataset[key]
except:
obj_deleted = True
self.assertTrue(obj_deleted)
database.close()
def create_camera_data(self, rgb: np.ndarray, depth: np.ndarray,
cam_intr_frame: str, fx: float, fy: float,
cx: float, cy: float, skew: float, w: int, h: int,
seg_mask: np.ndarray = np.empty(shape=(0,)), bbox: tuple = ()):
"""Create the CameraData object in the correct format expected by gqcnn
Parameters
---------
req: rgb: np.ndarray: rgb image
depth: np.ndarray: depth image
cam_intr_frame: str: the reference frame of the images. Grasp poses are computed wrt this frame
fx: float: focal length (x)
fy: float: focal length (y)
cx: float: principal point (x)
cy: float: principal point (y)
skew: float: skew coefficient
w: int: width
h: int: height
opt: seg_mask: np.ndarray: segmentation mask
bbox: tuple: a tuple of 4 values that define the mask bounding box = (x_min, y_min, x_max, y_max)
Returns:
CameraData: object that stores the input data required by plan_grasp()
"""
camera_data = CameraData()
# Create images
camera_data.rgb_img = ColorImage(rgb, frame=cam_intr_frame)
camera_data.depth_img = DepthImage(depth, frame=cam_intr_frame)
if seg_mask.size > 0:
camera_data.seg_img = BinaryImage(seg_mask, cam_intr_frame)
# Check image sizes
if camera_data.rgb_img.height != camera_data.depth_img.height or \
camera_data.rgb_img.width != camera_data.depth_img.width:
msg = ("Color image and depth image must be the same shape! Color"
" is %d x %d but depth is %d x %d") % (
camera_data.rgb_img.height, camera_data.rgb_img.width,
camera_data.depth_img.height, camera_data.depth_img.width)
raise AssertionError(msg)
if (camera_data.rgb_img.height < self.min_height
or camera_data.rgb_img.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,
camera_data.rgb_img.height, camera_data.rgb_img.width)
raise AssertionError(msg)
if camera_data.rgb_img.height != camera_data.seg_img.height or \
camera_data.rgb_img.width != camera_data.seg_img.width:
msg = ("Images and segmask must be the same shape! Color image is"
" %d x %d but segmask is %d x %d") % (
camera_data.rgb_img.height, camera_data.rgb_img.width,
camera_data.seg_img.height, camera_data.seg_img.width)
raise AssertionError(msg)
# set intrinsic params
camera_data.intrinsic_params = CameraIntrinsics(cam_intr_frame, fx, fy, cx, cy, skew, h, w)
# set mask bounding box
if len(bbox) == 4:
camera_data.bounding_box = {'min_x': bbox[0], 'min_y': bbox[1], 'max_x': bbox[2], 'max_y': bbox[3]}
return camera_data