def set_scene(self, config, state=None):
if self.render_mode == 'particle':
render_mode = 1
else:
render_mode = 2
camera_params = config['camera_params'][config['camera_name']]
params = np.array([
*config['init_pos'], config['stretchstiffness'],
config['bendingstiffness'], config['radius'], config['segment'],
config['mass'], config['scale'], *camera_params['pos'][:],
*camera_params['angle'][:], camera_params['width'],
camera_params['height'], render_mode
])
env_idx = 2
if self.version == 2:
robot_params = [1.] if self.action_mode in ['sawyer', 'franka'
] else []
self.params = (params, robot_params)
pyflex.set_scene(env_idx, params, 0, robot_params)
elif self.version == 1:
pyflex.set_scene(env_idx, params, 0)
num_particles = pyflex.get_n_particles()
# print("with {} segments, the number of particles are {}".format(config['segment'], num_particles))
# exit()
self.update_camera(config['camera_name'],
config['camera_params'][config['camera_name']])
if state is not None:
self.set_state(state)
self.current_config = deepcopy(config)
def set_scene(self, config, states=None):
'''
child envs can pass in specific fluid params through fluid param dic.
'''
# sample fluid properties.
fluid_params = self.sample_fluid_params(config['fluid'])
# set camera parameters.
self.initialize_camera()
camera_name = config.get('camera_name', self.camera_name)
camera_params = np.array([
*self.camera_params[camera_name]['pos'],
*self.camera_params[camera_name]['angle'], self.camera_width,
self.camera_height, self.render_mode
])
# create fluid
scene_params = np.concatenate((fluid_params, camera_params))
env_idx = 1
if self.version == 2:
robot_params = []
self.params = (scene_params, robot_params)
pyflex.set_scene(env_idx, scene_params, 0, robot_params)
elif self.version == 1:
pyflex.set_scene(env_idx, scene_params, 0)
self.particle_num = pyflex.get_n_particles()
def reset(self, config=None, initial_state=None, config_id=None):
if config is None:
if config_id is None:
if self.eval_flag:
eval_beg = int(0.8 * len(self.cached_configs))
config_id = np.random.randint(
low=eval_beg, high=len(self.cached_configs)
) if not self.deterministic else eval_beg
else:
train_high = int(0.8 * len(self.cached_configs))
config_id = np.random.randint(
low=0, high=max(train_high,
1)) if not self.deterministic else 0
self.current_config = self.cached_configs[config_id]
self.current_config_id = config_id
self.set_scene(self.cached_configs[config_id],
self.cached_init_states[config_id])
else:
self.current_config = config
self.set_scene(config, initial_state)
self.particle_num = pyflex.get_n_particles()
self.prev_reward = 0.
self.time_step = 0
obs = self._reset()
if self.recording:
self.video_frames.append(self.render(mode='rgb_array'))
return obs
def gen_PyFleX(info):
env, env_idx = info['env'], info['env_idx']
thread_idx, data_dir, data_names = info['thread_idx'], info['data_dir'], info['data_names']
n_rollout, time_step = info['n_rollout'], info['time_step']
shape_state_dim, dt = info['shape_state_dim'], info['dt']
gen_vision = info['gen_vision']
vision_dir, vis_width, vis_height = info['vision_dir'], info['vis_width'], info['vis_height']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2 ** 32)
# positions
stats = [init_stat(3)]
import pyflex
pyflex.init()
for i in range(n_rollout):
if i % 10 == 0:
print("%d / %d" % (i, n_rollout))
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
if env == 'RigidFall':
g_low, g_high = info['physics_param_range']
gravity = rand_float(g_low, g_high)
print("Generated RigidFall rollout {} with gravity {} from range {} ~ {}".format(
i, gravity, g_low, g_high))
n_instance = 3
draw_mesh = 1
scene_params = np.zeros(n_instance * 3 + 3)
scene_params[0] = n_instance
scene_params[1] = gravity
scene_params[-1] = draw_mesh
low_bound = 0.09
for j in range(n_instance):
x = rand_float(0., 0.1)
y = rand_float(low_bound, low_bound + 0.01)
z = rand_float(0., 0.1)
scene_params[j * 3 + 2] = x
scene_params[j * 3 + 3] = y
scene_params[j * 3 + 4] = z
low_bound += 0.21
pyflex.set_scene(env_idx, scene_params, thread_idx)
pyflex.set_camPos(np.array([0.2, 0.875, 2.0]))
n_particles = pyflex.get_n_particles()
n_shapes = 1 # the floor
positions = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
for j in range(time_step):
positions[j, :n_particles] = pyflex.get_positions().reshape(-1, 4)[:, :3]
ref_positions = positions[0]
for k in range(n_instance):
XX = ref_positions[64*k:64*(k+1)]
YY = positions[j, 64*k:64*(k+1)]
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
# print("MSE fit", np.mean(np.square(YY_fitted - YY)))
positions[j, 64*k:64*(k+1)] = YY_fitted
if gen_vision:
pyflex.step(capture=True, path=os.path.join(rollout_dir, str(j) + '.tga'))
else:
pyflex.step()
data = [positions[j], shape_quats[j], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
if gen_vision:
images = np.zeros((time_step, vis_height, vis_width, 3), dtype=np.uint8)
for j in range(time_step):
img_path = os.path.join(rollout_dir, str(j) + '.tga')
img = scipy.misc.imread(img_path)[:, :, :3][:, :, ::-1]
img = cv2.resize(img, (vis_width, vis_height), interpolation=cv2.INTER_AREA)
images[j] = img
os.system('rm ' + img_path)
store_data(['positions', 'images', 'scene_params'], [positions, images, scene_params],
os.path.join(vision_dir, str(rollout_idx) + '.h5'))
elif env == 'MassRope':
s_low, s_high = info['physics_param_range']
stiffness = rand_float(s_low, s_high)
print("Generated MassRope rollout {} with gravity {} from range {} ~ {}".format(
i, stiffness, s_low, s_high))
x = 0.
y = 1.0
z = 0.
length = 0.7
draw_mesh = 1.
scene_params = np.array([x, y, z, length, stiffness, draw_mesh])
pyflex.set_scene(env_idx, scene_params, 0)
pyflex.set_camPos(np.array([0.13, 2.0, 3.2]))
action = np.zeros(3)
# the last particle is the pin, regarded as shape
n_particles = pyflex.get_n_particles() - 1
n_shapes = 1 # the mass at the top of the rope
positions = np.zeros((time_step + 1, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step + 1, n_shapes, 4), dtype=np.float32)
action = np.zeros(3)
for j in range(time_step + 1):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
if j >= 1:
# append the action (position of the pin) to the previous time step
positions[j - 1, -1, :] = positions[j, -1, :]
ref_positions = positions[0]
# apply rigid projection to the rigid object
# cube: [0, 81)
# rope: [81, 95)
# pin: [95, 96)
XX = ref_positions[:81]
YY = positions[j, :81]
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
positions[j, :81] = YY_fitted
scale = 0.1
action[0] += rand_float(-scale, scale) - positions[j, -1, 0] * 0.1
action[2] += rand_float(-scale, scale) - positions[j, -1, 2] * 0.1
if gen_vision:
pyflex.step(action * dt, capture=True, path=os.path.join(rollout_dir, str(j) + '.tga'))
else:
pyflex.step(action * dt)
if j >= 1:
data = [positions[j - 1], shape_quats[j - 1], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j - 1) + '.h5'))
if gen_vision:
images = np.zeros((time_step, vis_height, vis_width, 3), dtype=np.uint8)
for j in range(time_step):
img_path = os.path.join(rollout_dir, str(j) + '.tga')
img = scipy.misc.imread(img_path)[:, :, :3][:, :, ::-1]
img = cv2.resize(img, (vis_width, vis_height), interpolation=cv2.INTER_AREA)
images[j] = img
os.system('rm ' + img_path)
store_data(['positions', 'images', 'scene_params'], [positions, images, scene_params],
os.path.join(vision_dir, str(rollout_idx) + '.h5'))
else:
raise AssertionError("Unsupported env")
# change dtype for more accurate stat calculation
# only normalize positions
datas = [positions[:time_step].astype(np.float64)]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0] * datas[j].shape[1]
stats[j] = combine_stat(stats[j], stat)
pyflex.clean()
return stats
boxes = calc_box_init(box_dis_x, box_dis_z)
for i in range(len(boxes)):
halfEdge = boxes[i][0]
center = boxes[i][1]
quat = boxes[i][2]
pyflex.add_box(halfEdge, center, quat)
### read scene info
print("Scene Upper:", pyflex.get_scene_upper())
print("Scene Lower:", pyflex.get_scene_lower())
print("Num particles:", pyflex.get_phases().reshape(-1, 1).shape[0])
print("Phases:", np.unique(pyflex.get_phases()))
n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
n_rigids = pyflex.get_n_rigids()
n_rigidPositions = pyflex.get_n_rigidPositions()
print("n_particles", n_particles)
print("n_shapes", n_shapes)
print("n_rigids", n_rigids)
print("n_rigidPositions", n_rigidPositions)
positions = np.zeros((time_step, n_particles, dim_position))
velocities = np.zeros((time_step, n_particles, dim_velocity))
shape_states = np.zeros((time_step, n_shapes, dim_shape_state))
x_box = x_center
v_box = 0
def gen_PyFleX(info):
env, root_num = info['env'], info['root_num']
thread_idx, data_dir, data_names = info['thread_idx'], info['data_dir'], info['data_names']
n_rollout, n_instance = info['n_rollout'], info['n_instance']
time_step, time_step_clip = info['time_step'], info['time_step_clip']
shape_state_dim, dt = info['shape_state_dim'], info['dt']
env_idx = info['env_idx']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32) ### NOTE: we might want to fix the seed for reproduction
# positions, velocities
if env_idx == 5: # RiceGrip
stats = [init_stat(6), init_stat(6)]
else:
stats = [init_stat(3), init_stat(3)]
import pyflex
pyflex.init()
for i in range(n_rollout):
if i % 10 == 0:
print("%d / %d" % (i, n_rollout))
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
if env == 'FluidFall':
scene_params = np.zeros(1)
pyflex.set_scene(env_idx, scene_params, thread_idx)
n_particles = pyflex.get_n_particles()
positions = np.zeros((time_step, n_particles, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles, 3), dtype=np.float32)
for j in range(time_step_clip):
p_clip = pyflex.get_positions().reshape(-1, 4)[:, :3]
pyflex.step()
for j in range(time_step):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
if j == 0:
velocities[j] = (positions[j] - p_clip) / dt
else:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j]]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'BoxBath':
# BoxBath
scene_params = np.zeros(1)
pyflex.set_scene(env_idx, scene_params, thread_idx)
n_particles = pyflex.get_n_particles()
positions = np.zeros((time_step, n_particles, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles, 3), dtype=np.float32)
for j in range(time_step_clip):
pyflex.step()
p = pyflex.get_positions().reshape(-1, 4)[:64, :3]
clusters = []
st_time = time.time()
kmeans = MiniBatchKMeans(n_clusters=root_num[0][0], random_state=0).fit(p)
# print('Time on kmeans', time.time() - st_time)
clusters.append([[kmeans.labels_]])
# centers = kmeans.cluster_centers_
ref_rigid = p
for j in range(time_step):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
# apply rigid projection to ground truth
XX = ref_rigid
YY = positions[j, :64]
# print("MSE init", np.mean(np.square(XX - YY)))
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
# print("MSE fit", np.mean(np.square(YY_fitted - YY)))
positions[j, :64] = YY_fitted
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j], clusters]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'FluidShake':
# if env is FluidShake
height = 1.0
border = 0.025
dim_x = rand_int(10, 12)
dim_y = rand_int(15, 20)
dim_z = 3
x_center = rand_float(-0.2, 0.2)
x = x_center - (dim_x-1)/2.*0.055
y = 0.055/2. + border + 0.01
z = 0. - (dim_z-1)/2.*0.055
box_dis_x = dim_x * 0.055 + rand_float(0., 0.3)
box_dis_z = 0.2
scene_params = np.array([x, y, z, dim_x, dim_y, dim_z, box_dis_x, box_dis_z])
pyflex.set_scene(env_idx, scene_params, 0)
boxes = calc_box_init_FluidShake(box_dis_x, box_dis_z, height, border)
for i in range(len(boxes)):
halfEdge = boxes[i][0]
center = boxes[i][1]
quat = boxes[i][2]
pyflex.add_box(halfEdge, center, quat)
n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
# print("n_particles", n_particles)
# print("n_shapes", n_shapes)
positions = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
x_box = x_center
v_box = 0.
for j in range(time_step_clip):
x_box_last = x_box
x_box += v_box * dt
shape_states_ = calc_shape_states_FluidShake(
x_box, x_box_last, scene_params[-2:], height, border)
pyflex.set_shape_states(shape_states_)
pyflex.step()
for j in range(time_step):
x_box_last = x_box
x_box += v_box * dt
v_box += rand_float(-0.15, 0.15) - x_box * 0.1
shape_states_ = calc_shape_states_FluidShake(
x_box, x_box_last, scene_params[-2:], height, border)
pyflex.set_shape_states(shape_states_)
positions[j, :n_particles] = pyflex.get_positions().reshape(-1, 4)[:, :3]
shape_states = pyflex.get_shape_states().reshape(-1, shape_state_dim)
for k in range(n_shapes):
positions[j, n_particles + k] = shape_states[k, :3]
shape_quats[j, k] = shape_states[k, 6:10]
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
# NOTE: 1) particle + glass wall positions, 2) particle + glass wall velocitys, 3) glass wall rotations, 4) scenen parameters
data = [positions[j], velocities[j], shape_quats[j], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'RiceGrip':
# if env is RiceGrip
# repeat the grip for R times
R = 3
gripper_config = sample_control_RiceGrip()
if i % R == 0:
### set scene
# x, y, z: [8.0, 10.0]
# clusterStiffness: [0.3, 0.7]
# clusterPlasticThreshold: [0.00001, 0.0005]
# clusterPlasticCreep: [0.1, 0.3]
x = rand_float(8.0, 10.0)
y = rand_float(8.0, 10.0)
z = rand_float(8.0, 10.0)
clusterStiffness = rand_float(0.3, 0.7)
clusterPlasticThreshold = rand_float(0.00001, 0.0005)
clusterPlasticCreep = rand_float(0.1, 0.3)
scene_params = np.array([x, y, z, clusterStiffness, clusterPlasticThreshold, clusterPlasticCreep])
pyflex.set_scene(env_idx, scene_params, thread_idx)
scene_params[4] *= 1000.
halfEdge = np.array([0.15, 0.8, 0.15])
center = np.array([0., 0., 0.])
quat = np.array([1., 0., 0., 0.])
pyflex.add_box(halfEdge, center, quat)
pyflex.add_box(halfEdge, center, quat)
n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
positions = np.zeros((time_step, n_particles + n_shapes, 6), dtype=np.float32)
velocities = np.zeros((time_step, n_particles + n_shapes, 6), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
for j in range(time_step_clip):
shape_states = calc_shape_states_RiceGrip(0, dt, shape_state_dim, gripper_config)
pyflex.set_shape_states(shape_states)
pyflex.step()
p = pyflex.get_positions().reshape(-1, 4)[:, :3]
clusters = []
st_time = time.time()
kmeans = MiniBatchKMeans(n_clusters=root_num[0][0], random_state=0).fit(p)
# print('Time on kmeans', time.time() - st_time)
clusters.append([[kmeans.labels_]])
# centers = kmeans.cluster_centers_
for j in range(time_step):
shape_states = calc_shape_states_RiceGrip(j * dt, dt, shape_state_dim, gripper_config)
pyflex.set_shape_states(shape_states)
positions[j, :n_particles, :3] = pyflex.get_rigidGlobalPositions().reshape(-1, 3)
positions[j, :n_particles, 3:] = pyflex.get_positions().reshape(-1, 4)[:, :3]
shape_states = pyflex.get_shape_states().reshape(-1, shape_state_dim)
for k in range(n_shapes):
positions[j, n_particles + k, :3] = shape_states[k, :3]
positions[j, n_particles + k, 3:] = shape_states[k, :3]
shape_quats[j, k] = shape_states[k, 6:10]
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j], shape_quats[j], clusters, scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
else:
raise AssertionError("Unsupported env")
# change dtype for more accurate stat calculation
# only normalize positions and velocities
datas = [positions.astype(np.float64), velocities.astype(np.float64)]
# NOTE: stats is of length 2, for positions and velocities
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0] * datas[j].shape[1]
stats[j] = combine_stat(stats[j], stat)
pyflex.clean()
return stats
def get_world_coords(rgb, depth, env):
height, width, _ = rgb.shape
K = intrinsic_from_fov(height, width, 45) # the fov is 90 degrees
# Apply back-projection: K_inv @ pixels * depth
cam_coords = np.ones((height, width, 4))
u0 = K[0, 2]
v0 = K[1, 2]
fx = K[0, 0]
fy = K[1, 1]
# Loop through each pixel in the image
for v in range(height):
for u in range(width):
# Apply equation in fig 3
x = (u - u0) * depth[v, u] / fx
y = (v - v0) * depth[v, u] / fy
z = depth[v, u]
cam_coords[v][u][:3] = (x, y, z)
particle_pos = pyflex.get_positions().reshape((-1, 4))
print('cloth pixels: ', np.count_nonzero(depth))
print("cloth particle num: ", pyflex.get_n_particles())
# debug: print camera coordinates
# print(cam_coords.shape)
# cnt = 0
# for v in range(height):
# for u in range(width):
# if depth[v][u] > 0:
# print("v: {} u: {} cnt: {} cam_coord: {} approximate particle pos: {}".format(
# v, u, cnt, cam_coords[v][u], particle_pos[cnt]))
# rgb = rgbd[:, :, :3].copy()
# rgb[v][u][0] = 255
# rgb[v][u][1] = 0
# rgb[v][u][2] = 0
# cv2.imshow('rgb', rgb[:, :, ::-1])
# cv2.waitKey()
# cnt += 1
# from cam coord to world coord
cam_x, cam_y, cam_z = env.camera_params['default_camera']['pos'][0], env.camera_params['default_camera']['pos'][1], env.camera_params['default_camera']['pos'][2]
cam_x_angle, cam_y_angle, cam_z_angle = env.camera_params['default_camera']['angle'][0], env.camera_params['default_camera']['angle'][1], env.camera_params['default_camera']['angle'][2]
# get rotation matrix: from world to camera
matrix1 = get_rotation_matrix(- cam_x_angle, [0, 1, 0])
# matrix2 = get_rotation_matrix(- cam_y_angle - np.pi, [np.cos(cam_x_angle), 0, np.sin(cam_x_angle)])
matrix2 = get_rotation_matrix(- cam_y_angle - np.pi, [1, 0, 0])
rotation_matrix = matrix2 @ matrix1
# get translation matrix: from world to camera
translation_matrix = np.zeros((4, 4))
translation_matrix[0][0] = 1
translation_matrix[1][1] = 1
translation_matrix[2][2] = 1
translation_matrix[3][3] = 1
translation_matrix[0][3] = - cam_x
translation_matrix[1][3] = - cam_y
translation_matrix[2][3] = - cam_z
# debug: from world to camera
cloth_x, cloth_y = env.current_config['ClothSize'][0], env.current_config['ClothSize'][1]
# cnt = 0
# for u in range(height):
# for v in range(width):
# if depth[u][v] > 0:
# world_coord = np.ones(4)
# world_coord[:3] = particle_pos[cnt][:3]
# convert_cam_coord = rotation_matrix @ translation_matrix @ world_coord
# # convert_cam_coord = translation_matrix @ matrix2 @ matrix1 @ world_coord
# print("u {} v {} \n world coord {} \n convert camera coord {} \n real camera coord {}".format(
# u, v, world_coord, convert_cam_coord, cam_coords[u][v]
# ))
# cnt += 1
# input('wait...')
# convert the camera coordinate back to the world coordinate using the rotation and translation matrix
cam_coords = cam_coords.reshape((-1, 4)).transpose() # 4 x (height x width)
world_coords = np.linalg.inv(rotation_matrix @ translation_matrix) @ cam_coords # 4 x (height x width)
world_coords = world_coords.transpose().reshape((height, width, 4))
# roughly check the final world coordinate with the actual coordinate
# firstu = 0
# firstv = 0
# for u in range(height):
# for v in range(width):
# if depth[u][v]:
# if u > firstu: # move to a new line
# firstu = u
# firstv = v
# cnt = (u - firstu) * cloth_x + (v - firstv)
# print("u {} v {} cnt{}\nworld_coord\t{}\nparticle coord\t{}\nerror\t{}".format(
# u, v, cnt, world_coords[u][v], particle_pos[cnt], np.linalg.norm( world_coords[u][v] - particle_pos[cnt])))
# rgb = rgbd[:, :, :3].copy()
# rgb[u][v][0] = 255
# rgb[u][v][1] = 0
# rgb[u][v][2] = 0
# cv2.imshow('rgb', rgb[:, :, ::-1])
# cv2.waitKey()
# exit()
return world_coords