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)