def __init__(
self,
root_dir,
item,
num_points=1024,
objs=[],
add_noise=False,
noise_trans=0,
mode="train",
refine=False,
selected_list=None,
is_debug=False,
):
"""
num is the number of points chosen feeding into PointNet
"""
self.is_debug = is_debug
self.mode = mode
self.max_lnk = 10
self.root_dir = root_dir
self.dataset_render = root_dir + "/render"
# models_dir no use
self.models_dir = root_dir + "/objects"
self.objnamelist = os.listdir(self.dataset_render)
self.mode = mode
self.list_rgb = []
self.list_depth = []
self.list_label = []
self.list_obj = []
self.list_instance = []
# obj/instance/arti
self.list_status = []
self.list_rank = []
self.meta_dict = {}
self.urdf_dict = {}
self.pt_dict = {}
self.noise_trans = noise_trans
self.refine = refine
ins_count = 0
if self.is_debug:
from mpl_toolkits.mplot3d import Axes3D
obj_category = item
instances_per_obj = os.listdir(self.dataset_render + "/" +
obj_category)
instances_per_obj.sort()
meta_dict_obj = {}
urdf_dict_obj = {}
for ins in instances_per_obj:
# pdb.set_trace()
if selected_list is not None and ins not in selected_list:
continue
base_path = self.dataset_render + "/" + obj_category + "/" + ins
print(base_path)
meta = {}
urdf_ins = {} # link, joint
# ********* add pts ************ #
for art_index in os.listdir(base_path):
sub_dir0 = base_path + "/" + art_index
input_file = open(sub_dir0 + "/all.txt")
while 1:
ins_count += 1
input_line = input_file.readline()
if not input_line:
break
if input_line[-1:] == "\n":
input_line = input_line[:-1]
self.list_rgb.append(sub_dir0 +
"/rgb/{}.png".format(input_line))
self.list_depth.append(sub_dir0 +
"/depth/{}.h5".format(input_line))
self.list_label.append(sub_dir0 +
"/mask/{}.png".format(input_line))
self.list_obj.append(obj_category)
self.list_instance.append(ins)
self.list_status.append(art_index)
self.list_rank.append(int(input_line))
try:
meta_file = open(sub_dir0 + "/gt.yml", "r")
meta_instance = yaml.load(meta_file)
meta[art_index] = meta_instance
except:
meta[art_index] = None
# Deal with different dataset in this part to parse the URDF
urdf_path = self.root_dir + "/urdf/" + obj_category + "/" + ins
if args.dataset == "sapien":
urdf_ins = get_urdf_mobility(urdf_path, False)
else:
urdf_ins = get_urdf(urdf_path)
# root_urdf = tree_urdf.getroot()
# rpy_xyz = {}
# list_xyz = [None] * self.max_lnk
# list_rpy = [None] * self.max_lnk
# list_box = [None] * self.max_lnk
# # ['obj'] ['link/joint']['xyz/rpy'] [0, 1, 2, 3, 4]
# num_links = 0
# for link in root_urdf.iter('link'):
# num_links += 1
# index_link = None
# if link.attrib['name']=='base_link':
# index_link = 0
# else:
# index_link = int(link.attrib['name'])
# for visual in link.iter('visual'):
# for origin in visual.iter('origin'):
# list_xyz[index_link] = [float(x) for x in origin.attrib['xyz'].split()]
# list_rpy[index_link] = [float(x) for x in origin.attrib['rpy'].split()]
# rpy_xyz['xyz'] = list_xyz
# rpy_xyz['rpy'] = list_rpy
# # rpy_xyz['box'] = list_box
# urdf_ins['link'] = rpy_xyz
# rpy_xyz = {}
# list_xyz = [None] * self.max_lnk
# list_rpy = [None] * self.max_lnk
# list_axis = [None] * self.max_lnk
# # here we still have to read the URDF file
# for joint in root_urdf.iter('joint'):
# index_joint = int(joint.attrib['name'][0])
# for origin in joint.iter('origin'):
# list_xyz[index_joint] = [float(x) for x in origin.attrib['xyz'].split()]
# list_rpy[index_joint] = [float(x) for x in origin.attrib['rpy'].split()]
# for axis in joint.iter('axis'):
# list_axis[index_joint]= [float(x) for x in axis.attrib['xyz'].split()]
# rpy_xyz['xyz'] = list_xyz
# rpy_xyz['rpy'] = list_rpy
# rpy_xyz['axis'] = list_axis
# urdf_ins['joint'] = rpy_xyz
# urdf_ins['num_links']= num_links
meta_dict_obj[ins] = meta
urdf_dict_obj[ins] = urdf_ins
print("Object {} instance {} buffer loaded".format(
obj_category, ins))
self.meta_dict[obj_category] = meta_dict_obj
self.urdf_dict[obj_category] = urdf_dict_obj
self.length = len(self.list_rgb)
self.height = 512
self.width = 512
self.xmap = np.array([[j for i in range(512)] for j in range(512)])
self.ymap = np.array([[i for i in range(512)] for j in range(512)])
self.num = num_points
self.add_noise = add_noise
rt_acc.append(acc)
print(item[0:8], " ".join(["{:0.4f}".format(x) for x in rt_acc]))
print('\n')
# >>>>>>>>>>>>>>>>>>>>>>>>>> 3D IoU & boundary computation <<<<<<<<<<<<<<<<<<<<<<<<<<< #
with open(root_dset + "/pickle/{}.pkl".format(args.item), "rb") as f:
all_factors = pickle.load(f)
with open(root_dset + "/pickle/{}_corners.pkl".format(args.item),
"rb") as fc:
all_corners = pickle.load(fc)
bbox3d_all = {}
for instance in test_ins:
if args.item in ['drawer']:
target_order = infos.datasets[args.item].spec_map[instance]
path_urdf = group_dir + '/sapien/objects/' + '/' + args.item + '/' + instance
urdf_ins = get_urdf_mobility(path_urdf)
joint_rpy = urdf_ins['joint']['rpy'][target_order[0]]
rot_mat = euler_matrix(joint_rpy[0], joint_rpy[1],
joint_rpy[2])[:3, :3]
norm_factors = all_factors[instance]
norm_corners = all_corners[instance]
bbox3d_per_part = [None] * num_parts
for p in range(num_parts):
norm_factor = norm_factors[p + 1]
norm_corner = norm_corners[p + 1]
nocs_corner = np.copy(norm_corner)
nocs_corner[0] = np.array([0.5, 0.5, 0.5]).reshape(
1, 3) - 0.5 * (norm_corner[1] - norm_corner[0]) * norm_factor
nocs_corner[1] = np.array([0.5, 0.5, 0.5]).reshape(
1, 3) + 0.5 * (norm_corner[1] - norm_corner[0]) * norm_factor
print(item[0:8], " ".join(["{:0.4f}".format(x) for x in rt_acc]))
print('\n')
# >>>>>>>>>>>>>>>>>>>>>>>>>> 3D IoU & boundary computation <<<<<<<<<<<<<<<<<<<<<<<<<<< #
with open(root_dset + "/pickle/{}.pkl".format(args.item), "rb") as f:
all_factors = pickle.load(f)
with open(root_dset + "/pickle/{}_corners.pkl".format(args.item),
"rb") as fc:
all_corners = pickle.load(fc)
bbox3d_all = {}
for instance in test_ins:
if args.item in ['drawer']:
target_order = infos.datasets[args.item].spec_map[instance]
path_urdf = my_dir + '/dataset/sapien/urdf/' + args.item + '/' + instance
urdf_ins = get_urdf_mobility(path_urdf, False)
joint_rpy = urdf_ins['joint']['rpy'][target_order[0]]
rot_mat = euler_matrix(joint_rpy[0], joint_rpy[1],
joint_rpy[2])[:3, :3]
norm_factors = all_factors[instance]
norm_corners = all_corners[instance]
bbox3d_per_part = [None] * num_parts
for p in range(num_parts):
norm_factor = norm_factors[p + 1]
norm_corner = norm_corners[p + 1]
nocs_corner = np.copy(norm_corner)
nocs_corner[0] = np.array([0.5, 0.5, 0.5]).reshape(
1, 3) - 0.5 * (norm_corner[1] - norm_corner[0]) * norm_factor
nocs_corner[1] = np.array([0.5, 0.5, 0.5]).reshape(
1, 3) + 0.5 * (norm_corner[1] - norm_corner[0]) * norm_factor
def render_data(data_root,
name_obj,
cur_urdf,
args=None,
cam_dis=1,
_WRITE_FLAG=True,
_RENDER_FLAG=True,
_CREATE_FOLDER=True,
RENDER_NUM=100,
ARTIC_CNT=20,
_RENDER_MODE='random',
_USE_GUI=True,
_IS_DUBUG=True):
#>>>>>>>>>>>>>>>>>>>>>>>>>> internal config >>>>>>>>>>>>>>>>>>>>>>#
save_path = data_root + '/render/' + name_obj
path_urdf = data_root + '/urdf/' + name_obj
roll = 0
steeringAngle = 0
camPosX = 0
camPosY = 0
camPosZ = 0
upAxisIndex = 2 # align with z
camDistance = 0
pixelWidth = 512
pixelHeight = 512
nearPlane = 0.01
farPlane = 10
fov = 75
cameraUp = [0, 0.5 * np.sqrt(3), 0.5] # z axis
cameraPos = [-1.1, -1.1, 1.1]
#>>>>>>>>>>>>>>>>>>>>>>>>> internal config end >>>>>>>>>>>>>>>>>>>#
pitch_low, pitch_high = [int(x) for x in args.pitch.split(',')]
yaw_low, yaw_high = [int(x) for x in args.yaw.split(',')]
max_angles = [float(x) / 180 * np.pi for x in args.max_angles.split(',')]
min_angles = [float(x) / 180 * np.pi for x in args.min_angles.split(',')]
print('we are setting pitch and yaw to: ', pitch_low, pitch_high, yaw_low,
yaw_high)
if not _WRITE_FLAG:
camInfo = pybullet.getDebugVisualizerCamera()
# tree_urdf = ET.parse("{}/{}/syn.urdf".format(path_urdf, cur_urdf))
# tree_urdf = ET.parse("{}/{}/mobility.urdf".format(path_urdf, cur_urdf))
# root = tree_urdf.getroot()
num_joints = 0
num_joints = len(os.listdir("{}/{}/".format(path_urdf, cur_urdf))) - 4
obj_parts = []
pybullet.setGravity(0, 0, -10)
for i in range(num_joints + 1): #
urdf_file = "{}/{}/syn_p{}.urdf".format(path_urdf, cur_urdf, i)
print('loading ', urdf_file)
# obj_p = pybullet.createVisualShape(urdf_file)
obj_p = pybullet.loadURDF(urdf_file)
obj_parts.append(obj_p)
if i == 0:
for joint in range(pybullet.getNumJoints(obj_parts[i])):
print("joint[", joint, "]=",
pybullet.getJointInfo(obj_parts[i], joint))
pybullet.setJointMotorControl2(obj_parts[i],
joint,
pybullet.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
pybullet.getJointInfo(obj_parts[i], joint)
simu_cnt = 0
main_start = time.time()
urdf_ins = get_urdf_mobility("{}/{}".format(path_urdf, cur_urdf))
num_joints = len(urdf_ins['obj_name']) - 1
# instance-wise offset for camera distance
try:
model_pts, norm_factors, corner_pts = get_model_pts(
data_root, name_obj, cur_urdf, obj_file_list=urdf_ins['obj_name'])
center_pts = [(x[0] + x[1]) / 2 for x in corner_pts]
tight_bb = corner_pts[0][1] - corner_pts[0][
0] # the size of this objects
min_dis = np.linalg.norm(tight_bb) / 2 * np.tan(
fov / 180 / 2 * np.pi) # todo
offset = min_dis / 2
except:
offset = 0.5
if _RENDER_MODE == 'random':
steeringAngleArray = np.random.rand(ARTIC_CNT, num_joints) * np.array(
[max_angles]).reshape(-1, num_joints)
elif _RENDER_MODE == 'linear':
steeringAngleArray = np.tile(
np.linspace(0, 1, ARTIC_CNT).reshape(-1, 1),
(1, num_joints)) * (np.array([max_angles]).reshape(-1) - np.array(
[min_angles]).reshape(-1)) + np.array([min_angles]).reshape(-1)
rdn_offset = 2 * offset * (np.random.rand(ARTIC_CNT, RENDER_NUM) - 0.5
) # -0.4, 0.4
lightDirectionArray = 10 * np.random.rand(ARTIC_CNT, RENDER_NUM,
3) # coming direction of light
lightDistanceArray = 0.9 + 0.2 * np.random.rand(ARTIC_CNT, RENDER_NUM)
lightColorArray = 0.9 + 0.1 * np.random.rand(ARTIC_CNT, RENDER_NUM, 3)
lightSpecularCoeffArray = 0.85 + 0.1 * np.random.rand(
ARTIC_CNT, RENDER_NUM)
lightAmbientCoeffArray = 0.1 + 0.2 * np.random.rand(ARTIC_CNT, RENDER_NUM)
lightDiffuseCoeffArray = 0.85 + 0.1 * np.random.rand(ARTIC_CNT, RENDER_NUM)
# get joint state
while (simu_cnt < ARTIC_CNT):
if (not os.path.exists(save_path +
'/{}/{}/depth/'.format(cur_urdf, simu_cnt))
) and _CREATE_FOLDER:
os.makedirs(save_path + '/{}/{}/depth/'.format(cur_urdf, simu_cnt))
os.makedirs(save_path + '/{}/{}/rgb/'.format(cur_urdf, simu_cnt))
os.makedirs(save_path + '/{}/{}/mask/'.format(cur_urdf, simu_cnt))
yml_dict = OrderedDict()
yml_file = save_path + '/{}/{}/gt.yml'.format(cur_urdf, simu_cnt)
# set articulation status
print('Rendering with joint angles {}'.format(
steeringAngleArray[simu_cnt, :] * 180 / np.pi))
for steer in range(num_joints):
steeringAngle = steeringAngleArray[simu_cnt, steer]
for j in range(num_joints + 1): #
pybullet.setJointMotorControl2(obj_parts[j],
steer,
pybullet.POSITION_CONTROL,
targetVelocity=0,
targetPosition=steeringAngle)
start = time.time()
# time.sleep(0.2)
# make sure the angle matches with set value
lstate0 = pybullet.getLinkState(
obj_parts[0],
linkIndex=pybullet.getNumJoints(obj_parts[0]) - 1,
computeForwardKinematics=True)
for m in range(10):
time.sleep(0.1)
lstate1 = pybullet.getLinkState(
obj_parts[0],
linkIndex=pybullet.getNumJoints(obj_parts[0]) - 1,
computeForwardKinematics=True)
q0 = lstate0[5]
q1 = lstate1[5]
angle_diff = 2 * np.arccos(
min(1, np.sum(np.array(q0) * np.array(q1)))) / np.pi * 180
# print('angle difference is: ', angle_diff)
if angle_diff < 0.05:
break
lstate0 = lstate1
img_id = 0
lastTime = time.time()
# view_num = 100
# pitch_choices = pitch_low + (pitch_high - pitch_low) *np.random.rand(view_num)
# yaw_choices = yaw_low + (yaw_high - yaw_low) * np.random.rand(view_num)
for i in range(RENDER_NUM):
flag = True
while flag:
# pitch = pitch_choices[i]
# yaw = yaw_choices[i]
# Adjust codes to promise the mask contains all labels
pitch = pitch_low + (pitch_high -
pitch_low) * np.random.rand(1)[0]
yaw = yaw_low + (yaw_high - yaw_low) * np.random.rand(1)[0]
if (img_id < RENDER_NUM and _RENDER_FLAG):
camTargetPos = 0.8 * min_dis * (np.random.rand(3) -
0.5) + center_pts[0]
nowTime = time.time()
offset = rdn_offset[simu_cnt, img_id]
lightDirection = lightDirectionArray[simu_cnt, img_id, :]
lightDistance = lightDistanceArray[simu_cnt, img_id]
lightColor = list(lightColorArray[simu_cnt, img_id, :])
lightAmbientCoeff = lightAmbientCoeffArray[simu_cnt,
img_id]
lightDiffuseCoeff = lightDiffuseCoeffArray[simu_cnt,
img_id]
lightSpecularCoeff = lightSpecularCoeffArray[simu_cnt,
img_id]
camDistance_final = min_dis * 2.8 + offset # [1.6, 2.6] * min_dis
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(
camTargetPos[0], camDistance_final, yaw, pitch, roll,
upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix, lightDirection=lightDirection,\
renderer= pybullet.ER_BULLET_HARDWARE_OPENGL)
w = img_arr[0]
h = img_arr[1]
rgb = img_arr[2]
depth_raw = img_arr[3].astype(np.float32)
mask = img_arr[4]
depth = 255.0 * nearPlane / (
farPlane -
(farPlane - nearPlane) * depth_raw) # *farPlane/255.0
far = farPlane
near = nearPlane
depth_to_save = 2.0 * far * near / (far + near -
(far - near) *
(2 * depth_raw - 1.0))
np_rgb_arr = np.reshape(rgb, (h, w, 4))[:, :, :3]
# cv2 deal with BGR
temp = np_rgb_arr[:, :, 0].copy()
np_rgb_arr[:, :, 0] = np_rgb_arr[:, :, 2]
np_rgb_arr[:, :, 2] = temp
np_depth_arr = np.reshape(depth, (h, w, 1))
np_mask_arr = (np.reshape(mask,
(h, w, 1))).astype(np.uint8)
check_mask = True
num_parts = num_joints + 1
# breakpoint()
for c in range(1, num_parts):
if np.sum(np_mask_arr == c) < 10:
check_mask = False
break
if check_mask == True:
flag = False
image_path = save_path + '/{}/{}'.format(
cur_urdf, simu_cnt)
rgb_name = image_path + '/rgb/{0:06d}.png'.format(
img_id)
depth_img_name = image_path + '/depth/{0:06d}.png'.format(
img_id)
depth_name = image_path + '/depth/{0:06d}.h5'.format(
img_id)
mask_name = image_path + '/mask/{0:06d}.png'.format(
img_id)
if i == 0:
joint_pos = OrderedDict()
for joint in range(
pybullet.getNumJoints(obj_parts[0])):
lstate = pybullet.getLinkState(
obj_parts[0],
linkIndex=joint,
computeForwardKinematics=True)
joint_pos[joint] = OrderedDict([
(0, list(lstate[0])), (1, list(lstate[1])),
(2, list(lstate[2])), (3, list(lstate[3])),
(4, list(lstate[4])), (5, list(lstate[5]))
])
# print('Joint {} lstate under {} : \n'.format(joint, steeringAngleArray[simu_cnt, :]), lstate[4:6])
# breakpoint()
if _WRITE_FLAG is True:
cv2.imwrite(rgb_name, np_rgb_arr)
cv2.imwrite(depth_img_name, np_depth_arr)
cv2.imwrite(mask_name, np_mask_arr)
hf = h5py.File(depth_name, 'w')
hf.create_dataset('data', data=depth_to_save)
yml_dict['frame_{}'.format(img_id)] = OrderedDict([
('obj', joint_pos), ('viewMat', list(viewMatrix)),
('projMat', list(projectionMatrix))
])
if not _WRITE_FLAG:
time.sleep(1)
img_id += 1
lastTime = nowTime
if _WRITE_FLAG:
with open(yml_file, 'w') as f:
yaml.dump(yml_dict, f, default_flow_style=False)
simu_cnt += 1
stop = time.time()
main_stop = time.time()
print("Total time %f" % (main_stop - main_start))
pybullet.resetSimulation()