def main():
for cls_name in tqdm(sel_classes):
print(cls_name)
# if cls_name != 'driller':
# continue
rd_stat = []
td_stat = []
pose_observed = []
pose_rendered = []
observed_set_file = os.path.join(
observed_set_dir, "NDtrain_observed_{}.txt".format(cls_name)
)
with open(observed_set_file) as f:
image_list = [x.strip() for x in f.readlines()]
for data in image_list:
pose_observed_path = os.path.join(
gt_observed_dir, cls_name, data + "-pose.txt"
)
src_pose_m = np.loadtxt(pose_observed_path, skiprows=1)
src_euler = np.squeeze(mat2euler(src_pose_m[:, :3]))
src_quat = euler2quat(src_euler[0], src_euler[1], src_euler[2]).reshape(
1, -1
)
src_trans = src_pose_m[:, 3]
pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3)))))
for rendered_idx in range(num_rendered_per_observed):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack(
(
euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]),
tgt_trans.reshape((3, 1)),
)
)
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count = 0
while r_dist > angle_max or not (
48 < center_x < (640 - 48) and 48 < center_y < (480 - 48)
):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack(
(
euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]),
tgt_trans.reshape((3, 1)),
)
)
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count += 1
if count == 100:
print(rendered_idx)
print(
"{}: {}, {}, {}, {}".format(
data, r_dist, t_dist, center_x, center_y
)
)
print(
"count: {}, image_path: {}, rendered_idx: {}".format(
count,
pose_observed_path.replace("pose.txt", "color.png"),
rendered_idx,
)
)
tgt_quat = euler2quat(tgt_euler[0], tgt_euler[1], tgt_euler[2]).reshape(
1, -1
)
pose_rendered.append(np.hstack((tgt_quat, tgt_trans.reshape(1, 3))))
rd_stat.append(r_dist)
td_stat.append(t_dist)
rd_stat = np.array(rd_stat)
td_stat = np.array(td_stat)
print("r dist: {} +/- {}".format(np.mean(rd_stat), np.std(rd_stat)))
print("t dist: {} +/- {}".format(np.mean(td_stat), np.std(td_stat)))
output_file_name = os.path.join(
pose_dir,
"LM6d_occ_dsm_{}_NDtrain_rendered_pose_{}.txt".format(version, cls_name),
)
with open(output_file_name, "w") as text_file:
for x in pose_rendered:
text_file.write("{}\n".format(" ".join(map(str, np.squeeze(x)))))
print("{} finished".format(__file__))
src_euler = np.squeeze(mat2euler(src_pose_m[:3, :3]))
src_quat = euler2quat(src_euler[0], src_euler[1],
src_euler[2]).reshape(1, -1)
src_trans = src_pose_m[:, 3]
pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3)))))
for rendered_idx in range(num_rendered_per_observed):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi,
3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack(
(euler2mat(tgt_euler[0], tgt_euler[1],
tgt_euler[2]), tgt_trans.reshape((3, 1))))
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count = 0
while r_dist > angle_max or not (16 < center_x <
(640 - 16) and 16 < center_y <
(480 - 16)):
tgt_euler = src_euler + np.random.normal(
0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack(
src_pose_m = np.loadtxt(pose_observed_path, skiprows=1)
src_euler = np.squeeze(mat2euler(src_pose_m[:3, :3]))
src_quat = euler2quat(src_euler[0], src_euler[1],
src_euler[2]).reshape(1, -1)
src_trans = src_pose_m[:, 3]
pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3)))))
for rendered_idx in range(num_rendered_per_observed):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi,
3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack((euler2mat(tgt_euler[0], tgt_euler[1],
tgt_euler[2]),
tgt_trans.reshape((3, 1))))
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count = 0
while (r_dist > angle_max
or not (16 < center_x < (640 - 16) and 16 < center_y <
(480 - 16))):
tgt_euler = src_euler + np.random.normal(
0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
src_euler = np.squeeze(mat2euler(src_pose_m[:3, :3]))
src_quat = euler2quat(src_euler[0], src_euler[1],
src_euler[2]).reshape(1, -1)
src_trans = src_pose_m[:, 3]
pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3)))))
for rendered_idx in range(num_rendered_per_observed):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi,
3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack((
euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]),
tgt_trans.reshape((3, 1)),
))
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count = 0
while r_dist > angle_max or not (16 < center_x <
(640 - 16) and 16 < center_y <
(480 - 16)):
tgt_euler = src_euler + np.random.normal(
0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
for observed_idx in tqdm(image_list):
pose_observed_path = os.path.join(data_dir, "{}-pose.txt".format(observed_idx))
src_pose_m = np.loadtxt(pose_observed_path, skiprows=1)
src_euler = np.squeeze(mat2euler(src_pose_m[:3, :3]))
src_quat = euler2quat(src_euler[0], src_euler[1], src_euler[2]).reshape(1, -1)
src_trans = src_pose_m[:, 3]
pose_observed.append((np.hstack((src_quat, src_trans.reshape(1, 3)))))
for rendered_idx in range(num_rendered_per_observed):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack((euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]), tgt_trans.reshape((3, 1))))
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]
center_y = transform[1] / transform[2]
count = 0
while r_dist > angle_max or not (16 < center_x < (640 - 16) and 16 < center_y < (480 - 16)):
tgt_euler = src_euler + np.random.normal(0, angle_std / 180 * pi, 3)
x_error = np.random.normal(0, x_std, 1)[0]
y_error = np.random.normal(0, y_std, 1)[0]
z_error = np.random.normal(0, z_std, 1)[0]
tgt_trans = src_trans + np.array([x_error, y_error, z_error])
tgt_pose_m = np.hstack((euler2mat(tgt_euler[0], tgt_euler[1], tgt_euler[2]), tgt_trans.reshape((3, 1))))
r_dist, t_dist = calc_rt_dist_m(tgt_pose_m, src_pose_m)
transform = np.matmul(K, tgt_trans.reshape(3, 1))
center_x = transform[0] / transform[2]