# >>>>>>>>>>>>>>>>>>>>>>>>>> 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
if args.item in ['drawer']:
nocs_corner[0] = np.dot(nocs_corner[0].reshape(1, 3) - 0.5,
rot_mat.T) + 0.5
def __preprocess_and_save__(self, index):
obj_category = self.list_obj[index]
ins = self.list_instance[index]
obj = self.objnamelist.index(obj_category)
art_status = self.list_status[index]
frame_order = self.list_rank[index]
label = self.list_label[index]
h5_save_path = self.root_dir + '/hdf5/' + obj_category + '/' + ins + '/' + art_status
if (not os.path.exists(h5_save_path)):
os.makedirs(h5_save_path)
h5_save_name = h5_save_path + '/{}.h5'.format(frame_order)
num_parts = self.urdf_dict[obj_category][ins]['num_links']
model_offsets = self.urdf_dict[obj_category][ins]['link']
joint_offsets = self.urdf_dict[obj_category][ins]['joint']
parts_model_point = [None] * num_parts
parts_world_point = [None] * num_parts
parts_target_point = [None] * num_parts
parts_cloud_cam = [None] * num_parts
parts_cloud_world = [None] * num_parts
parts_cloud_canon = [None] * num_parts
parts_cloud_urdf = [None] * num_parts
parts_cloud_norm = [None] * num_parts
parts_world_pos = [None] * num_parts
parts_world_orn = [None] * num_parts
parts_urdf_pos = [None] * num_parts
parts_urdf_orn = [None] * num_parts
parts_urdf_box = [None] * num_parts
parts_model2world = [None] * num_parts
parts_canon2urdf = [None] * num_parts
parts_target_r = [None] * num_parts
parts_target_t = [None] * num_parts
parts_mask = [None] * num_parts
choose_x = [None] * num_parts
choose_y = [None] * num_parts
choose_to_whole = [None] * num_parts
# rgb/depth/label
print('current image: ', self.list_rgb[index])
img = Image.open(self.list_rgb[index])
img = np.array(img) #.astype(np.uint8)
depth = np.array(h5py.File(self.list_depth[index], 'r')['data'])
label = np.array(Image.open(self.list_label[index]))
# pose infos
pose_dict = self.meta_dict[obj_category][ins][art_status][
'frame_{}'.format(frame_order)]
urdf_dict = self.urdf_dict[obj_category][ins]
viewMat = np.array(pose_dict['viewMat']).reshape(4, 4).T
projMat = np.array(pose_dict['projMat']).reshape(4, 4).T
parts_world_pos[0] = np.array([0, 0, 0])
parts_world_orn[0] = np.array([0, 0, 0, 1])
for link in range(0, num_parts):
if link > 0:
parts_world_pos[link] = np.array(
pose_dict['obj'][link - 1][4]).astype(np.float32)
parts_world_orn[link] = np.array(
pose_dict['obj'][link - 1][5]).astype(np.float32)
for link in range(num_parts):
if link == 1 and num_parts == 2:
parts_urdf_pos[link] = np.array(urdf_dict['joint']['xyz'][
link -
1]) # todo, accumulate joints pffsets != link offsets
else:
parts_urdf_pos[link] = -np.array(
urdf_dict['link']['xyz'][link])
parts_urdf_orn[link] = np.array(urdf_dict['link']['rpy'][link])
for k in range(num_parts):
center_world_orn = parts_world_orn[k]
center_world_orn = np.array([
center_world_orn[3], center_world_orn[0], center_world_orn[1],
center_world_orn[2]
])
my_model2world_r = quaternion_matrix(
center_world_orn)[:4, :4] # [w, x, y, z]
my_model2world_t = parts_world_pos[k]
my_model2world_mat = np.copy(my_model2world_r)
for m in range(3):
my_model2world_mat[m, 3] = my_model2world_t[m]
my_world2camera_mat = viewMat
my_camera2clip_mat = projMat
my_model2camera_mat = np.dot(my_world2camera_mat,
my_model2world_mat)
parts_model2world[k] = my_model2world_mat
# depth to cloud data
mask = np.array((label[:, :] < num_parts) & (label[:, :] > -1)).astype(
np.uint8)
mask_whole = np.copy(mask)
for n in range(num_parts):
parts_mask[n] = np.array((label[:, :] == (n))).astype(np.uint8)
choose_to_whole[n] = np.where(parts_mask[n] > 0)
#>>>>>>>>>>------- rendering target pcloud from depth image --------<<<<<<<<<#
# first get projected map
ymap = self.ymap
xmap = self.xmap
h = self.height
w = self.width
u_map = ymap * 2 / w - 1
v_map = (512 - xmap) * 2 / h - 1
v1_map = xmap * 2 / h - 1
w_channel = -depth
projected_map = np.stack(
[u_map * w_channel, v_map * w_channel, depth,
w_channel]).transpose([1, 2, 0])
projected_map1 = np.stack(
[u_map * w_channel, v1_map * w_channel, depth,
w_channel]).transpose([1, 2, 0])
for s in range(num_parts):
x_set, y_set = choose_to_whole[s]
if len(x_set) < 10:
print('data is empty, skipping!!!')
return None
else:
choose_x[s] = x_set
choose_y[s] = y_set
# ---------------> from projected map into target part_cloud
# order: cam->world->canon)
projected_points = projected_map[
choose_x[s][:].astype(np.uint16),
choose_y[s][:].astype(np.uint16), :]
projected_points = np.reshape(projected_points, [-1, 4])
depth_channel = -projected_points[:, 3:4]
cloud_cam = np.dot(
projected_points[:, 0:2] -
np.dot(depth_channel, projMat[0:2, 2:3].T),
np.linalg.pinv(projMat[:2, :2].T))
projected_points1 = projected_map1[
choose_x[s][:].astype(np.uint16),
choose_y[s][:].astype(np.uint16), :]
projected_points1 = np.reshape(projected_points1, [-1, 4])
cloud_cam_real = np.dot(
projected_points1[:, 0:2] -
np.dot(depth_channel, projMat[0:2, 2:3].T),
np.linalg.pinv(projMat[:2, :2].T))
cloud_cam_real = np.concatenate((cloud_cam_real, depth_channel),
axis=1)
cloud_cam = np.concatenate((cloud_cam, depth_channel), axis=1)
cloud_cam_full = np.concatenate(
(cloud_cam, np.ones((cloud_cam.shape[0], 1))), axis=1)
# modify, todo
camera_pose_mat = np.linalg.pinv(viewMat.T)
camera_pose_mat[:3, :] = -camera_pose_mat[:3, :]
cloud_world = np.dot(cloud_cam_full, camera_pose_mat)
cloud_canon = np.dot(cloud_world,
np.linalg.pinv(parts_model2world[s].T))
# canon points should be points coordinates centered in the inertial frame
parts_cloud_cam[s] = cloud_cam_real[:, :3]
parts_cloud_world[s] = cloud_world[:, :3]
parts_cloud_canon[s] = cloud_canon[:, :3]
for k in range(num_parts):
center_joint_orn = parts_urdf_orn[k]
my_canon2urdf_r = euler_matrix(
center_joint_orn[0], center_joint_orn[1],
center_joint_orn[2])[:4, :4] # [w, x, y, z]
my_canon2urdf_t = parts_urdf_pos[k]
my_canon2urdf_mat = my_canon2urdf_r
for m in range(3):
my_canon2urdf_mat[m, 3] = my_canon2urdf_t[m]
part_points_space = np.concatenate(
(parts_cloud_canon[k],
np.ones((parts_cloud_canon[k].shape[0], 1))),
axis=1)
parts_cloud_urdf[k] = np.dot(part_points_space,
my_canon2urdf_mat.T)
#>>>>>>>>>>>>>>> go to PNCS space
for link in range(num_parts):
tight_w = max(parts_cloud_urdf[link][:, 0]) - min(
parts_cloud_urdf[link][:, 0])
tight_l = max(parts_cloud_urdf[link][:, 1]) - min(
parts_cloud_urdf[link][:, 1])
tight_h = max(parts_cloud_urdf[link][:, 2]) - min(
parts_cloud_urdf[link][:, 2])
norm_factor = np.sqrt(1) / np.sqrt(tight_w**2 + tight_l**2 +
tight_h**2)
base_p = np.array([
min(parts_cloud_urdf[link][:, 0]),
min(parts_cloud_urdf[link][:, 1]),
min(parts_cloud_urdf[link][:, 2])
]).reshape(1, 3)
extre_p = np.array([
max(parts_cloud_urdf[link][:, 0]),
max(parts_cloud_urdf[link][:, 1]),
max(parts_cloud_urdf[link][:, 2])
]).reshape(1, 3)
center_p = (extre_p - base_p) / 2 * norm_factor
parts_cloud_norm[link] = (parts_cloud_urdf[link][:, :3] -
base_p) * norm_factor + np.array(
[0.5, 0.5, 0.5]).reshape(
1, 3) - center_p.reshape(1, 3)
x_set_pcloud = np.concatenate(choose_x, axis=0)
y_set_pcloud = np.concatenate(choose_y, axis=0)
# save into h5 for rgb_img, input_pts, mask, correpsonding urdf_points
print('Writing to ', h5_save_name)
hf = h5py.File(h5_save_name, 'w')
hf.create_dataset('rgb', data=img)
hf.create_dataset('mask', data=mask)
cloud_cam = hf.create_group('gt_points')
for part_i, points in enumerate(parts_cloud_cam):
cloud_cam.create_dataset(str(part_i), data=points)
coord_gt = hf.create_group('gt_coords')
for part_i, points in enumerate(parts_cloud_urdf):
coord_gt.create_dataset(str(part_i), data=points)
hf.close()
################# for debug only, let me know if you have questions #################
if self.is_debug:
figure = plt.figure(dpi=200)
ax = plt.subplot(121)
plt.imshow(img)
plt.title('RGB image')
ax1 = plt.subplot(122)
plt.imshow(depth)
plt.title('depth image')
plt.show()
plot3d_pts(
[parts_cloud_cam],
[['part {}'.format(i) for i in range(len(parts_cloud_cam))]],
s=5,
title_name=['camera coords'])
plot3d_pts(
[parts_cloud_world],
[['part {}'.format(i) for i in range(len(parts_cloud_world))]],
s=5,
title_name=['world coords'])
plot3d_pts(
[parts_cloud_canon],
[['part {}'.format(i) for i in range(len(parts_cloud_canon))]],
s=5,
title_name=['canon coords'])
plot3d_pts(
[parts_cloud_urdf],
[['part {}'.format(i) for i in range(len(parts_cloud_urdf))]],
s=5,
title_name=['urdf coords'])
return None
def __preprocess_and_save__(self, index):
obj_category = self.list_obj[index]
ins = self.list_instance[index]
obj = self.objnamelist.index(obj_category)
art_status = self.list_status[index]
frame_order = self.list_rank[index]
label = self.list_label[index]
h5_save_path = (self.root_dir + "/hdf5/" + obj_category + "/" + ins +
"/" + art_status)
if not os.path.exists(h5_save_path):
os.makedirs(h5_save_path)
h5_save_name = h5_save_path + "/{}.h5".format(frame_order)
num_parts = self.urdf_dict[obj_category][ins]["num_links"]
new_num_parts = num_parts - 1
model_offsets = self.urdf_dict[obj_category][ins]["link"]
joint_offsets = self.urdf_dict[obj_category][ins]["joint"]
parts_model_point = [None] * new_num_parts
parts_world_point = [None] * new_num_parts
parts_target_point = [None] * new_num_parts
parts_cloud_cam = [None] * new_num_parts
parts_cloud_world = [None] * new_num_parts
parts_cloud_canon = [None] * new_num_parts
parts_cloud_urdf = [None] * new_num_parts
parts_cloud_norm = [None] * new_num_parts
parts_world_pos = [None] * new_num_parts
parts_world_orn = [None] * new_num_parts
parts_urdf_pos = [None] * new_num_parts
parts_urdf_orn = [None] * new_num_parts
parts_urdf_box = [None] * new_num_parts
parts_model2world = [None] * new_num_parts
parts_canon2urdf = [None] * new_num_parts
parts_target_r = [None] * new_num_parts
parts_target_t = [None] * new_num_parts
parts_mask = [None] * new_num_parts
choose_x = [None] * new_num_parts
choose_y = [None] * new_num_parts
choose_to_whole = [None] * new_num_parts
# rgb/depth/label
print("current image: ", self.list_rgb[index])
img = Image.open(self.list_rgb[index])
img = np.array(img) # .astype(np.uint8)
depth = np.array(h5py.File(self.list_depth[index], "r")["data"])
label = np.array(Image.open(self.list_label[index]))
# pose infos
pose_dict = self.meta_dict[obj_category][ins][art_status][
"frame_{}".format(frame_order)]
urdf_dict = self.urdf_dict[obj_category][ins]
viewMat = np.array(pose_dict["viewMat"]).reshape(4, 4).T
projMat = np.array(pose_dict["projMat"]).reshape(4, 4).T
parts_world_pos[0] = np.array([0, 0, 0])
parts_world_orn[0] = np.array([0, 0, 0, 1])
# SAPIEN: skip the virtual base part
for link in range(1, num_parts):
if link > 0:
parts_world_pos[link - 1] = np.array(
pose_dict["obj"][link - 1][4]).astype(np.float32)
parts_world_orn[link - 1] = np.array(
pose_dict["obj"][link - 1][5]).astype(np.float32)
for link in range(1, num_parts):
if link == 1 and num_parts == 2:
parts_urdf_pos[link] = np.array(urdf_dict["joint"]["xyz"][
link -
1]) # todo, accumulate joints offsets != link offsets
else:
# Only change this branch for SAPIEN data
parts_urdf_pos[link - 1] = -np.array(
urdf_dict["link"]["xyz"][link][0])
parts_urdf_orn[link - 1] = np.array(
urdf_dict["link"]["rpy"][link][0])
for k in range(1, num_parts):
center_world_orn = parts_world_orn[k - 1]
center_world_orn = np.array([
center_world_orn[3],
center_world_orn[0],
center_world_orn[1],
center_world_orn[2],
])
my_model2world_r = quaternion_matrix(
center_world_orn)[:4, :4] # [w, x, y, z]
my_model2world_t = parts_world_pos[k - 1]
my_model2world_mat = np.copy(my_model2world_r)
for m in range(3):
my_model2world_mat[m, 3] = my_model2world_t[m]
my_world2camera_mat = viewMat
my_camera2clip_mat = projMat
my_model2camera_mat = np.dot(my_world2camera_mat,
my_model2world_mat)
parts_model2world[k - 1] = my_model2world_mat
# depth to cloud data
# SAPIEN, throw away the label 0 for virtual base link
mask = np.array((label[:, :] < num_parts) & (label[:, :] > 0)).astype(
np.uint8)
mask_whole = np.copy(mask)
for n in range(1, num_parts):
parts_mask[n - 1] = np.array((label[:, :] == (n))).astype(np.uint8)
choose_to_whole[n - 1] = np.where(parts_mask[n - 1] > 0)
# >>>>>>>>>>------- rendering target pcloud from depth image --------<<<<<<<<<#
# first get projected map
# ymap = self.ymap
# xmap = self.xmap
# h = self.height
# w = self.width
# u_map = ymap * 2 / w - 1
# v_map = (512 - xmap) * 2 / h - 1
# v1_map = xmap * 2 / h - 1
# w_channel = -depth
# projected_map = np.stack(
# [u_map * w_channel, v_map * w_channel, depth, w_channel]
# ).transpose([1, 2, 0])
# projected_map1 = np.stack(
# [u_map * w_channel, v1_map * w_channel, depth, w_channel]
# ).transpose([1, 2, 0])
for s in range(1, num_parts):
x_set, y_set = choose_to_whole[s - 1]
if len(x_set) < 10:
print(ins, art_status, frame_order)
print(
"data is empty, skipping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
)
return None
else:
choose_x[s - 1] = x_set
choose_y[s - 1] = y_set
# ---------------> from projected map into target part_cloud
# order: cam->world->canon)
# MotionNet Modification
point_num = np.shape(choose_x[s - 1])[0]
cloud_cam = []
cloud_cam_real = []
for i in range(point_num):
x = choose_x[s - 1][i]
y = choose_y[s - 1][i]
y_real = 512 - y
cloud_cam.append([
(y + projMat[0, 2]) * depth[x, y] / projMat[0, 0],
-(x + projMat[1, 2]) * depth[x, y] / (-projMat[1, 1]),
-depth[x, y],
])
cloud_cam_real.append([
(y + projMat[0, 2]) * depth[x, y] / projMat[0, 0],
-(x + projMat[1, 2]) * depth[x, y] / (-projMat[1, 1]),
-depth[x, y],
])
# cloud_cam.append(
# [
# (x + projMat[0, 2]) * depth[x, y_real] / projMat[0, 0],
# -(y_real + projMat[1, 2]) * depth[x, y_real] / (-projMat[1, 1]),
# -depth[x, y_real],
# ]
# )
# pdb.set_trace()
# print(cloud_cam)
# break
cloud_cam = np.array(cloud_cam)
cloud_cam_real = np.array(cloud_cam_real)
# pdb.set_trace()
# MotionNet Modification
# projected_points = projected_map[choose_x[s-1][:].astype(np.uint16), choose_y[s-1][:].astype(np.uint16), :]
# projected_points = np.reshape(projected_points, [-1, 4])
# depth_channel = - projected_points[:, 3:4]
# cloud_cam = np.dot(projected_points[:, 0:2] - np.dot(depth_channel, projMat[0:2, 2:3].T), np.linalg.pinv(projMat[:2, :2].T))
# projected_points1 = projected_map1[choose_x[s-1][:].astype(np.uint16), choose_y[s-1][:].astype(np.uint16), :]
# projected_points1 = np.reshape(projected_points1, [-1, 4])
# cloud_cam_real = np.dot(projected_points1[:, 0:2] - np.dot(depth_channel, projMat[0:2, 2:3].T), np.linalg.pinv(projMat[:2, :2].T))
# cloud_cam_real = np.concatenate((cloud_cam_real, depth_channel), axis=1)
# cloud_cam = np.concatenate((cloud_cam, depth_channel), axis=1)
cloud_cam_full = np.concatenate(
(cloud_cam, np.ones((cloud_cam.shape[0], 1))), axis=1)
# modify, todo
camera_pose_mat = np.linalg.pinv(viewMat.T)
# MotionNet modification
# camera_pose_mat[:3, :] = -camera_pose_mat[:3, :]
cloud_world = np.dot(cloud_cam_full, camera_pose_mat)
cloud_canon = np.dot(cloud_world,
np.linalg.pinv(parts_model2world[s - 1].T))
# canon points should be points coordinates centered in the inertial frame
parts_cloud_cam[s - 1] = cloud_cam_real[:, :3]
parts_cloud_world[s - 1] = cloud_world[:, :3]
parts_cloud_canon[s - 1] = cloud_canon[:, :3]
for k in range(1, num_parts):
center_joint_orn = parts_urdf_orn[k - 1]
my_canon2urdf_r = euler_matrix(
center_joint_orn[0], center_joint_orn[1],
center_joint_orn[2])[:4, :4] # [w, x, y, z]
my_canon2urdf_t = parts_urdf_pos[k - 1]
my_canon2urdf_mat = my_canon2urdf_r
for m in range(3):
my_canon2urdf_mat[m, 3] = my_canon2urdf_t[m]
part_points_space = np.concatenate(
(
parts_cloud_canon[k - 1],
np.ones((parts_cloud_canon[k - 1].shape[0], 1)),
),
axis=1,
)
parts_cloud_urdf[k - 1] = np.dot(part_points_space,
my_canon2urdf_mat.T)
# >>>>>>>>>>>>>>> go to NPCS space
# NPCS here is not stored into hdf5
for link in range(1, num_parts):
tight_w = max(parts_cloud_urdf[link - 1][:, 0]) - min(
parts_cloud_urdf[link - 1][:, 0])
tight_l = max(parts_cloud_urdf[link - 1][:, 1]) - min(
parts_cloud_urdf[link - 1][:, 1])
tight_h = max(parts_cloud_urdf[link - 1][:, 2]) - min(
parts_cloud_urdf[link - 1][:, 2])
norm_factor = np.sqrt(1) / np.sqrt(tight_w**2 + tight_l**2 +
tight_h**2)
base_p = np.array([
min(parts_cloud_urdf[link - 1][:, 0]),
min(parts_cloud_urdf[link - 1][:, 1]),
min(parts_cloud_urdf[link - 1][:, 2]),
]).reshape(1, 3)
extre_p = np.array([
max(parts_cloud_urdf[link - 1][:, 0]),
max(parts_cloud_urdf[link - 1][:, 1]),
max(parts_cloud_urdf[link - 1][:, 2]),
]).reshape(1, 3)
center_p = (extre_p - base_p) / 2 * norm_factor
parts_cloud_norm[link - 1] = (
(parts_cloud_urdf[link - 1][:, :3] - base_p) * norm_factor +
np.array([0.5, 0.5, 0.5]).reshape(1, 3) -
center_p.reshape(1, 3))
x_set_pcloud = np.concatenate(choose_x, axis=0)
y_set_pcloud = np.concatenate(choose_y, axis=0)
# save into h5 for rgb_img, input_pts, mask, correpsonding urdf_points
print("Writing to ", h5_save_name)
hf = h5py.File(h5_save_name, "w")
hf.create_dataset("rgb", data=img)
hf.create_dataset("mask", data=mask)
cloud_cam = hf.create_group("gt_points")
for part_i, points in enumerate(parts_cloud_cam):
cloud_cam.create_dataset(str(part_i), data=points)
coord_gt = hf.create_group("gt_coords")
for part_i, points in enumerate(parts_cloud_urdf):
coord_gt.create_dataset(str(part_i), data=points)
hf.close()
################# for debug only, let me know if you have questions #################
if self.is_debug:
figure = plt.figure(dpi=200)
ax = plt.subplot(121)
plt.imshow(img)
plt.title('RGB image')
ax1 = plt.subplot(122)
plt.imshow(depth)
plt.title('depth image')
plt.show()
plot3d_pts(
[parts_cloud_cam],
[['part {}'.format(i) for i in range(len(parts_cloud_cam))]],
s=5,
title_name=['camera coords'])
plot3d_pts(
[parts_cloud_world],
[['part {}'.format(i) for i in range(len(parts_cloud_world))]],
s=5,
title_name=['world coords'])
plot3d_pts(
[parts_cloud_canon],
[['part {}'.format(i) for i in range(len(parts_cloud_canon))]],
s=5,
title_name=['canon coords'])
plot3d_pts(
[parts_cloud_urdf],
[['part {}'.format(i) for i in range(len(parts_cloud_urdf))]],
s=5,
title_name=['urdf coords'])
return None
my_t_final = np.array(
[my_mat_final[0][3], my_mat_final[1][3], my_mat_final[2][3]])
my_pred = np.append(my_r_final, my_t_final)
my_result.append(my_pred.tolist())
my_result_np = np.array(my_result)
my_result_mean = np.mean(my_result, axis=0)
my_r = my_result_mean[:4]
my_t = my_result_mean[4:]
my_euler_form_r = euler_from_quaternion(my_r)
print("my_euler_form_r", my_euler_form_r)
#my_euler_form_r <class 'tuple'>: (0.9198490563735781, -0.007832527911272334, -0.47081842893943104)
my_euler_yaw = list(my_euler_form_r)[2]
my_rotation_matrix_from_euler = euler_matrix(my_euler_form_r[0],
my_euler_form_r[1],
my_euler_form_r[2])
my_rotation_matrix_from_q = quaternion_matrix(my_r)
x = np.dot(my_rotation_matrix_from_euler,
np.array([[1, 0, 0, 1]]).transpose()).flatten()
y = np.dot(my_rotation_matrix_from_euler,
np.array([[0, 1, 0, 1]]).transpose()).flatten()
z = np.dot(my_rotation_matrix_from_euler,
np.array([[0, 0, 1, 1]]).transpose()).flatten()
newvs = []
grasp_vector = [math.cos(my_euler_yaw), math.sin(my_euler_yaw), 0]
# newvs.append([0,0,0,grasp_vector[0],grasp_vector[1],grasp_vector[2]])
mx = my_rotation_matrix_from_euler[0, 0:3]
my = my_rotation_matrix_from_euler[1, 0:3]
for name in group:
image = proj.findImageByName(name)
ned, ypr, quat = image.get_camera_pose(opt=True)
src_list.append(ned)
ned, ypr, quat = image.get_camera_pose()
dst_list.append(ned)
A = get_recenter_affine(src_list, dst_list)
# extract the rotation matrix (R) from the affine transform
scale, shear, angles, trans, persp = transformations.decompose_matrix(A)
print(' scale:', scale)
print(' shear:', shear)
print(' angles:', angles)
print(' translate:', trans)
print(' perspective:', persp)
R = transformations.euler_matrix(*angles)
print("R:\n{}".format(R))
# fixme (just group):
# update the optimized camera locations based on best fit
camera_list = []
# load optimized poses
for image in proj.image_list:
if image.name in group:
ned, ypr, quat = image.get_camera_pose(opt=True)
else:
# this is just fodder to match size/index of the lists
ned, ypr, quat = image.get_camera_pose()
camera_list.append(ned)
def optmizer(project_dir, optmize_options):
group_id = optmize_options[0]
refine = optmize_options[1]
cam_calibration = optmize_options[2]
d2r = math.pi / 180.0
r2d = 180.0 / math.pi
# return a 3d affine tranformation between current camera locations
# and original camera locations.
def get_recenter_affine(src_list, dst_list):
print('get_recenter_affine():')
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for i in range(len(src_list)):
src_ned = src_list[i]
src[0].append(src_ned[0])
src[1].append(src_ned[1])
src[2].append(src_ned[2])
src[3].append(1.0)
dst_ned = dst_list[i]
dst[0].append(dst_ned[0])
dst[1].append(dst_ned[1])
dst[2].append(dst_ned[2])
dst[3].append(1.0)
# print("{} <-- {}".format(dst_ned, src_ned))
A = transformations.superimposition_matrix(src, dst, scale=True)
print("A:\n", A)
return A
# transform a point list given an affine transform matrix
def transform_points(A, pts_list):
src = [[], [], [], []]
for p in pts_list:
src[0].append(p[0])
src[1].append(p[1])
src[2].append(p[2])
src[3].append(1.0)
dst = A.dot(np.array(src))
result = []
for i in range(len(pts_list)):
result.append(
[float(dst[0][i]),
float(dst[1][i]),
float(dst[2][i])])
return result
proj = ProjectMgr.ProjectMgr(project_dir)
proj.load_images_info()
source_file = os.path.join(proj.analysis_dir, 'matches_grouped')
print('Match file:', source_file)
matches = pickle.load(open(source_file, "rb"))
print('Match features:', len(matches))
# load the group connections within the image set
groups = Groups.load(proj.analysis_dir)
# sort from smallest to largest: groups.sort(key=len)
opt = Optimizer.Optimizer(project_dir)
opt.setup(proj,
groups,
group_id,
matches,
optimized=refine,
cam_calib=cam_calibration)
cameras, features, cam_index_map, feat_index_map, fx_opt, fy_opt, cu_opt, cv_opt, distCoeffs_opt = opt.run(
)
# mark all the optimized poses as invalid
for image in proj.image_list:
opt_cam_node = image.node.getChild('camera_pose_opt', True)
opt_cam_node.setBool('valid', False)
for i, cam in enumerate(cameras):
image_index = cam_index_map[i]
image = proj.image_list[image_index]
ned_orig, ypr_orig, quat_orig = image.get_camera_pose()
print('optimized cam:', cam)
rvec = cam[0:3]
tvec = cam[3:6]
Rned2cam, jac = cv2.Rodrigues(rvec)
cam2body = image.get_cam2body()
Rned2body = cam2body.dot(Rned2cam)
Rbody2ned = np.matrix(Rned2body).T
(yaw, pitch,
roll) = transformations.euler_from_matrix(Rbody2ned, 'rzyx')
#print "orig ypr =", image.camera_pose['ypr']
#print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
pos = -np.matrix(Rned2cam).T * np.matrix(tvec).T
newned = pos.T[0].tolist()[0]
print(image.name, ned_orig, '->', newned, 'dist:',
np.linalg.norm(np.array(ned_orig) - np.array(newned)))
image.set_camera_pose(newned,
yaw * r2d,
pitch * r2d,
roll * r2d,
opt=True)
image.placed = True
proj.save_images_info()
print('Updated the optimized camera poses.')
# update and save the optimized camera calibration
proj.cam.set_K(fx_opt, fy_opt, cu_opt, cv_opt, optimized=True)
proj.cam.set_dist_coeffs(distCoeffs_opt.tolist(), optimized=True)
proj.save()
# compare original camera locations with optimized camera locations and
# derive a transform matrix to 'best fit' the new camera locations
# over the original ... trusting the original group gps solution as
# our best absolute truth for positioning the system in world
# coordinates.
#
# each optimized group needs a separate/unique fit
matches_opt = list(matches) # shallow copy
refit_group_orientations = True
if refit_group_orientations:
group = groups[group_id]
print('refitting group size:', len(group))
src_list = []
dst_list = []
# only consider images that are in the current group
for name in group:
image = proj.findImageByName(name)
ned, ypr, quat = image.get_camera_pose(opt=True)
src_list.append(ned)
ned, ypr, quat = image.get_camera_pose()
dst_list.append(ned)
A = get_recenter_affine(src_list, dst_list)
# extract the rotation matrix (R) from the affine transform
scale, shear, angles, trans, persp = transformations.decompose_matrix(
A)
print(' scale:', scale)
print(' shear:', shear)
print(' angles:', angles)
print(' translate:', trans)
print(' perspective:', persp)
R = transformations.euler_matrix(*angles)
print("R:\n{}".format(R))
# fixme (just group):
# update the optimized camera locations based on best fit
camera_list = []
# load optimized poses
for image in proj.image_list:
if image.name in group:
ned, ypr, quat = image.get_camera_pose(opt=True)
else:
# this is just fodder to match size/index of the lists
ned, ypr, quat = image.get_camera_pose()
camera_list.append(ned)
# refit
new_cams = transform_points(A, camera_list)
# update position
for i, image in enumerate(proj.image_list):
if not image.name in group:
continue
ned, [y, p, r], quat = image.get_camera_pose(opt=True)
image.set_camera_pose(new_cams[i], y, p, r, opt=True)
proj.save_images_info()
if True:
# update optimized pose orientation.
dist_report = []
for i, image in enumerate(proj.image_list):
if not image.name in group:
continue
ned_orig, ypr_orig, quat_orig = image.get_camera_pose()
ned, ypr, quat = image.get_camera_pose(opt=True)
Rbody2ned = image.get_body2ned(opt=True)
# update the orientation with the same transform to keep
# everything in proper consistent alignment
newRbody2ned = R[:3, :3].dot(Rbody2ned)
(yaw, pitch, roll) = transformations.euler_from_matrix(
newRbody2ned, 'rzyx')
image.set_camera_pose(new_cams[i],
yaw * r2d,
pitch * r2d,
roll * r2d,
opt=True)
dist = np.linalg.norm(
np.array(ned_orig) - np.array(new_cams[i]))
print('image: {}'.format(image.name))
print(' orig pos: {}'.format(ned_orig))
print(' fit pos: {}'.format(new_cams[i]))
print(' dist moved: {}'.format(dist))
dist_report.append((dist, image.name))
proj.save_images_info()
dist_report = sorted(dist_report,
key=lambda fields: fields[0],
reverse=False)
print('Image movement sorted lowest to highest:')
for report in dist_report:
print('{} dist: {}'.format(report[1], report[0]))
# tranform the optimized point locations using the same best
# fit transform for the camera locations.
new_feats = transform_points(A, features)
# update any of the transformed feature locations that have
# membership in the currently processing group back to the
# master match structure. Note we process groups in order of
# little to big so if a match is in more than one group it
# follows the larger group.
for i, feat in enumerate(new_feats):
match_index = feat_index_map[i]
match = matches_opt[match_index]
in_group = False
for m in match[2:]:
if proj.image_list[m[0]].name in group:
in_group = True
break
if in_group:
#print(' before:', match)
match[0] = feat
#print(' after:', match)
else:
# not refitting group orientations, just copy over optimized
# coordinates
for i, feat in enumerate(features):
match_index = feat_index_map[i]
match = matches_opt[match_index]
match[0] = feat
# write out the updated match_dict
print('Updating matches file:', len(matches_opt), 'features')
pickle.dump(matches_opt, open(source_file, 'wb'))
#proj.cam.set_K(fx_opt/scale[0], fy_opt/scale[0], cu_opt/scale[0], cv_opt/scale[0], optimized=True)
#proj.save()
# temp write out just the points so we can plot them with gnuplot
f = open(os.path.join(proj.analysis_dir, 'opt-plot.txt'), 'w')
for m in matches_opt:
try:
f.write('%.2f %.2f %.2f\n' % (m[0][0], m[0][1], m[0][2]))
except:
pass
f.close()
# temp write out direct and optimized camera positions
f1 = open(os.path.join(proj.analysis_dir, 'cams-direct.txt'), 'w')
f2 = open(os.path.join(proj.analysis_dir, 'cams-opt.txt'), 'w')
for name in groups[group_id]:
image = proj.findImageByName(name)
ned1, ypr1, quat1 = image.get_camera_pose()
ned2, ypr2, quat2 = image.get_camera_pose(opt=True)
f1.write('%.2f %.2f %.2f\n' % (ned1[1], ned1[0], -ned1[2]))
f2.write('%.2f %.2f %.2f\n' % (ned2[1], ned2[0], -ned2[2]))
f1.close()
f2.close()