def test_indexing(self, height=50, width=100):
color_data = (255 * np.random.rand(height, width, 3)).astype(np.uint8)
im = ColorImage(color_data, 'a')
# test valid indexing on color images
i = int(height * np.random.rand())
j = int(width * np.random.rand())
k = int(3 * np.random.rand())
logging.info('Indexing with i=%d, j=%d, k=%d' % (i, j, k))
c_true = color_data[i, j, k]
c_read = im[i, j, k]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image ijk indexing failed')
c_true = color_data[i, j, :]
c_read = im[i, j]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image ij indexing failed')
c_true = color_data[i, :, :]
c_read = im[i]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image i indexing failed')
# test valid slicing on color images
i_start = 0
j_start = 0
k_start = 0
i_stop = int(height * np.random.rand())
j_stop = int(width * np.random.rand())
k_stop = int(3 * np.random.rand())
i_step = 1
j_step = 1
k_step = 1
logging.info('Slicing with i_start=%d, i_stop=%d, i_step=%d, \
j_start=%d, j_stop=%d, j_step=%d, \
k_start=%d, k_stop=%d, k_step=%d' \
%(i_start, i_stop, i_step, \
j_start, j_stop, j_step, \
k_start, k_stop, k_step))
c_true = color_data[i_start:i_stop:i_step, j_start:j_stop:j_step,
k_start:k_stop:k_step]
c_read = im[i_start:i_stop:i_step, j_start:j_stop:j_step,
k_start:k_stop:k_step]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image ijk slicing failed')
# test out of bounds indexing on color image
caught_out_of_bounds = False
try:
c_read = im[-1, j, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, -1, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, j, -1]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[height, j, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, width, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, j, 3]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
# test out of bounds slicing on color image. (Python slicing does not cause out of bound)
caught_out_of_bounds = False
try:
c_read = im[-1:i_stop:i_step, j_start:j_stop:j_step,
k_start:k_stop:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i_start:i_stop:i_step, -1:j_stop:j_step,
k_start:k_stop:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i_start:i_stop:i_step, j_start:j_stop:j_step,
-1:k_stop:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i_start:height + 1:i_step, j_start:j_stop:j_step,
k_start:k_stop:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i_start:i_stop:i_step, j_start:width + 1:j_step,
k_start:k_stop:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i_start:i_stop:i_step, j_start:j_stop:j_step,
k_start:4:k_step]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
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
def saved_frames(self):
"""list of perception.ColorImage : Any color images that have been saved
due to recording or the max_frames argument.
"""
return [ColorImage(f) for f in self._saved_frames]
#%%
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)
#%%
color_im = ColorImage(img.astype(np.uint8), encoding="bgr8", frame='pcl')
depth_im = DepthImage(d.astype(np.float32), frame='pcl')
color_im = color_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
depth_im = depth_im.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
rgbd_state = RgbdImageState(rgbd_im, camera_int)
#%%
grasp = grasp_policy(rgbd_state)
#%%
img2 = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
img2 = cv2.circle(img2,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3)
plt.imshow(img2)
#%%
img3 = cv2.cvtColor(d, cv2.COLOR_BGR2RGB)
img3 = cv2.circle(img3,(int(grasp.grasp.center.x),int(grasp.grasp.center.y)),2,(255,0,0),3)
for i in range(data['num_labels']):
bbox = data['objects'][i]['box']
classnum = data['objects'][i]['class']
classname = ['grasp', 'singulate', 'suction', 'quit'][classnum]
if classname == "grasp":
grasp_boxes.append(bbox)
elif classname == "suction":
suction_boxes.append(bbox)
elif classname == "singulate":
singulate_boxes.append(bbox)
elif classname == "quit":
break
main_mask = crop_img(c_img)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
grasps = []
viz_info = []
for i in range(len(grasp_boxes)):
bbox = grasp_boxes[i]
center_mass, direction = bbox_to_grasp(bbox, c_img, d_img)
viz_info.append([center_mass, direction])
suctions = []
for i in range(len(suction_boxes)):
suctions.append("compute_suction?")
if len(grasps) > 0 or len(suctions) > 0:
# # sensor
camera_intr = CameraIntrinsics.load(camera_intr_filename)
# images
color_cvmat = cv2.imread(color_img_filename)
depth_cvmat = cv2.imread(depth_img_filename) # load as 8UC3
depth_cvmat = cv2.cvtColor(depth_cvmat, cv2.COLOR_BGR2GRAY) # 8UC1
depth_cvmat = depth_cvmat.astype(np.float32) * 1.0 / 255.0 # 32FC1
# cv2.imshow('img', color_cvmat)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
# create wrapped BerkeleyAutomation/perception RGB and depth images
color_im = ColorImage(color_cvmat,
frame=camera_intr.frame,
encoding="bgr8")
depth_im = DepthImage(depth_cvmat, frame=camera_intr.frame)
# 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)
print(msg)
# assume not mask is provided
# segmask = depth_im.invalid_pixel_mask().inverse()
segmask = BinaryImage(255 * np.ones(depth_im.shape).astype(np.uint8),
end_ind_tensors.sort(key=filename_to_file_num)
grasp_tensors.sort(key=filename_to_file_num)
metric_tensors.sort(key=filename_to_file_num)
# extract metadata
num_image_tensors = len(image_tensors)
num_grasp_tensors = len(grasp_tensors)
# load and display each image by selecting one uniformly at random
# CTRL+C to exit
image_tensor_inds = np.arange(num_image_tensors)
########################################################################
camera_intr_filename = "/home/silvia/dex-net/planning/primesense_overhead_ir.intr"
camera_intr = CameraIntrinsics.load(camera_intr_filename)
color_im = ColorImage(np.zeros([400, 400, 3]).astype(np.uint8))
grasp_quality_fn = GraspQualityFunctionFactory.quality_function(
'gqcnn', config['metric'])
for ind in range(num_image_tensors):
print("File: " + str(ind))
image_arr = np.load(image_tensors[ind])['arr_0']
binary_arr = np.load(binary_image_tensors[ind])['arr_0']
start_ind_arr = np.load(start_ind_tensors[ind])['arr_0']
end_ind_arr = np.load(end_ind_tensors[ind])['arr_0']
datapoints_in_file = min(len(start_ind_arr), DATAPOINTS_PER_FILE)
for i in range(datapoints_in_file):
start_ind = start_ind_arr[i]
end_ind = end_ind_arr[i]
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 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!'
)
import numpy as np
import cv2
import IPython
from perception import ColorImage, BinaryImage
from tpc.perception.crop import crop_img
"""
Script to run crop on sample images
Can be used to fine tune crop if necessary
"""
if __name__ == "__main__":
get_img = lambda ind: cv2.imread("debug_imgs/new_setup_crop/img" + str(ind) + ".png")
write_img = lambda img, ind: cv2.imwrite("debug_imgs/new_setup_crop/img_o" + str(ind) + ".png", img)
for i in range(11, 12):
img = get_img(i)
crop_mask = crop_img(img, use_preset=True)
viz = ColorImage(img).mask_binary(crop_mask)
write_img(viz.data, i)
from tpc.perception.cluster_registration import view_hsv
"""
Script to run hsv segmentation on sample images
Can be used to fine tune values if necessary
Works best under consistent lighting (blinds closed) because white highlights on bricks
can be confused with the white background
"""
def get_img(ind):
return cv2.imread("debug_imgs/new_setup_hsv/img" + str(ind) + ".png")
def write_img(img, ind):
cv2.imwrite("debug_imgs/new_setup_hsv/img_o" + str(ind) + ".png", img)
def hsv_channels(ind):
img = get_img(ind)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
hue = hsv[:,:,0]
sat = hsv[:,:,1]
val = hsv[:,:,2]
write_img(hue, str(ind) + "hue")
write_img(sat, str(ind) + "sat")
write_img(val, str(ind) + "val")
if __name__ == "__main__":
for i in range(15, 16):
img = get_img(i)
viz = view_hsv(ColorImage(img))
write_img(viz.data, i)
# hsv_channels(12)
def detect_img():
global color_img
global mask
global depth_im
global color_im
global segmask
global center
global angle
global best_angle
global prev_q_value
global best_center
k4a = PyK4A(
Config(
color_resolution=pyk4a.ColorResolution.RES_720P,
camera_fps=pyk4a.FPS.FPS_30,
depth_mode=pyk4a.DepthMode.WFOV_2X2BINNED,
synchronized_images_only=True,
))
k4a.start()
# getters and setters directly get and set on device
k4a.whitebalance = 4500
assert k4a.whitebalance == 4500
k4a.whitebalance = 4510
assert k4a.whitebalance == 4510
mask = np.ones((WIDTH, HEIGHT), dtype=np.uint8) * 255
while True:
capture = k4a.get_capture()
if np.any(capture.depth) and np.any(capture.color):
depth_img = np.array(capture.depth, dtype=np.uint16)
color_img = np.array(capture.transformed_color, dtype=np.uint8)
depth_img = depth_img / 65535.0 * 100
depth_im = DepthImage(depth_img.astype("float32"),
frame=camera_intr.frame)
color_im = ColorImage(np.zeros([HEIGHT, WIDTH,
3]).astype(np.uint8),
frame=camera_intr.frame)
seg_mask = cv2.resize(mask.copy(), (WIDTH, HEIGHT),
interpolation=cv2.INTER_CUBIC)
segmask = BinaryImage(seg_mask)
#print(depth_img.shape)
normdepth_img = np.uint8(
cv2.normalize(depth_img * 65535 / 100,
dst=None,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX) * 255)
backtorgb = cv2.cvtColor(normdepth_img, cv2.COLOR_GRAY2RGB)
backtorgb = cv2.line(
backtorgb, (int(best_center[0]), int(best_center[1])),
(int(best_center[0]), int(best_center[1]) + 1), (255, 0, 0), 3)
cv2.imshow("k4a", backtorgb)
cv2.imshow("k4a_color", color_img)
key = cv2.waitKey(10)
if key != -1:
cv2.destroyAllWindows()
break
k4a.stop()
# read images
depth_data = np.load(d_file)
#""" #HO FATTO LA CAPPELLA DI CREARE IL DATASET CON UNA ROTAZIONE, QUESTO COMPENSA
rows, cols, _ = depth_data.shape
M = cv2.getRotationMatrix2D((cols / 2, rows / 2), 3, 1)
cv2_img = cv2.warpAffine(depth_data, M, (cols, rows))
depth_data = np.array(cv2_img, dtype=np.float32)
#"""
c_image = np.array(cv2.imread(rgb_file))
#print depth_data
depth_im = DepthImage(depth_data, frame=camera_intr.frame)
print depth_im.shape
color_im = ColorImage(c_image, frame=camera_intr.frame)
# inpaint
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
segmask = None
# create state
#segmask=None
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
flag_human = "y" #raw_input("Do you want to use human contribute? [y/N]")
# query policy
policy_start = time.time()
def lego_demo(self):
self.rollout_data = []
self.get_new_grasp = True
if not DEBUG:
self.gm.position_head()
while True:
time.sleep(1) #making sure the robot is finished moving
c_img = self.cam.read_color_data()
d_img = self.cam.read_depth_data()
cv2.imwrite("debug_imgs/c_img.png", c_img)
print "\n new iteration"
if (not c_img == None and not d_img == None):
#label image
data = self.wl.label_image(c_img)
c_img = self.cam.read_color_data()
grasp_boxes = []
suction_boxes = []
singulate_boxes = []
for i in range(data['num_labels']):
bbox = data['objects'][i]['box']
classnum = data['objects'][i]['class']
classname = ['grasp', 'singulate', 'suction',
'quit'][classnum]
if classname == "grasp":
grasp_boxes.append(bbox)
elif classname == "suction":
suction_boxes.append(bbox)
elif classname == "singulate":
singulate_boxes.append(bbox)
elif classname == "quit":
self.ds.save_rollout()
self.gm.move_to_home()
self.ds.append_data_to_rollout(c_img, data)
main_mask = crop_img(c_img)
col_img = ColorImage(c_img)
workspace_img = col_img.mask_binary(main_mask)
grasps = []
viz_info = []
for i in range(len(grasp_boxes)):
bbox = grasp_boxes[i]
center_mass, direction = bbox_to_grasp(bbox, c_img, d_img)
viz_info.append([center_mass, direction])
pose, rot = self.gm.compute_grasp(center_mass, direction,
d_img)
grasps.append(
self.gripper.get_grasp_pose(pose[0],
pose[1],
pose[2],
rot,
c_img=workspace_img.data))
suctions = []
for i in range(len(suction_boxes)):
suctions.append("compute_suction?")
if len(grasps) > 0 or len(suctions) > 0:
cv2.imwrite(
"grasps.png",
visualize(workspace_img, [v[0] for v in viz_info],
[v[1] for v in viz_info]))
print "running grasps"
for grasp in grasps:
print "grasping", grasp
self.gm.execute_grasp(grasp)
print "running suctions"
for suction in suctions:
print "suctioning", suction
#execute suction
elif len(singulate_boxes) > 0:
print("singulating")
bbox = singulate_boxes[0]
obj_mask = bbox_to_mask(bbox, c_img)
start, end = find_singulation(col_img, main_mask, obj_mask)
display_singulation(start, end, workspace_img)
self.gm.singulate(start, end, c_img, d_img)
else:
print("cleared workspace")
self.whole_body.move_to_go()
self.gm.position_head()
def wrapped_images(self, mesh, object_to_camera_poses,
render_mode, stable_pose=None, mat_props=None,
light_props=None,debug=False):
"""Create ObjectRender objects of the given mesh at the list of object to camera poses.
Parameters
----------
mesh : :obj:`Mesh3D`
The mesh to be rendered.
object_to_camera_poses : :obj:`list` of :obj:`RigidTransform`
A list of object to camera transforms to render from.
render_mode : int
One of RenderMode.COLOR, RenderMode.DEPTH, or
RenderMode.SCALED_DEPTH.
stable_pose : :obj:`StablePose`
A stable pose to render the object in.
mat_props : :obj:`MaterialProperties`
Material properties for the mesh
light_props : :obj:`MaterialProperties`
Lighting properties for the scene
debug : bool
Whether or not to debug the C++ meshrendering code.
Returns
-------
:obj:`list` of :obj:`ObjectRender`
A list of ObjectRender objects generated from the given parameters.
"""
# pre-multiply the stable pose
world_to_camera_poses = [T_obj_camera.as_frames('obj', 'camera') for T_obj_camera in object_to_camera_poses]
if stable_pose is not None:
t_obj_stp = np.array([0,0,-stable_pose.r.dot(stable_pose.x0)[2]])
T_obj_stp = RigidTransform(rotation=stable_pose.r,
translation=t_obj_stp,
from_frame='obj',
to_frame='stp')
stp_to_camera_poses = copy.copy(object_to_camera_poses)
object_to_camera_poses = []
for T_stp_camera in stp_to_camera_poses:
T_stp_camera.from_frame = 'stp'
object_to_camera_poses.append(T_stp_camera.dot(T_obj_stp))
# set lighting mode
enable_lighting = True
if render_mode == RenderMode.SEGMASK or render_mode == RenderMode.DEPTH or \
render_mode == RenderMode.DEPTH_SCENE:
enable_lighting = False
# render both image types (doesn't really cost any time)
color_ims, depth_ims = self.images(mesh, object_to_camera_poses,
mat_props=mat_props,
light_props=light_props,
enable_lighting=enable_lighting,
debug=debug)
# convert to image wrapper classes
images = []
if render_mode == RenderMode.SEGMASK:
# wrap binary images
for binary_im in color_ims:
images.append(BinaryImage(binary_im[:,:,0], frame=self._camera_intr.frame, threshold=0))
elif render_mode == RenderMode.COLOR:
# wrap color images
for color_im in color_ims:
images.append(ColorImage(color_im, frame=self._camera_intr.frame))
elif render_mode == RenderMode.COLOR_SCENE:
# wrap color and depth images
for color_im in color_ims:
images.append(ColorImage(color_im, frame=self._camera_intr.frame))
# render images of scene objects
color_scene_ims = {}
for name, scene_obj in self._scene.iteritems():
scene_object_to_camera_poses = []
for world_to_camera_pose in world_to_camera_poses:
scene_object_to_camera_poses.append(world_to_camera_pose * scene_obj.T_mesh_world)
color_scene_ims[name] = self.wrapped_images(scene_obj.mesh, scene_object_to_camera_poses, RenderMode.COLOR, mat_props=scene_obj.mat_props, light_props=light_props, debug=debug)
# combine with scene images
# TODO: re-add farther
for i in range(len(images)):
for j, name in enumerate(color_scene_ims.keys()):
zero_px = images[i].zero_pixels()
images[i].data[zero_px[:,0], zero_px[:,1], :] = color_scene_ims[name][i].image.data[zero_px[:,0], zero_px[:,1], :]
elif render_mode == RenderMode.DEPTH:
# render depth image
for depth_im in depth_ims:
images.append(DepthImage(depth_im, frame=self._camera_intr.frame))
elif render_mode == RenderMode.DEPTH_SCENE:
# create empty depth images
for depth_im in depth_ims:
images.append(DepthImage(depth_im, frame=self._camera_intr.frame))
# render images of scene objects
depth_scene_ims = {}
for name, scene_obj in self._scene.iteritems():
scene_object_to_camera_poses = []
for world_to_camera_pose in world_to_camera_poses:
scene_object_to_camera_poses.append(world_to_camera_pose * scene_obj.T_mesh_world)
depth_scene_ims[name] = self.wrapped_images(scene_obj.mesh, scene_object_to_camera_poses, RenderMode.DEPTH, mat_props=scene_obj.mat_props, light_props=light_props)
# combine with scene images
for i in range(len(images)):
for j, name in enumerate(depth_scene_ims.keys()):
images[i] = images[i].combine_with(depth_scene_ims[name][i].image)
elif render_mode == RenderMode.RGBD:
# create RGB-D images
for color_im, depth_im in zip(color_ims, depth_ims):
c = ColorImage(color_im, frame=self._camera_intr.frame)
d = DepthImage(depth_im, frame=self._camera_intr.frame)
images.append(RgbdImage.from_color_and_depth(c, d))
elif render_mode == RenderMode.RGBD_SCENE:
# create RGB-D images
for color_im, depth_im in zip(color_ims, depth_ims):
c = ColorImage(color_im, frame=self._camera_intr.frame)
d = DepthImage(depth_im, frame=self._camera_intr.frame)
images.append(RgbdImage.from_color_and_depth(c, d))
# render images of scene objects
rgbd_scene_ims = {}
for name, scene_obj in self._scene.iteritems():
scene_object_to_camera_poses = []
for world_to_camera_pose in world_to_camera_poses:
scene_object_to_camera_poses.append(world_to_camera_pose * scene_obj.T_mesh_world)
rgbd_scene_ims[name] = self.wrapped_images(scene_obj.mesh, scene_object_to_camera_poses, RenderMode.RGBD, mat_props=scene_obj.mat_props, light_props=light_props, debug=debug)
# combine with scene images
for i in range(len(images)):
for j, name in enumerate(rgbd_scene_ims.keys()):
images[i] = images[i].combine_with(rgbd_scene_ims[name][i].image)
elif render_mode == RenderMode.SCALED_DEPTH:
# convert to color image
for depth_im in depth_ims:
d = DepthImage(depth_im, frame=self._camera_intr.frame)
images.append(d.to_color())
else:
raise ValueError('Render mode %s not supported' %(render_mode))
# create object renders
if stable_pose is not None:
object_to_camera_poses = copy.copy(stp_to_camera_poses)
rendered_images = []
for image, T_obj_camera in zip(images, object_to_camera_poses):
T_camera_obj = T_obj_camera.inverse()
rendered_images.append(ObjectRender(image, T_camera_obj))
return rendered_images
def visualize(self):
""" Visualize predictions """
logging.info('Visualizing ' + self.datapoint_type)
# iterate through shuffled file indices
for i in self.indices:
im_filename = self.im_filenames[i]
pose_filename = self.pose_filenames[i]
label_filename = self.label_filenames[i]
logging.info('Loading Image File: ' + im_filename + ' Pose File: ' + pose_filename + ' Label File: ' + label_filename)
# load tensors from files
metric_tensor = np.load(os.path.join(self.data_dir, label_filename))['arr_0']
label_tensor = 1 * (metric_tensor > self.metric_thresh)
image_tensor = np.load(os.path.join(self.data_dir, im_filename))['arr_0']
hand_poses_tensor = np.load(os.path.join(self.data_dir, pose_filename))['arr_0']
pose_tensor = self._read_pose_data(hand_poses_tensor, self.input_data_mode)
# score with neural network
pred_p_success_tensor = self._gqcnn.predict(image_tensor, pose_tensor)
# compute results
classification_result = ClassificationResult([pred_p_success_tensor],
[label_tensor])
logging.info('Error rate on files: %.3f' %(classification_result.error_rate))
logging.info('Precision on files: %.3f' %(classification_result.precision))
logging.info('Recall on files: %.3f' %(classification_result.recall))
mispred_ind = classification_result.mispredicted_indices()
correct_ind = classification_result.correct_indices()
# IPython.embed()
if self.datapoint_type == 'true_positive' or self.datapoint_type == 'true_negative':
vis_ind = correct_ind
else:
vis_ind = mispred_ind
num_visualized = 0
# visualize
for ind in vis_ind:
# limit the number of sampled datapoints displayed per object
if num_visualized >= self.samples_per_object:
break
num_visualized += 1
# don't visualize the datapoints that we don't want
if self.datapoint_type == 'true_positive':
if classification_result.labels[ind] == 0:
continue
elif self.datapoint_type == 'true_negative':
if classification_result.labels[ind] == 1:
continue
elif self.datapoint_type == 'false_positive':
if classification_result.labels[ind] == 0:
continue
elif self.datapoint_type == 'false_negative':
if classification_result.labels[ind] == 1:
continue
logging.info('Datapoint %d of files for %s' %(ind, im_filename))
logging.info('Depth: %.3f' %(hand_poses_tensor[ind, 2]))
data = image_tensor[ind,...]
if self.display_image_type == RenderMode.SEGMASK:
image = BinaryImage(data)
elif self.display_image_type == RenderMode.GRAYSCALE:
image = GrayscaleImage(data)
elif self.display_image_type == RenderMode.COLOR:
image = ColorImage(data)
elif self.display_image_type == RenderMode.DEPTH:
image = DepthImage(data)
elif self.display_image_type == RenderMode.RGBD:
image = RgbdImage(data)
elif self.display_image_type == RenderMode.GD:
image = GdImage(data)
vis2d.figure()
if self.display_image_type == RenderMode.RGBD:
vis2d.subplot(1,2,1)
vis2d.imshow(image.color)
grasp = Grasp2D(Point(image.center, 'img'), 0, hand_poses_tensor[ind, 2], self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.subplot(1,2,2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp)
elif self.display_image_type == RenderMode.GD:
vis2d.subplot(1,2,1)
vis2d.imshow(image.gray)
grasp = Grasp2D(Point(image.center, 'img'), 0, hand_poses_tensor[ind, 2], self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.subplot(1,2,2)
vis2d.imshow(image.depth)
vis2d.grasp(grasp)
else:
vis2d.imshow(image)
grasp = Grasp2D(Point(image.center, 'img'), 0, hand_poses_tensor[ind, 2], self.gripper_width_m)
grasp.camera_intr = grasp.camera_intr.resize(1.0 / 3.0)
vis2d.grasp(grasp)
vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(ind,
classification_result.pred_probs[ind,1],
classification_result.labels[ind]))
vis2d.show()
# cleanup
self._cleanup()
# wait for Grasp Planning Service and create Service Proxy
rospy.wait_for_service('%s/grasp_planner' % (namespace))
rospy.wait_for_service('%s/grasp_planner_segmask' % (namespace))
plan_grasp = rospy.ServiceProxy('%s/grasp_planner' % (namespace),
GQCNNGraspPlanner)
plan_grasp_segmask = rospy.ServiceProxy(
'%s/grasp_planner_segmask' % (namespace), GQCNNGraspPlannerSegmask)
cv_bridge = CvBridge()
# setup sensor
camera_intr = CameraIntrinsics.load(camera_intr_filename)
# read images
depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame)
color_im = ColorImage(np.zeros([depth_im.height, depth_im.width,
3]).astype(np.uint8),
frame=camera_intr.frame)
# read segmask
if segmask_filename is not None:
segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame)
grasp_resp = plan_grasp_segmask(color_im.rosmsg, depth_im.rosmsg,
camera_intr.rosmsg, segmask.rosmsg)
else:
grasp_resp = plan_grasp(color_im.rosmsg, depth_im.rosmsg,
camera_intr.rosmsg)
grasp = grasp_resp.grasp
# convert to a grasp action
grasp_type = grasp.grasp_type
if grasp_type == GQCNNGrasp.PARALLEL_JAW:
def wrapped_render(self, render_modes, front_and_back=False):
"""Render images of the scene and wrap them with Image wrapper classes
from the Berkeley AUTOLab's perception module.
Parameters
----------
render_modes : list of perception.RenderMode
A list of the desired image types to return, from the perception
module's RenderMode enum.
front_and_back : bool
If True, all surface normals are treated as if they're facing the camera.
Returns
-------
list of perception.Image
A list containing a corresponding Image sub-class for each type listed
in render_modes.
"""
# Render raw images
render_color = False
for mode in render_modes:
if mode != RenderMode.DEPTH and mode != RenderMode.SCALED_DEPTH:
render_color = True
break
color_im, depth_im = None, None
if render_color:
color_im, depth_im = self.render(render_color, front_and_back=front_and_back)
else:
depth_im = self.render(render_color)
# For each specified render mode, add an Image object of the appropriate type
images = []
for render_mode in render_modes:
# Then, convert them to an image wrapper class
if render_mode == RenderMode.SEGMASK:
images.append(BinaryImage((depth_im > 0.0).astype(np.uint8), frame=self.camera.intrinsics.frame, threshold=0))
elif render_mode == RenderMode.COLOR:
images.append(ColorImage(color_im, frame=self.camera.intrinsics.frame))
elif render_mode == RenderMode.GRAYSCALE:
images.append(ColorImage(color_im, frame=self.camera.intrinsics.frame).to_grayscale())
elif render_mode == RenderMode.DEPTH:
images.append(DepthImage(depth_im, frame=self.camera.intrinsics.frame))
elif render_mode == RenderMode.SCALED_DEPTH:
images.append(DepthImage(depth_im, frame=self.camera.intrinsics.frame).to_color())
elif render_mode == RenderMode.RGBD:
c = ColorImage(color_im, frame=self.camera.intrinsics.frame)
d = DepthImage(depth_im, frame=self.camera.intrinsics.frame)
images.append(RgbdImage.from_color_and_depth(c, d))
elif render_mode == RenderMode.GD:
g = ColorImage(color_im, frame=self.camera.intrinsics.frame).to_grayscale()
d = DepthImage(depth_im, frame=self.camera.intrinsics.frame)
images.append(GdImage.from_grayscale_and_depth(g, d))
else:
raise ValueError('Render mode {} not supported'.format(render_mode))
return images
def test_indexing(self, height=50, width=100):
color_data = (255 * np.random.rand(height, width, 3)).astype(np.uint8)
im = ColorImage(color_data, 'a')
# test valid indexing on color images
i = int(height * np.random.rand())
j = int(width * np.random.rand())
k = int(3 * np.random.rand())
print
logging.info('Indexing with i=%d, j=%d, k=%d' % (i, j, k))
c_true = color_data[i, j, k]
c_read = im[i, j, k]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image ijk indexing failed')
c_true = color_data[i, j, :]
c_read = im[i, j]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image ij indexing failed')
c_true = color_data[i, :, :]
c_read = im[i]
self.assertTrue(np.sum(np.abs(c_true - c_read)) < 1e-5,
msg='Image i indexing failed')
# test out of bounds indexing on color image
caught_out_of_bounds = False
try:
c_read = im[-1, j, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, -1, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, j, -1]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[height, j, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, width, k]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
caught_out_of_bounds = False
try:
c_read = im[i, j, 3]
except ValueError as e:
caught_out_of_bounds = True
self.assertTrue(caught_out_of_bounds)
type=str,
default='cfg/tools/sample_antipodal_grasp.yaml',
help='path to configuration file to use')
args = parser.parse_args()
depth_im_filename = args.depth_im_filename
camera_intr_filename = args.camera_intr_filename
config_filename = args.config_filename
# read config
config = YamlConfig(config_filename)
policy_config = config['policy']
# read image
depth_im_arr = pkl.load(open(depth_im_filename, 'r'))
depth_im = DepthImage(depth_im_arr)
color_im = ColorImage(
np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8))
rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im)
segmask_arr = 255 * (depth_im_arr < 1.0)
segmask = BinaryImage(segmask_arr.astype(np.uint8))
camera_intr = CameraIntrinsics.load(camera_intr_filename)
state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask)
# init policy
policy = UniformRandomGraspingPolicy(policy_config)
policy_start = time.time()
action = policy(state)
logging.info('Planning took %.3f sec' % (time.time() - policy_start))
# vis final grasp
if policy_config['vis']['final_grasp']:
vis.figure(size=(10, 10))
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 detect_img():
global color_img
global mask
global depth_im
global color_im
global segmask
global center
global angle
global best_angle
global prev_q_value
global best_center
global depth_frame
global frames
def nothing(x):
pass
mask = np.ones((640, 480), dtype=np.uint8) * 255
cv2.namedWindow("rgb", cv2.WINDOW_NORMAL)
cv2.resizeWindow("rgb", 1280, 720)
cv2.createTrackbar('W', "rgb", 0, 100, nothing)
cv2.namedWindow("mask", cv2.WINDOW_NORMAL)
cv2.resizeWindow("mask", 640, 480)
cv2.namedWindow("depth", cv2.WINDOW_NORMAL)
cv2.resizeWindow("depth", 1000, 1000)
line_arr1 = [0, 1, 3, 2, 0, 4, 6, 2]
line_arr2 = [5, 4, 6, 7, 5, 1, 3, 7]
divide_value = 65535
pipeline.start(config)
frames = pipeline.wait_for_frames()
while 1:
try:
frames = pipeline.wait_for_frames()
w = cv2.getTrackbarPos('W', "rgb")
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
depth_to_color_img = np.asanyarray(
depth_frame.get_data()) / divide_value
color_img = np.asanyarray(color_frame.get_data())
#print(depth_to_color_img)
depth_im = DepthImage(depth_to_color_img.astype("float32"),
frame=camera_intr.frame)
color_im = ColorImage(np.zeros([480, 640, 3]).astype(np.uint8),
frame=camera_intr.frame)
seg_mask = cv2.resize(mask.copy(), (640, 480),
interpolation=cv2.INTER_CUBIC)
segmask = BinaryImage(seg_mask)
show_img = cv2.resize(color_img.copy(), (640, 480),
interpolation=cv2.INTER_CUBIC)
try:
ret, img_binary = cv2.threshold(mask, 127, 255, 0)
contours, color_hierachy = cv2.findContours(
img_binary.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
c0 = np.reshape(contours[0], (-1, 2))
show_img = cv2.drawContours(np.uint8(show_img), contours, -1,
(0, 255, 0), 1)
except:
pass
show_img_resize = cv2.resize(show_img.copy(), (640, 480),
interpolation=cv2.INTER_CUBIC)
try:
r = 50
show_img_resize = cv2.line(
show_img_resize,
(int(best_center[0] -
r * math.cos(best_angle / 180 * math.pi)),
int(best_center[1] -
r * math.sin(best_angle / 180 * math.pi))),
(int(best_center[0] +
r * math.cos(best_angle / 180 * math.pi)),
int(best_center[1] +
r * math.sin(best_angle / 180 * math.pi))),
(0, 0, 255), 5)
show_img_resize = cv2.line(
show_img_resize,
(int(best_center[0]), int(best_center[1])),
(int(best_center[0] + 1), int(best_center[1])),
(0, 136, 255), 7)
show_img_resize = cv2.line(
show_img_resize,
(int(center[0] - r * math.cos(angle / 180 * math.pi)),
int(center[1] - r * math.sin(angle / 180 * math.pi))),
(int(center[0] + r * math.cos(angle / 180 * math.pi)),
int(center[1] + r * math.sin(angle / 180 * math.pi))),
(255, 100, 255), 5)
show_img_resize = cv2.line(
show_img_resize, (int(center[0]), int(center[1])),
(int(center[0] + 1), int(center[1])), (100, 136, 255), 7)
except:
pass
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_to_color_img * 1000, alpha=2), 1)
cv2.imshow("rgb", show_img_resize)
cv2.imshow("depth", depth_colormap)
cv2.imshow("mask", seg_mask)
k = cv2.waitKey(5) & 0xFF
if k == ord('s'):
cv2.destroyWindow("rgb")
except Exception as inst:
print(inst)
pass
end()
def generate_segmask_dataset(output_dataset_path,
config,
save_tensors=True,
warm_start=False):
""" Generate a segmentation training dataset
Parameters
----------
dataset_path : str
path to store the dataset
config : dict
dictionary-like objects containing parameters of the simulator and visualization
save_tensors : bool
save tensor datasets (for recreating state)
warm_start : bool
restart dataset generation from a previous state
"""
# read subconfigs
dataset_config = config['dataset']
image_config = config['images']
vis_config = config['vis']
# debugging
debug = config['debug']
if debug:
np.random.seed(SEED)
# read general parameters
num_states = config['num_states']
num_images_per_state = config['num_images_per_state']
states_per_flush = config['states_per_flush']
states_per_garbage_collect = config['states_per_garbage_collect']
# set max obj per state
max_objs_per_state = config['state_space']['heap']['max_objs']
# read image parameters
im_height = config['state_space']['camera']['im_height']
im_width = config['state_space']['camera']['im_width']
segmask_channels = max_objs_per_state + 1
# create the dataset path and all subfolders if they don't exist
if not os.path.exists(output_dataset_path):
os.mkdir(output_dataset_path)
image_dir = os.path.join(output_dataset_path, 'images')
if not os.path.exists(image_dir):
os.mkdir(image_dir)
color_dir = os.path.join(image_dir, 'color_ims')
if image_config['color'] and not os.path.exists(color_dir):
os.mkdir(color_dir)
depth_dir = os.path.join(image_dir, 'depth_ims')
if image_config['depth'] and not os.path.exists(depth_dir):
os.mkdir(depth_dir)
amodal_dir = os.path.join(image_dir, 'amodal_masks')
if image_config['amodal'] and not os.path.exists(amodal_dir):
os.mkdir(amodal_dir)
modal_dir = os.path.join(image_dir, 'modal_masks')
if image_config['modal'] and not os.path.exists(modal_dir):
os.mkdir(modal_dir)
semantic_dir = os.path.join(image_dir, 'semantic_masks')
if image_config['semantic'] and not os.path.exists(semantic_dir):
os.mkdir(semantic_dir)
# setup logging
experiment_log_filename = os.path.join(output_dataset_path,
'dataset_generation.log')
if os.path.exists(experiment_log_filename) and not warm_start:
os.remove(experiment_log_filename)
Logger.add_log_file(logger, experiment_log_filename, global_log_file=True)
config.save(
os.path.join(output_dataset_path, 'dataset_generation_params.yaml'))
metadata = {}
num_prev_states = 0
# set dataset params
if save_tensors:
# read dataset subconfigs
state_dataset_config = dataset_config['states']
image_dataset_config = dataset_config['images']
state_tensor_config = state_dataset_config['tensors']
image_tensor_config = image_dataset_config['tensors']
obj_pose_dim = POSE_DIM * max_objs_per_state
obj_com_dim = POINT_DIM * max_objs_per_state
state_tensor_config['fields']['obj_poses']['height'] = obj_pose_dim
state_tensor_config['fields']['obj_coms']['height'] = obj_com_dim
state_tensor_config['fields']['obj_ids']['height'] = max_objs_per_state
image_tensor_config['fields']['camera_pose']['height'] = POSE_DIM
if image_config['color']:
image_tensor_config['fields']['color_im'] = {
'dtype': 'uint8',
'channels': 3,
'height': im_height,
'width': im_width
}
if image_config['depth']:
image_tensor_config['fields']['depth_im'] = {
'dtype': 'float32',
'channels': 1,
'height': im_height,
'width': im_width
}
if image_config['modal']:
image_tensor_config['fields']['modal_segmasks'] = {
'dtype': 'uint8',
'channels': segmask_channels,
'height': im_height,
'width': im_width
}
if image_config['amodal']:
image_tensor_config['fields']['amodal_segmasks'] = {
'dtype': 'uint8',
'channels': segmask_channels,
'height': im_height,
'width': im_width
}
if image_config['semantic']:
image_tensor_config['fields']['semantic_segmasks'] = {
'dtype': 'uint8',
'channels': 1,
'height': im_height,
'width': im_width
}
# create dataset filenames
state_dataset_path = os.path.join(output_dataset_path, 'state_tensors')
image_dataset_path = os.path.join(output_dataset_path, 'image_tensors')
if warm_start:
if not os.path.exists(state_dataset_path) or not os.path.exists(
image_dataset_path):
logger.error(
'Attempting to warm start without saved tensor dataset')
exit(1)
# open datasets
logger.info('Opening state dataset')
state_dataset = TensorDataset.open(state_dataset_path,
access_mode='READ_WRITE')
logger.info('Opening image dataset')
image_dataset = TensorDataset.open(image_dataset_path,
access_mode='READ_WRITE')
# read configs
state_tensor_config = state_dataset.config
image_tensor_config = image_dataset.config
# clean up datasets (there may be datapoints with indices corresponding to non-existent data)
num_state_datapoints = state_dataset.num_datapoints
num_image_datapoints = image_dataset.num_datapoints
num_prev_states = num_state_datapoints
# clean up images
image_ind = num_image_datapoints - 1
image_datapoint = image_dataset[image_ind]
while image_ind > 0 and image_datapoint[
'state_ind'] >= num_state_datapoints:
image_ind -= 1
image_datapoint = image_dataset[image_ind]
images_to_remove = num_image_datapoints - 1 - image_ind
logger.info('Deleting last %d image tensors' % (images_to_remove))
if images_to_remove > 0:
image_dataset.delete_last(images_to_remove)
num_image_datapoints = image_dataset.num_datapoints
else:
# create datasets from scratch
logger.info('Creating datasets')
state_dataset = TensorDataset(state_dataset_path,
state_tensor_config)
image_dataset = TensorDataset(image_dataset_path,
image_tensor_config)
# read templates
state_datapoint = state_dataset.datapoint_template
image_datapoint = image_dataset.datapoint_template
if warm_start:
if not os.path.exists(
os.path.join(output_dataset_path, 'metadata.json')):
logger.error(
'Attempting to warm start without previously created dataset')
exit(1)
# Read metadata and indices
metadata = json.load(
open(os.path.join(output_dataset_path, 'metadata.json'), 'r'))
test_inds = np.load(os.path.join(image_dir,
'test_indices.npy')).tolist()
train_inds = np.load(os.path.join(image_dir,
'train_indices.npy')).tolist()
# set obj ids and splits
reverse_obj_ids = metadata['obj_ids']
obj_id_map = utils.reverse_dictionary(reverse_obj_ids)
obj_splits = metadata['obj_splits']
obj_keys = obj_splits.keys()
mesh_filenames = metadata['meshes']
# Get list of images generated so far
generated_images = sorted(
os.listdir(color_dir)) if image_config['color'] else sorted(
os.listdir(depth_dir))
num_total_images = len(generated_images)
# Do our own calculation if no saved tensors
if num_prev_states == 0:
num_prev_states = num_total_images // num_images_per_state
# Find images to remove and remove them from all relevant places if they exist
num_images_to_remove = num_total_images - (num_prev_states *
num_images_per_state)
logger.info(
'Deleting last {} invalid images'.format(num_images_to_remove))
for k in range(num_images_to_remove):
im_name = generated_images[-(k + 1)]
im_basename = os.path.splitext(im_name)[0]
im_ind = int(im_basename.split('_')[1])
if os.path.exists(os.path.join(depth_dir, im_name)):
os.remove(os.path.join(depth_dir, im_name))
if os.path.exists(os.path.join(color_dir, im_name)):
os.remove(os.path.join(color_dir, im_name))
if os.path.exists(os.path.join(semantic_dir, im_name)):
os.remove(os.path.join(semantic_dir, im_name))
if os.path.exists(os.path.join(modal_dir, im_basename)):
shutil.rmtree(os.path.join(modal_dir, im_basename))
if os.path.exists(os.path.join(amodal_dir, im_basename)):
shutil.rmtree(os.path.join(amodal_dir, im_basename))
if im_ind in train_inds:
train_inds.remove(im_ind)
elif im_ind in test_inds:
test_inds.remove(im_ind)
else:
# Create initial env to generate metadata
env = BinHeapEnv(config)
obj_id_map = env.state_space.obj_id_map
obj_keys = env.state_space.obj_keys
obj_splits = env.state_space.obj_splits
mesh_filenames = env.state_space.mesh_filenames
save_obj_id_map = obj_id_map.copy()
save_obj_id_map[ENVIRONMENT_KEY] = np.iinfo(np.uint32).max
reverse_obj_ids = utils.reverse_dictionary(save_obj_id_map)
metadata['obj_ids'] = reverse_obj_ids
metadata['obj_splits'] = obj_splits
metadata['meshes'] = mesh_filenames
json.dump(metadata,
open(os.path.join(output_dataset_path, 'metadata.json'),
'w'),
indent=JSON_INDENT,
sort_keys=True)
train_inds = []
test_inds = []
# generate states and images
state_id = num_prev_states
while state_id < num_states:
# create env and set objects
create_start = time.time()
env = BinHeapEnv(config)
env.state_space.obj_id_map = obj_id_map
env.state_space.obj_keys = obj_keys
env.state_space.set_splits(obj_splits)
env.state_space.mesh_filenames = mesh_filenames
create_stop = time.time()
logger.info('Creating env took %.3f sec' %
(create_stop - create_start))
# sample states
states_remaining = num_states - state_id
for i in range(min(states_per_garbage_collect, states_remaining)):
# log current rollout
if state_id % config['log_rate'] == 0:
logger.info('State: %06d' % (state_id))
try:
# reset env
env.reset()
state = env.state
split = state.metadata['split']
# render state
if vis_config['state']:
env.view_3d_scene()
# Save state if desired
if save_tensors:
# set obj state variables
obj_pose_vec = np.zeros(obj_pose_dim)
obj_com_vec = np.zeros(obj_com_dim)
obj_id_vec = np.iinfo(
np.uint32).max * np.ones(max_objs_per_state)
j = 0
for obj_state in state.obj_states:
obj_pose_vec[j * POSE_DIM:(j + 1) *
POSE_DIM] = obj_state.pose.vec
obj_com_vec[j * POINT_DIM:(j + 1) *
POINT_DIM] = obj_state.center_of_mass
obj_id_vec[j] = int(obj_id_map[obj_state.key])
j += 1
# store datapoint env params
state_datapoint['state_id'] = state_id
state_datapoint['obj_poses'] = obj_pose_vec
state_datapoint['obj_coms'] = obj_com_vec
state_datapoint['obj_ids'] = obj_id_vec
state_datapoint['split'] = split
# store state datapoint
image_start_ind = image_dataset.num_datapoints
image_end_ind = image_start_ind + num_images_per_state
state_datapoint['image_start_ind'] = image_start_ind
state_datapoint['image_end_ind'] = image_end_ind
# clean up
del obj_pose_vec
del obj_com_vec
del obj_id_vec
# add state
state_dataset.add(state_datapoint)
# render images
for k in range(num_images_per_state):
# reset the camera
if num_images_per_state > 1:
env.reset_camera()
obs = env.render_camera_image(color=image_config['color'])
if image_config['color']:
color_obs, depth_obs = obs
else:
depth_obs = obs
# vis obs
if vis_config['obs']:
if image_config['depth']:
plt.figure()
plt.imshow(depth_obs)
plt.title('Depth Observation')
if image_config['color']:
plt.figure()
plt.imshow(color_obs)
plt.title('Color Observation')
plt.show()
if image_config['modal'] or image_config[
'amodal'] or image_config['semantic']:
# render segmasks
amodal_segmasks, modal_segmasks = env.render_segmentation_images(
)
# retrieve segmask data
modal_segmask_arr = np.iinfo(np.uint8).max * np.ones(
[im_height, im_width, segmask_channels],
dtype=np.uint8)
amodal_segmask_arr = np.iinfo(np.uint8).max * np.ones(
[im_height, im_width, segmask_channels],
dtype=np.uint8)
stacked_segmask_arr = np.zeros(
[im_height, im_width, 1], dtype=np.uint8)
modal_segmask_arr[:, :, :env.
num_objects] = modal_segmasks
amodal_segmask_arr[:, :, :env.
num_objects] = amodal_segmasks
if image_config['semantic']:
for j in range(env.num_objects):
this_obj_px = np.where(
modal_segmasks[:, :, j] > 0)
stacked_segmask_arr[this_obj_px[0],
this_obj_px[1], 0] = j + 1
# visualize
if vis_config['semantic']:
plt.figure()
plt.imshow(stacked_segmask_arr.squeeze())
plt.show()
if save_tensors:
# save image data as tensors
if image_config['color']:
image_datapoint['color_im'] = color_obs
if image_config['depth']:
image_datapoint['depth_im'] = depth_obs[:, :, None]
if image_config['modal']:
image_datapoint[
'modal_segmasks'] = modal_segmask_arr
if image_config['amodal']:
image_datapoint[
'amodal_segmasks'] = amodal_segmask_arr
if image_config['semantic']:
image_datapoint[
'semantic_segmasks'] = stacked_segmask_arr
image_datapoint['camera_pose'] = env.camera.pose.vec
image_datapoint[
'camera_intrs'] = env.camera.intrinsics.vec
image_datapoint['state_ind'] = state_id
image_datapoint['split'] = split
# add image
image_dataset.add(image_datapoint)
# Save depth image and semantic masks
if image_config['color']:
ColorImage(color_obs).save(
os.path.join(
color_dir, 'image_{:06d}.png'.format(
num_images_per_state * state_id + k)))
if image_config['depth']:
DepthImage(depth_obs).save(
os.path.join(
depth_dir, 'image_{:06d}.png'.format(
num_images_per_state * state_id + k)))
if image_config['modal']:
modal_id_dir = os.path.join(
modal_dir,
'image_{:06d}'.format(num_images_per_state *
state_id + k))
if not os.path.exists(modal_id_dir):
os.mkdir(modal_id_dir)
for i in range(env.num_objects):
BinaryImage(modal_segmask_arr[:, :, i]).save(
os.path.join(modal_id_dir,
'channel_{:03d}.png'.format(i)))
if image_config['amodal']:
amodal_id_dir = os.path.join(
amodal_dir,
'image_{:06d}'.format(num_images_per_state *
state_id + k))
if not os.path.exists(amodal_id_dir):
os.mkdir(amodal_id_dir)
for i in range(env.num_objects):
BinaryImage(amodal_segmask_arr[:, :, i]).save(
os.path.join(amodal_id_dir,
'channel_{:03d}.png'.format(i)))
if image_config['semantic']:
GrayscaleImage(stacked_segmask_arr.squeeze()).save(
os.path.join(
semantic_dir, 'image_{:06d}.png'.format(
num_images_per_state * state_id + k)))
# Save split
if split == TRAIN_ID:
train_inds.append(num_images_per_state * state_id + k)
else:
test_inds.append(num_images_per_state * state_id + k)
# auto-flush after every so many timesteps
if state_id % states_per_flush == 0:
np.save(os.path.join(image_dir, 'train_indices.npy'),
train_inds)
np.save(os.path.join(image_dir, 'test_indices.npy'),
test_inds)
if save_tensors:
state_dataset.flush()
image_dataset.flush()
# delete action objects
for obj_state in state.obj_states:
del obj_state
del state
gc.collect()
# update state id
state_id += 1
except Exception as e:
# log an error
logger.warning('Heap failed!')
logger.warning('%s' % (str(e)))
logger.warning(traceback.print_exc())
if debug:
raise
del env
gc.collect()
env = BinHeapEnv(config)
env.state_space.obj_id_map = obj_id_map
env.state_space.obj_keys = obj_keys
env.state_space.set_splits(obj_splits)
env.state_space.mesh_filenames = mesh_filenames
# garbage collect
del env
gc.collect()
# write all datasets to file, save indices
np.save(os.path.join(image_dir, 'train_indices.npy'), train_inds)
np.save(os.path.join(image_dir, 'test_indices.npy'), test_inds)
if save_tensors:
state_dataset.flush()
image_dataset.flush()
logger.info('Generated %d image datapoints' %
(state_id * num_images_per_state))
def run_kinect():
global image
global depth_im
global color_im
global toggle
global segmask
global policy
global image
global im1
global ax1
global best_center
global t
global best_angle
global prev_q_value
global prev_depth
global depth_img
global depth_raw
global state
global no_find
global z
global center
global q_value
global angle
global mean_sector
global no_valid_grasp_count
global color_to_depth_raw
global grip_success
global robot_collided
global no_valid_move_y
global r_image
global out_classes
global out_boxes
global r_class_names
global out_scores
if maskrcnn_run == 1:
global img
global result
global color_img_raw
print("_______________RUNKINECT_____________")
cv2.namedWindow('DEPTH_REAL', cv2.WINDOW_NORMAL)
cv2.resizeWindow('DEPTH_REAL', 1000, 1000)
cv2.namedWindow('COLOR_TO_DEPTH', cv2.WINDOW_NORMAL)
cv2.resizeWindow('COLOR_TO_DEPTH', 1000, 1000)
cv2.namedWindow('SEGMENTATION', cv2.WINDOW_NORMAL)
cv2.resizeWindow('SEGMENTATION', 1000, 1000)
prev_z = 0
best_angle = 0
class_color = {
'apple': (0, 0, 255),
'bae': (0, 135, 224),
'banana': (0, 255, 255),
'bread': (38, 201, 255),
'cake': (157, 229, 252),
'carrot': (0, 145, 255),
'cucumber': (0, 184, 58),
'gagi': (107, 0, 98),
'grape': (82, 0, 75),
'green_apple': (0, 255, 179),
'green_pepper': (0, 212, 149),
'hamburger': (138, 218, 255),
'kiwi': (85, 112, 95),
'lemon': (0, 255, 251),
'orange': (0, 200, 255),
'peach': (217, 230, 255),
'pepper': (0, 0, 255),
'pumkin': (30, 100, 186),
'tomato': (0, 0, 255)
}
max_image = 100
while 1:
try:
print("KINECT IS ON1")
depth_data = np.array(dataread(), dtype=np.uint8)
print("KINECT IS ON2")
depth_real_data = np.array(dataread_depth(), dtype=np.uint16)
depth_real_img = depth_real_data[96:512 - 96, 96:512 - 96] / 5000
# if max_image < depth_real_data.max():
# max_image = depth_real_data.max()
# print(max_image)
color_to_depth_data = np.array(dataread_color_to_depth(),
dtype=np.uint8)
color_to_depth_img = color_to_depth_data[96:512 - 96, 96:512 - 96,
0:3]
color_to_depth_raw = color_to_depth_img.copy()
if maskrcnn_run == 1:
color_img_raw = color_to_depth_raw.copy()
color_data = np.array(dataread_color(), dtype=np.uint8)
depth_img_ = depth_data.copy() / 65535
depth_raw = depth_data[:, :, 0]
depth_temp = depth_img_[:, :, 0]
depth_img = depth_temp.astype("float32")
print("KINECT IS ON3")
depth_img = (depth_raw) / 255
color_img = color_data[:, :, 0:3]
depth_im = DepthImage(depth_real_img.astype("float32"),
frame=camera_intr.frame)
color_im = ColorImage(np.zeros(
[depth_im.height, depth_im.width, 3]).astype(np.uint8),
frame=camera_intr.frame)
hsv = cv2.cvtColor(color_to_depth_img, cv2.COLOR_BGR2HSV)
lower_red = np.array([50, 50, 50])
upper_red = np.array([180, 180, 180])
mask = cv2.inRange(color_to_depth_img, lower_red, upper_red)
res = cv2.bitwise_and(color_to_depth_img,
color_to_depth_img,
mask=mask)
print(best_center)
print(best_angle)
mask2 = np.mean(color_to_depth_img, axis=2)
mask2[mask2 >= 50] = 255
mask2[mask2 < 50] = 0
mask2[mask2 == 255] = 1
mask2[mask2 == 0] = 255
mask2[mask2 == 1] = 0
mask2 = np.uint8(mask2)
mask[mask == 255] = 1
mask[mask == 0] = 255
mask[mask == 1] = 0
segmask_img = depth_real_img.astype("float32")
threshold = np.mean(segmask_img) * 0.8
#segmask_img[0:160,:] = segmask_img[0:160,:]*1.1
segmask_img[segmask_img < threshold] = 0
segmask_img[segmask_img > threshold] = 255
segmask_img[segmask_img == 0] = 254
segmask_img[segmask_img == 255] = 0
segmask_img[segmask_img == 254] = 255
#segmask_img[:130,:] = 0
#segmask_img[257:,:] = 0
#segmask_img[:,:37] = 0
#segmask_img[:,282:] = 0
segmask_img = np.uint8(segmask_img)
r = 40
res_mask = cv2.bitwise_or(segmask_img, mask)
res_mask = cv2.bitwise_or(res_mask, mask2)
#res_mask[:52,:] = 0
#res_mask[res_mask>100] = 255
#res_mask[res_mask<=100] = 0
#res_mask[0:73,:] = 0
#res_mask[242:,:] = 0
#res_mask[:,0:27] = 0
#res_mask[:,282:] = 0
segmask = BinaryImage(res_mask)
depth_colormap = cv2.applyColorMap(
cv2.convertScaleAbs(depth_real_img * 150, alpha=2), 1)
#print("MEAN IMAGE")
try:
sector = color_to_depth_img.get()
mean_sector = np.mean(sector[265:270, 150:180])
#print(mean_sector)
except:
sector = color_to_depth_img
mean_sector = np.mean(sector[265:270, 150:180])
#print(mean_sector)
pass
#ddddddfdfdf= np.array(color_img_raw.get())
# mean_img = np.array(color_img_raw[256:276,143:180],dtype=float)
try:
r = 80
color_to_depth_img = cv2.line(
color_to_depth_img,
(int(best_center[0] - 96 -
r * math.cos(best_angle / 180 * math.pi)),
int(best_center[1] - 96 -
r * math.sin(best_angle / 180 * math.pi))),
(int(best_center[0] - 96 +
r * math.cos(best_angle / 180 * math.pi)),
int(best_center[1] - 96 +
r * math.sin(best_angle / 180 * math.pi))),
(0, 0, 255), 2)
color_to_depth_img = cv2.line(
color_to_depth_img,
(int(best_center[0] - 96), int(best_center[1] - 96)),
(int(best_center[0] - 96 + 1), int(best_center[1] - 96)),
(0, 136, 255), 3)
except:
pass
cv2.imshow('COLOR_TO_DEPTH', color_to_depth_img)
cv2.imshow('DEPTH_REAL', np.uint8(depth_colormap))
cv2.imshow('SEGMENTATION', np.uint8(res_mask))
cv2.waitKey(1)
time.sleep(0.01)
except:
print("KINECT_EXEPTION")
time.sleep(0.01)