def load_step_data(path, seq, step, real_data=False): if not real_data: depth_image = np.load('{}/{}_{}_depth.npy'.format(path, seq, step))[0] color_image = cv2.imread('{}/{}_{}_color_image.png'.format( path, seq, step)) cam_intr = np.load('./data/ycb_data/camera_intr.npy') cam_pose = np.load('./data/ycb_data/camera_pose.npy') else: data_path = osp.join(path, 'data_{}_{}.pkl'.format(seq, step)) # data_path = osp.join('./real_data/real_data/seq{}'.format(seq), 'data_{}.pkl'.format(step)) with open(data_path, 'rb') as f: data = pickle.load(f) if 'depth_init' in data.keys(): depth_scale = np.loadtxt( './real_data/real_data_new/camera_depth_scale.txt') cam_intr = np.load('./real_data/real_data_new/camera_intr.npy') cam_pose = np.loadtxt( './real_data/real_data_new/camera_pose.txt') depth_image = data['depth_init'] * depth_scale color_image = data['color_init'] cam_pts, rgb_pts = get_pointcloud(color_image, depth_image, cam_intr) cam_pts = np.transpose( np.dot(cam_pose[0:3, 0:3], np.transpose(cam_pts)) + np.tile(cam_pose[0:3, 3:], (1, cam_pts.shape[0]))) else: print(data.keys()) cam_intr_0 = np.load(osp.join(path, 'camera_intr.npy')) cam_intr_1 = np.load(osp.join(path, 'camera_intr_1.npy')) cam_pose_0 = np.loadtxt('./data/camera_pose.txt') cam_pose_1 = np.loadtxt('./data/camera_pose2.txt') depth_image_0 = data['depth_init_0'] * 9.919921874999999556e-01 color_image_0 = data['color_init_0'] cam_pts_0, rgb_pts_0 = get_pointcloud(color_image_0, depth_image_0, cam_intr_0) cam_pts_0 = np.transpose( np.dot(cam_pose_0[0:3, 0:3], np.transpose(cam_pts_0)) + np.tile(cam_pose_0[0:3, 3:], (1, cam_pts_0.shape[0]))) depth_image_1 = data['depth_init_1'] * 9.865234375000000444e-01 color_image_1 = data['color_init_1'] cam_pts_1, rgb_pts_1 = get_pointcloud(color_image_1, depth_image_1, cam_intr_1) cam_pts_1 = np.transpose( np.dot(cam_pose_1[0:3, 0:3], np.transpose(cam_pts_1)) + np.tile(cam_pose_1[0:3, 3:], (1, cam_pts_1.shape[0]))) return cam_pts_0, rgb_pts_0 return cam_pts, rgb_pts
def __getitem__(self, index): # gather tokens and samples needed for data extraction tokens = self.token[index] if len(tokens.split('_')) < 2: print(tokens) im_token = tokens.split('_')[0] annotation_token = tokens.split('_')[1] sample_data = self.nusc.get('sample_data', im_token) image_name = sample_data['filename'] sample = self.nusc.get('sample', sample_data['sample_token']) lidar_token = sample['data']['LIDAR_TOP'] # get the sample_data for the image batch #image_path = '/data/sets/nuscenes/' + image_name img = imread('/home/fengjia/data/sets/nuscenes/' + image_name) im = np.array(img) # get ground truth boxes _, boxes, camera_intrinsic = self.nusc.get_sample_data(im_token, box_vis_level=BoxVisibility.ALL) for box in boxes: corners = view_points(box.corners(), view=camera_intrinsic, normalize=True) if box.token == annotation_token: # Find the crop area of the box width = corners[0].max() - corners[0].min() height = corners[1].max() - corners[1].min() x_mid = (corners[0].max() + corners[0].min())/2 y_mid = (corners[1].max() + corners[1].min())/2 side = max(width, height)*random.uniform(1.0,1.2) if (x_mid - side/2) < 0: side = x_mid*2 if (y_mid - side/2) < 0: side = y_mid*2 # Crop the image bottom_left = [int(x_mid - side/2), int(y_mid - side/2)] top_right = [int(x_mid + side/2), int(y_mid + side/2)] corners[0]=corners[0] - bottom_left[0] corners[1]=corners[1] - bottom_left[1] crop_img = im[bottom_left[1]:top_right[1],bottom_left[0]:top_right[0]] # Scale to same size scale = 128/ side scaled = cv2.resize(crop_img, (128, 128)) crop_img = np.transpose(scaled, (2,0,1)) crop_img = crop_img.astype(np.float32) crop_img /= 255 # Get corresponding point cloud for the crop pcl, m, offset, camera_intrinsic, box_corners = get_pointcloud(self.nusc, bottom_left, top_right, box, lidar_token, im_token) break pcl = pcl.astype(np.float32) box_corners = box_corners.astype(np.float32) return crop_img, pcl, offset, m, camera_intrinsic, box_corners
def get_scene_heightmap(self, color_image, depth_image, segmentation_mask, cam_pose): camera_points, color_points, segmentation_points = utils.get_pointcloud( color_image, depth_image, segmentation_mask, self._scene_cam_intrinsics) camera_points = utils.transform_pointcloud(camera_points, cam_pose) color_heightmap, depth_heightmap, segmentation_heightmap = utils.get_heightmap( camera_points, color_points, segmentation_points, self._view_bounds, self._heightmap_pix_size, zero_level=self._view_bounds[2, 0]) self._depth_heightmap = depth_heightmap return color_heightmap, depth_heightmap, segmentation_heightmap
def simulation_experiment_challenging(): cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) object_inventory = [ 'imported_part_0', 'imported_part_2', 'imported_part_3', 'imported_part_6', 'imported_part_7', 'imported_part_8' ] cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) root = 'C:/Users/louxi/Desktop/icra2021/zb_2/' p = Perception() # perception module p g = Manipulation() positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15], [0, 0.20, 0.15]] env_cloud = np.load(root + 'env.npy') if cid != -1: i = 0 planning = 0 vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking) panda = Robot(cid) for i in range(10): add_object(cid, 'imported_part_' + str(i), random.choice(positions)) while i < 51: print('experiment number: ', i) rgb_raw, depth_raw = panda.get_rgbd_image() # Perception m_list = p.segmentation(rgb_raw, depth_raw) # segment scene target_object = random.choice(object_inventory) print(target_object) anchor = np.asarray( Image.open( os.path.join( 'C:/Users\louxi\Desktop\icra2021\siamese_data', target_object, 'rgb_image_' + str(random.randint(0, 499)) + '.png'))) anchor = center_crop_150(anchor) object_mask = p.classification(m_list, rgb_raw, anchor) # load anchor here # object_depth[np.where(object_mask < 0.5)] = 0 rgb_raw[np.where(object_mask < 0.5)] = 0 object_depth = np.copy(depth_raw) object_depth[np.where(object_mask < 0.5)] = 0 object_cloud = get_pointcloud(object_depth, cam_intrinsics, panda.depth_m) ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10] segmented_cloud = np.array([ pts for pts in object_cloud if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3] and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4] ]) if len(segmented_cloud) == 0: continue # pptk.viewer(segmented_cloud) # # fig = plt.figure(0) # sub_1 = fig.add_subplot(1, 2, 1) # sub_1.imshow(anchor) # sub_2 = fig.add_subplot(1, 2, 2) # sub_2.imshow(rgb_raw) # plt.show() initial_poses, dummy_points = grasp_pose_generation( 90, segmented_cloud, 200) # collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses) landing_mtx = g.grasping(segmented_cloud, np.average(segmented_cloud, axis=0), initial_poses) ending_mtx = np.copy(landing_mtx) ending_mtx[-1] = ending_mtx[-1] + 0.15 emptyBuff = bytearray() res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing0', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing1', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing2', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing3', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing4', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing5', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) input('s') vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(0.5) vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal( cid, "finish", vrep.simx_opmode_blocking) if finish == 18: res, result = vrep.simxGetIntegerSignal( cid, "collision", vrep.simx_opmode_blocking) if result == 1: planning = planning + 1 print('result is %d. start next experiment' % result) break i = i + 1 # vrep.simxStopSimulation(cid, operationMode=vrep.simx_opmode_blocking) time.sleep(3.0) # input('s') print(planning)
def simulation_experiment_standard(): cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) object_inventory = [ 'imported_part_0', 'imported_part_5', 'imported_part_2', 'imported_part_3', 'imported_part_6', 'imported_part_9' ] cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) root = 'C:/Users/louxi/Desktop/icra2021/zb_2/' p = Perception() # perception module p g = Manipulation() # positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15], [0, 0.20, 0.15]] env_cloud = np.load(root + 'env.npy') emptyBuff = bytearray() if cid != -1: i = 0 planning = 0 vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking) panda = Robot(cid) # for i in range(10): # add_object(cid, 'imported_part_' + str(i), random.choice(positions)) # for i in range(10): # add_object(cid, 'imported_part_' + str(i), [random.uniform(-0.10, 0.10), random.uniform(-0.10, 0.10), 0.15]) while True: # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing0', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_0.npy', np.array(retFloats)) # time.sleep(1) # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing1', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_1.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing2', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_2.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing3', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_3.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing4', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_4.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing5', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_5.npy', np.array(retFloats)) # # time.sleep(1) # # input('s') # objects_nb = input('number') objects_nb = '0' rgb_raw, depth_raw = panda.get_rgbd_image() # im = Image.fromarray(rgb_raw) # im.save('imported_part_5.png') # input('save?') # Perception m_list = p.segmentation(rgb_raw, depth_raw) # segment scene anchor = np.asarray( Image.open( 'C:/Users\louxi\Desktop\icra2021\siamese_data\inventory/' + object_inventory[int(objects_nb)] + '.png')) anchor = center_crop_150(anchor) object_mask = p.classification(m_list, rgb_raw, anchor) # load anchor here # object_depth[np.where(object_mask < 0.5)] = 0 rgb_raw[np.where(object_mask < 0.5)] = 0 # plt.imshow(rgb_raw) # plt.show() object_depth = np.copy(depth_raw) object_depth[np.where(object_mask < 0.5)] = 0 object_cloud = get_pointcloud(object_depth, cam_intrinsics, panda.depth_m) ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10] segmented_cloud = np.array([ pts for pts in object_cloud if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3] and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4] ]) # pptk.viewer(segmented_cloud) recognized = 'yes' if recognized == 'yes': initial_poses, dummy_points = grasp_pose_generation( 60, segmented_cloud, 200) # collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses) # print(len(collision_free_poses)) pt = np.average(segmented_cloud, axis=0) print('landing' + str(i)) pose = np.load('pose_' + str(i) + '.npy') i = i + 1 # landing_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0], # pose[1][0], pose[1][1], pose[1][2], pt[1], # pose[2][0], pose[2][1], pose[2][2], pt[2]] landing_mtx = pose print(landing_mtx) # vis_grasp(pose, pt, segmented_cloud) ending_mtx = np.copy(landing_mtx) ending_mtx[-1] = ending_mtx[-1] + 0.15 # recognized = input('correct?') if recognized == 'yes': vrep.simxCallScriptFunction( cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) vrep.simxCallScriptFunction( cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(0.5) vrep.simxCallScriptFunction( cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal( cid, "finish", vrep.simx_opmode_blocking) if finish == 18: res, result = vrep.simxGetIntegerSignal( cid, "collision", vrep.simx_opmode_blocking) if result == 1: planning = planning + 1 print('result is %d. start next experiment' % result) break time.sleep(3.0) else: continue
def get_height_img(self, image_size=(240, 240)): ret, color_img_left, depth_img_left = self.get_VS_image( self.VS_left_handle) if ret == 0: surface_pts_left, rgb_pts_left = utils.get_pointcloud( color_img_left, depth_img_left, self.left_cam_intrinsics, self.leftmat) else: return 8, 0, 0 ret, color_img_right, depth_img_right = self.get_VS_image( self.VS_right_handle) if ret == 0: surface_pts_right, rgb_pts_right = utils.get_pointcloud( color_img_right, depth_img_right, self.right_cam_intrinsics, self.rightmat) else: return 8, 0, 0 surface_pts = np.concatenate((surface_pts_left, surface_pts_right), axis=0) color_pts = np.concatenate((rgb_pts_left, rgb_pts_right), axis=0) #surface_pts=surface_pts_left #color_pts=rgb_pts_left #surface_pts = surface_pts_right #color_pts = rgb_pts_right heightmap_size = np.round(image_size).astype(int) # Filter out surface points outside heightmap boundaries heightmap_valid_ind = np.logical_and( np.logical_and( np.logical_and( surface_pts[:, 0] >= self.workspace_limits[0][0], surface_pts[:, 0] < self.workspace_limits[0][1]), surface_pts[:, 1] >= self.workspace_limits[1][0]), surface_pts[:, 1] < self.workspace_limits[1][1]) surface_pts = surface_pts[heightmap_valid_ind] color_pts = color_pts[heightmap_valid_ind] color_heightmap_r = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) color_heightmap_g = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) color_heightmap_b = np.zeros((heightmap_size[0], heightmap_size[1], 1), dtype=np.uint8) depth_heightmap = np.zeros(heightmap_size) heightmap_pix_x = np.floor( (surface_pts[:, 0] - self.workspace_limits[0][0]) / (self.workspace_limits[0][1] - self.workspace_limits[0][0]) * heightmap_size[0]).astype(int) heightmap_pix_y = np.floor( (surface_pts[:, 1] - self.workspace_limits[1][0]) / (self.workspace_limits[1][1] - self.workspace_limits[1][0]) * heightmap_size[1]).astype(int) color_heightmap_r[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [0]] color_heightmap_g[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [1]] color_heightmap_b[heightmap_pix_y, heightmap_pix_x] = color_pts[:, [2]] color_heightmap = np.concatenate( (color_heightmap_r, color_heightmap_g, color_heightmap_b), axis=2) depth_heightmap[heightmap_pix_y, heightmap_pix_x] = surface_pts[:, 2] depth_heightmap[depth_heightmap < 0] = 0 return 0, color_heightmap, depth_heightmap
top_right = [np.int(corners[0].max()), np.int(corners[1].max())] if box.name.split('.')[0] == 'vehicle': if box.name.split('.')[1] != 'emergency': name = box.name.split('.')[1] else: name = '' elif box.name.split('.')[0] == 'human': name = 'pedestrian' elif box.name.split('.')[0] == 'movable_object': if box.name.split('.')[1] != 'debris' and box.name.split('.')[1] != 'pushable_pullable': name = box.name.split('.')[1] else: name = '' else: name = '' pcl, _, _, _, _ = get_pointcloud(nusc, bottom_left, top_right, box, lidar_token, im_token) # print('pcl shape ', pcl.shape) # print(np.shape(pcl)[1]) # if len(pcl) != 0 and np.shape(pcl)[1] == 400: if len(pcl) != 0 and np.shape(pcl)[1] == 20: annotation_token = box.token annotations = annotations + [im_token + "_" + annotation_token] counter = counter + 1 # if (name == 'car' or name == 'pedestrian') and len(pcl) != 0: # if np.shape(pcl)[1] == 400: # # o3d.visualization.draw_geometries([pcl]) # # v = pptk.viewer(pcl) # annotation_token = box.token # annotations = annotations + [im_token + "_" + annotation_token] # counter = counter + 1 print(counter)