def get_camera_pose_in_world(camera_pose, fat_world_pose=None, type='quat', cam_to_body=None): # print(camera_pose) # this matrix gives world to camera transform camera_pose_matrix = np.zeros((4,4)) # camera_rotation = np.linalg.inv(RT_transform.quat2mat(get_wxyz_quaternion(camera_pose['quaternion_xyzw_worldframe']))) camera_rotation = RT_transform.quat2mat(get_wxyz_quaternion(camera_pose['quaternion_xyzw_worldframe'])) camera_pose_matrix[:3, :3] = camera_rotation camera_location = [i for i in camera_pose['location_worldframe']] # camera_pose_matrix[:, 3] = np.matmul(-1 * camera_rotation, camera_location).tolist() + [1] camera_pose_matrix[:, 3] = camera_location + [1] # # make it camera to world # camera_pose_matrix = np.linalg.inv(camera_pose_matrix) if cam_to_body is not None: camera_pose_matrix = np.matmul(camera_pose_matrix, np.linalg.inv(cam_to_body)) if fat_world_pose is not None: fat_world_matrix = np.zeros((4,4)) fat_world_matrix[:3,:3] = RT_transform.quat2mat(get_wxyz_quaternion(fat_world_pose['quaternion_xyzw'])) fat_world_matrix[:,3] = fat_world_pose['location'] + [1] camera_pose_world = np.matmul(fat_world_matrix, camera_pose_matrix) else: camera_pose_world = camera_pose_matrix # make it camera to world # camera_pose_world = np.linalg.inv(camera_pose_world) if type == 'quat': quat = RT_transform.mat2quat(camera_pose_world[:3, :3]).tolist() return camera_pose_world[:3,3], get_xyzw_quaternion(quat) elif type == 'rot': return camera_pose_world
def get_object_pose_in_world(object_pose, camera_pose, fat_world_pose=None, type='quat'): # Returns in cm object_pose_matrix = np.zeros((4,4)) object_pose_matrix[:3,:3] = RT_transform.quat2mat(get_wxyz_quaternion(object_pose['quaternion_xyzw'])) object_pose_matrix[:,3] = object_pose['location'] + [1] # camera_pose_matrix = np.zeros((4,4)) # camera_pose_matrix[:3, :3] = RT_transform.quat2mat(get_wxyz_quaternion(camera_pose['quaternion_xyzw_worldframe'])) # camera_pose_matrix[:, 3] = camera_pose['location_worldframe'] + [1] camera_pose_matrix = get_camera_pose_in_world(camera_pose, fat_world_pose, type='rot', cam_to_body=None) object_pose_world = np.matmul(camera_pose_matrix, object_pose_matrix) # object_pose_world = np.matmul(np.linalg.inv(camera_pose_matrix), object_pose_matrix) # scale = np.array([[0.01,0,0,0],[0,0.01,0,0],[0,0,0.01,0],[0,0,0,1]]) # object_pose_world = np.matmul(scale, object_pose_world) if fat_world_pose is not None: fat_world_matrix = np.zeros((4,4)) fat_world_matrix[:3,:3] = RT_transform.quat2mat(get_wxyz_quaternion(fat_world_pose['quaternion_xyzw'])) fat_world_matrix[:,3] = fat_world_pose['location'] + [1] object_pose_world = np.matmul(fat_world_matrix, object_pose_world) # print(object_pose_world) if type == 'quat': quat = RT_transform.mat2quat(object_pose_world[:3, :3]).tolist() return object_pose_world[:3,3].tolist(), get_xyzw_quaternion(quat)
def gen_gt_observed(): for cls_idx, cls_name in idx2class.items(): print(cls_idx, cls_name) # uncomment here to only generate data for ape # if cls_name != 'ape': # continue with open( os.path.join(observed_set_dir, "{}_all.txt".format(cls_name)), "r") as f: all_indices = [line.strip("\r\n") for line in f.readlines()] # render machine model_dir = os.path.join(LM6d_root, "models", cls_name) render_machine = Render_Py(model_dir, K, width, height, ZNEAR, ZFAR) for observed_idx in tqdm(all_indices): video_name, prefix = observed_idx.split("/") # read pose ------------------------------------- observed_meta_path = os.path.join( observed_data_root, "{}-meta.mat".format(observed_idx)) meta_data = sio.loadmat(observed_meta_path) inner_id = np.where( np.squeeze(meta_data["cls_indexes"]) == cls_idx) if len(meta_data["poses"].shape) == 2: pose = meta_data["poses"] else: pose = np.squeeze(meta_data["poses"][:, :, inner_id]) new_pose_path = os.path.join(gt_observed_root, cls_name, "{}-pose.txt".format(prefix)) mkdir_if_missing(os.path.join(gt_observed_root, cls_name)) # write pose write_pose_file(new_pose_path, cls_idx, pose) # ----------------------render color, depth ------------ rgb_gl, depth_gl = render_machine.render( RT_transform.mat2quat(pose[:3, :3]), pose[:, -1]) if any([x in observed_idx for x in ["000128", "000256", "000512"]]): rgb_gl = rgb_gl.astype("uint8") render_color_path = os.path.join(gt_observed_root, cls_name, "{}-color.png".format(prefix)) cv2.imwrite(render_color_path, rgb_gl) # depth depth_save = depth_gl * DEPTH_FACTOR depth_save = depth_save.astype("uint16") render_depth_path = os.path.join(gt_observed_root, cls_name, "{}-depth.png".format(prefix)) cv2.imwrite(render_depth_path, depth_save) # --------------------- render label ---------------------------------- render_label = depth_gl != 0 render_label = render_label.astype("uint8") # write label label_path = os.path.join(gt_observed_root, cls_name, "{}-label.png".format(prefix)) cv2.imwrite(label_path, render_label)
def gen_render_real(): for cls_idx, cls_name in idx2class.items(): print(cls_idx, cls_name) if cls_name == 'driller': continue with open( os.path.join(real_set_dir, 'occLM_val_real_{}.txt'.format(cls_name)), 'r') as f: all_indices = [line.strip('\r\n') for line in f.readlines()] # render machine model_dir = os.path.join(LM6d_root, 'models', cls_name) render_machine = Render_Py(model_dir, K, width, height, ZNEAR, ZFAR) for real_idx in tqdm(all_indices): video_name, prefix = real_idx.split('/') # video name is "test" # read pose ------------------------------------- real_meta_path = os.path.join(real_data_root, "02/{}-meta.mat".format(prefix)) meta_data = sio.loadmat(real_meta_path) inner_id = np.where( np.squeeze(meta_data['cls_indexes']) == cls_idx) if len(meta_data['poses'].shape) == 2: pose = meta_data['poses'] else: pose = np.squeeze(meta_data['poses'][:, :, inner_id]) new_pose_path = os.path.join(render_real_root, cls_name, "{}-pose.txt".format(prefix)) mkdir_if_missing(os.path.join(render_real_root, cls_name)) # write pose write_pose_file(new_pose_path, cls_idx, pose) # ----------------------render color, depth ------------ rgb_gl, depth_gl = render_machine.render( RT_transform.mat2quat(pose[:3, :3]), pose[:, -1]) rgb_gl = rgb_gl.astype('uint8') render_color_path = os.path.join(render_real_root, cls_name, "{}-color.png".format(prefix)) cv2.imwrite(render_color_path, rgb_gl) # depth depth_save = depth_gl * DEPTH_FACTOR depth_save = depth_save.astype('uint16') render_depth_path = os.path.join(render_real_root, cls_name, "{}-depth.png".format(prefix)) cv2.imwrite(render_depth_path, depth_save) #--------------------- render label ---------------------------------- render_label = depth_gl != 0 render_label = render_label.astype('uint8') # write label label_path = os.path.join(render_real_root, cls_name, "{}-label.png".format(prefix)) cv2.imwrite(label_path, render_label)
def render_pose(rendered_dir, count, class_name, fixed_transforms_dict, camera_intrinsics, camera_pose, rotation_angles, location, rotation): width = 960 height = 540 K = np.array([[camera_intrinsics['fx'], 0, camera_intrinsics['cx']], [0, camera_intrinsics['fy'], camera_intrinsics['cy']], [0, 0, 1]]) # Check these TODO ZNEAR = 0.1 ZFAR = 20 depth_factor = 1000 model_dir = os.path.join(LM6d_root, "models", class_name) # model_dir = os.path.join(LM6d_root, "aligned_cm", class_name, "google_16k") render_machine = Render_Py(model_dir, K, width, height, ZNEAR, ZFAR) # camera_pose_matrix = np.zeros((4,4)) # camera_pose_matrix[:, 3] = [i/100 for i in camera_pose['location_worldframe']] + [1] # camera_pose_matrix[:3, :3] = RT_transform.quat2mat(get_wxyz_quaternion(camera_pose['quaternion_xyzw_worldframe'])) # camera_pose_matrix[3,:3] = [0,0,0] # print(camera_pose_matrix) fixed_transform = np.transpose(np.array(fixed_transforms_dict[class_name])) fixed_transform[:3, 3] = [i / 100 for i in fixed_transform[:3, 3]] object_world_transform = np.zeros((4, 4)) object_world_transform[:3, :3] = RT_transform.euler2mat( rotation_angles[0], rotation_angles[1], rotation_angles[2]) # object_world_transform[:3,:3] = RT_transform.euler2mat(0,0,0) # object_world_transform[:3, :3] = RT_transform.quat2mat(get_wxyz_quaternion(rotation)) object_world_transform[:, 3] = [i / 100 for i in location] + [1] # print(fixed_transform) # total_transform = np.matmul(np.linalg.inv(camera_pose_matrix), object_world_transform) # fixed_transform = np.matmul(m, fixed_transform) total_transform = np.matmul(object_world_transform, fixed_transform) # total_transform = object_world_transform pose_rendered_q = RT_transform.mat2quat(total_transform[:3, :3]).tolist( ) + total_transform[:3, 3].flatten().tolist() # pose_rendered_q = RT_transform.mat2quat(object_world_transform[:3,:3]).tolist() + object_world_transform[:3,3].flatten().tolist() # print(pose_rendered_q) # rendered_dir = '.' # image_file = os.path.join( # rendered_dir, # "{}-{}-color.png".format(count, class_name), # ) # depth_file = os.path.join( # rendered_dir, # "{}-{}-depth.png".format(count, class_name), # ) rgb_gl, depth_gl = render_machine.render(pose_rendered_q[:4], np.array(pose_rendered_q[4:])) rgb_gl = rgb_gl.astype("uint8") depth_gl = (depth_gl * depth_factor).astype(np.uint16) return rgb_gl, depth_gl
def flow2se3(depth_object, flow, mask_image, K): """ give flow from object to image, calculate the pose :param depth_object: height x width, ndarray the depth map of object image. :param flow: height x width x (w, h) flow from object image to real image :param mask_image: height x width, the mask of real image :param K: 3x3 intrinsic matrix :return: se3: 3x4 matrix. """ height = depth_object.shape[0] width = depth_object.shape[1] assert mask_image.shape == (height, width) valid_in_object = (depth_object != 0).flatten() all_op = backproject_camera(depth_object, intrinsic_matrix=K) # all_op = all_op.reshape((3, width, height)) x, y = np.meshgrid(np.arange(width), np.arange(height)) x = x.astype(np.float64) y = y.astype(np.float64) x += flow[:, :, 0] y += flow[:, :, 1] x = x.flatten() y = y.flatten() all_ip = np.vstack((x, y)) valid_in_image = (mask_image != 0).flatten() valid = np.where(np.logical_and(valid_in_object, valid_in_image))[0] objectPoints = all_op[:, valid].astype(np.float64).transpose() imagePoints = all_ip[:, valid].astype(np.float64).transpose() convex, rvec, tvec, inliers = cv2.solvePnPRansac(objectPoints, imagePoints, K, np.zeros(4)) se3_q = np.zeros(7) if convex: R, _ = cv2.Rodrigues(rvec) se3_q[:4] = RT_transform.mat2quat(R) se3_q[4:] = tvec.flatten() return convex, se3_q else: se3_q[0] = 1 return convex, se3_q
def stat_poses(): pz = np.array([0, 0, 1]) new_points = {} pose_dict = {} trans_stat = {} quat_stat = {} for cls_idx, cls_name in idx2class.items(): # uncomment here to only generate data for ape # if cls_name != 'ape': # continue new_points[cls_name] = {'pz': []} train_idx_file = os.path.join(observed_set_dir, "{}_train.txt".format(cls_name)) with open(train_idx_file, 'r') as f: observed_indices = [line.strip() for line in f.readlines()] num_observed = len(observed_indices) pose_dict[cls_name] = np.zeros((num_observed, 7)) # quat, translation trans_stat[cls_name] = {} quat_stat[cls_name] = {} for observed_i, observed_idx in enumerate(tqdm(observed_indices)): prefix = observed_idx.split('/')[1] pose_path = os.path.join(gt_observed_dir, cls_name, '{}-pose.txt'.format(prefix)) assert os.path.exists(pose_path), 'path {} not exists'.format( pose_path) pose = np.loadtxt(pose_path, skiprows=1) rot = pose[:3, :3] # print(rot) quat = np.squeeze(se3.mat2quat(rot)) src_trans = pose[:3, 3] pose_dict[cls_name][observed_i, :4] = quat pose_dict[cls_name][observed_i, 4:] = src_trans new_pz = np.dot(rot, pz.reshape((-1, 1))).reshape((3, )) new_points[cls_name]['pz'].append(new_pz) new_points[cls_name]['pz'] = np.array(new_points[cls_name]['pz']) new_points[cls_name]['pz_mean'] = np.mean(new_points[cls_name]['pz'], 0) new_points[cls_name]['pz_std'] = np.std(new_points[cls_name]['pz'], 0) trans_mean = np.mean(pose_dict[cls_name][:, 4:], 0) trans_std = np.std(pose_dict[cls_name][:, 4:], 0) trans_stat[cls_name]['trans_mean'] = trans_mean trans_stat[cls_name]['trans_std'] = trans_std quat_mean = np.mean(pose_dict[cls_name][:, :4], 0) quat_std = np.std(pose_dict[cls_name][:, :4], 0) quat_stat[cls_name]['quat_mean'] = quat_mean quat_stat[cls_name]['quat_std'] = quat_std print('new z: ', 'mean: ', new_points[cls_name]['pz_mean'], 'std: ', new_points[cls_name]['pz_std']) new_points[cls_name]['angle'] = [ ] # angle between mean vector and points pz_mean = new_points[cls_name]['pz_mean'] for p_i in range(new_points[cls_name]['pz'].shape[0]): deg = angle(pz_mean, new_points[cls_name]['pz'][p_i, :]) new_points[cls_name]['angle'].append(deg) new_points[cls_name]['angle'] = np.array(new_points[cls_name]['angle']) print('angle mean: ', np.mean(new_points[cls_name]['angle']), 'angle std: ', np.std(new_points[cls_name]['angle']), 'angle max: ', np.max(new_points[cls_name]['angle'])) new_points[cls_name]['angle_max'] = np.max( new_points[cls_name]['angle']) ############### print() def vis_points(): import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D sel_p = 'pz' ax = plt.figure().add_subplot(111, projection='3d') ax.scatter(new_points[cls_name][sel_p][:, 0], new_points[cls_name][sel_p][:, 1], new_points[cls_name][sel_p][:, 2], c='r', marker='^') ax.scatter(0, 0, 0, c='b', marker='o') ax.scatter(0, 0, 1, c='b', marker='o') ax.scatter(0, 1, 0, c='b', marker='o') ax.scatter(1, 0, 0, c='b', marker='o') ax.quiver(0, 0, 0, 0, 0, 1) pz_mean = new_points[cls_name]['pz_mean'] ax.quiver(0, 0, 0, pz_mean[0], pz_mean[1], pz_mean[2]) ax.scatter(pz_mean[0], pz_mean[1], pz_mean[2], c='b', marker='o') ax.set_xlabel('X Label') ax.set_ylabel('Y Label') ax.set_zlabel('Z Label') ax.set_xlim([-1.5, 1.5]) ax.set_ylim([-1.5, 1.5]) ax.set_zlim([-1.5, 1.5]) plt.title(cls_name + '-' + sel_p) plt.show() # vis_points() return pose_dict, quat_stat, trans_stat, new_points
def gen_observed(): # output path observed_root_dir = os.path.join(LINEMOD_syn_root, "data", "observed") image_set_dir = os.path.join(LINEMOD_syn_root, "image_set") mkdir_if_missing(observed_root_dir) mkdir_if_missing(image_set_dir) syn_poses_path = os.path.join(observed_pose_dir, "LM6d_ds_train_observed_pose_all.pkl") with open(syn_poses_path, "rb") as f: syn_pose_dict = cPickle.load(f) for class_idx, class_name in enumerate(classes): if class_name == "__back_ground__": continue # uncomment here to only generate data for ape # if class_name not in ['ape']: # continue # init render machines brightness_ratios = [0.2, 0.25, 0.3, 0.35, 0.4] model_dir = os.path.join(LINEMOD_syn_root, "models", class_name) render_machine = Render_Py_Light(model_dir, K, width, height, ZNEAR, ZFAR, brightness_ratios) syn_poses = syn_pose_dict[class_name] num_poses = syn_poses.shape[0] observed_index_list = [ "{}/{:06d}".format(class_name, i + 1) for i in range(num_poses) ] observed_set_path = os.path.join( image_set_dir, "observed/LM6d_data_syn_train_observed_{}.txt".format(class_name)) mkdir_if_missing(os.path.join(image_set_dir, "observed")) f_observed_set = open(observed_set_path, "w") for idx, observed_index in enumerate(tqdm(observed_index_list)): f_observed_set.write("{}\n".format(observed_index)) prefix = observed_index.split("/")[1] observed_dir = os.path.join(observed_root_dir, class_name) mkdir_if_missing(observed_dir) observed_color_file = os.path.join(observed_dir, prefix + "-color.png") observed_depth_file = os.path.join(observed_dir, prefix + "-depth.png") observed_pose_file = os.path.join(observed_dir, prefix + "-pose.txt") observed_label_file = os.path.join(observed_dir, prefix + "-label.png") pose_quat = syn_poses[idx, :] pose = se3.se3_q2m(pose_quat) # generate random light_position if idx % 6 == 0: light_position = [1, 0, 1] elif idx % 6 == 1: light_position = [1, 1, 1] elif idx % 6 == 2: light_position = [0, 1, 1] elif idx % 6 == 3: light_position = [-1, 1, 1] elif idx % 6 == 4: light_position = [-1, 0, 1] elif idx % 6 == 5: light_position = [0, 0, 1] else: raise Exception("???") light_position = np.array(light_position) * 0.5 # inverse yz light_position[0] += pose[0, 3] light_position[1] -= pose[1, 3] light_position[2] -= pose[2, 3] # randomly adjust color and intensity for light_intensity colors = np.array([[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0], [1, 1, 1]]) intensity = np.random.uniform(0.9, 1.1, size=(3, )) colors_randk = random.randint(0, colors.shape[0] - 1) light_intensity = colors[colors_randk] * intensity # randomly choose a render machine rm_randk = random.randint(0, len(brightness_ratios) - 1) # get render result rgb_gl, depth_gl = render_machine.render(se3.mat2quat( pose[:3, :3]), pose[:, -1], light_position, light_intensity, brightness_k=rm_randk) rgb_gl = rgb_gl.astype("uint8") # gt_observed label label_gl = np.zeros(depth_gl.shape) # print('depth gl:', depth_gl.shape) label_gl[depth_gl != 0] = 1 cv2.imwrite(observed_color_file, rgb_gl) depth_gl = (depth_gl * depth_factor).astype(np.uint16) cv2.imwrite(observed_depth_file, depth_gl) cv2.imwrite(observed_label_file, label_gl) text_file = open(observed_pose_file, "w") text_file.write("{}\n".format(class_idx)) pose_str = "{} {} {} {}\n{} {} {} {}\n{} {} {} {}".format( pose[0, 0], pose[0, 1], pose[0, 2], pose[0, 3], pose[1, 0], pose[1, 1], pose[1, 2], pose[1, 3], pose[2, 0], pose[2, 1], pose[2, 2], pose[2, 3], ) text_file.write(pose_str) print(class_name, " done")
yu_pred = sio.loadmat(yu_pred_file) rois = yu_pred['rois'] if len(rois) != 0: labels = rois[:, 0] # 1: found; -1: not found if labels != -1: try: meta_data = sio.loadmat( real_meta_path.format(real_index)) except: raise Exception(real_index) proposal_idx = np.where(labels == 1) assert len(proposal_idx) == 1 pose_ori_q = yu_pred['poses'][proposal_idx].reshape(7) pose_ori_m = RT_transform.se3_q2m(pose_ori_q) pose_ori_q[:4] = RT_transform.mat2quat(pose_ori_m[:, :3]) pose_gt = meta_data['poses'] if len(pose_gt.shape) > 2: pose_gt = pose_gt[:, :, list(meta_data['cls_indexes'][0]). index(big_class_idx)] print("{}, {:04d}, {}".format( class_name, idx, RT_transform.calc_rt_dist_m(pose_ori_m, pose_gt))) pose_ori_file = os.path.join( rendered_dir, '{}_{}_{}-pose.txt'.format(class_name, real_prefix_list[idx], 0)) image_file = os.path.join(
def stat_poses(): pz = np.array([0, 0, 1]) new_points = {} pose_dict = {} trans_stat = {} quat_stat = {} for cls_idx, cls_name in idx2class.items(): # uncomment here to only generate data for ape # if cls_name != 'ape': # continue new_points[cls_name] = {"pz": []} train_idx_file = os.path.join(observed_set_dir, "{}_train.txt".format(cls_name)) with open(train_idx_file, "r") as f: observed_indices = [line.strip() for line in f.readlines()] num_observed = len(observed_indices) pose_dict[cls_name] = np.zeros((num_observed, 7)) # quat, translation trans_stat[cls_name] = {} quat_stat[cls_name] = {} for observed_i, observed_idx in enumerate(tqdm(observed_indices)): prefix = observed_idx.split("/")[1] pose_path = os.path.join(gt_observed_dir, cls_name, "{}-pose.txt".format(prefix)) assert os.path.exists(pose_path), "path {} not exists".format(pose_path) pose = np.loadtxt(pose_path, skiprows=1) rot = pose[:3, :3] # print(rot) quat = np.squeeze(se3.mat2quat(rot)) src_trans = pose[:3, 3] pose_dict[cls_name][observed_i, :4] = quat pose_dict[cls_name][observed_i, 4:] = src_trans new_pz = np.dot(rot, pz.reshape((-1, 1))).reshape((3,)) new_points[cls_name]["pz"].append(new_pz) new_points[cls_name]["pz"] = np.array(new_points[cls_name]["pz"]) new_points[cls_name]["pz_mean"] = np.mean(new_points[cls_name]["pz"], 0) new_points[cls_name]["pz_std"] = np.std(new_points[cls_name]["pz"], 0) trans_mean = np.mean(pose_dict[cls_name][:, 4:], 0) trans_std = np.std(pose_dict[cls_name][:, 4:], 0) trans_stat[cls_name]["trans_mean"] = trans_mean trans_stat[cls_name]["trans_std"] = trans_std quat_mean = np.mean(pose_dict[cls_name][:, :4], 0) quat_std = np.std(pose_dict[cls_name][:, :4], 0) quat_stat[cls_name]["quat_mean"] = quat_mean quat_stat[cls_name]["quat_std"] = quat_std print("new z: ", "mean: ", new_points[cls_name]["pz_mean"], "std: ", new_points[cls_name]["pz_std"]) new_points[cls_name]["angle"] = [] # angle between mean vector and points pz_mean = new_points[cls_name]["pz_mean"] for p_i in range(new_points[cls_name]["pz"].shape[0]): deg = angle(pz_mean, new_points[cls_name]["pz"][p_i, :]) new_points[cls_name]["angle"].append(deg) new_points[cls_name]["angle"] = np.array(new_points[cls_name]["angle"]) print( "angle mean: ", np.mean(new_points[cls_name]["angle"]), "angle std: ", np.std(new_points[cls_name]["angle"]), "angle max: ", np.max(new_points[cls_name]["angle"]), ) new_points[cls_name]["angle_max"] = np.max(new_points[cls_name]["angle"]) print() def vis_points(): import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # noqa:F401 sel_p = "pz" ax = plt.figure().add_subplot(111, projection="3d") ax.scatter( new_points[cls_name][sel_p][:, 0], new_points[cls_name][sel_p][:, 1], new_points[cls_name][sel_p][:, 2], c="r", marker="^", ) ax.scatter(0, 0, 0, c="b", marker="o") ax.scatter(0, 0, 1, c="b", marker="o") ax.scatter(0, 1, 0, c="b", marker="o") ax.scatter(1, 0, 0, c="b", marker="o") ax.quiver(0, 0, 0, 0, 0, 1) pz_mean = new_points[cls_name]["pz_mean"] ax.quiver(0, 0, 0, pz_mean[0], pz_mean[1], pz_mean[2]) ax.scatter(pz_mean[0], pz_mean[1], pz_mean[2], c="b", marker="o") ax.set_xlabel("X Label") ax.set_ylabel("Y Label") ax.set_zlabel("Z Label") ax.set_xlim([-1.5, 1.5]) ax.set_ylim([-1.5, 1.5]) ax.set_zlim([-1.5, 1.5]) plt.title(cls_name + "-" + sel_p) plt.show() # vis_points() return pose_dict, quat_stat, trans_stat, new_points
inner_id = np.where(np.squeeze(meta_data['cls_indexes']) == class_idx) if len(meta_data['poses'].shape) == 2: pose = meta_data['poses'] else: pose = np.squeeze(meta_data['poses'][:, :, inner_id]) # adjust light position according to pose light_position = np.array(light_position) * 0.5 # inverse yz light_position[0] += pose[0, 3] light_position[1] -= pose[1, 3] light_position[2] -= pose[2, 3] rgb_gl, depth_gl = render_machine.render(RT_transform.mat2quat(pose[:3, :3]), pose[:, -1], light_position, light_intensity, class_name=cls_name, brightness_k=rm_randk) rgb_dict[class_idx] = rgb_gl real_color_img = cv2.imread(real_color_file, cv2.IMREAD_COLOR) import matplotlib.pyplot as plt fig = plt.figure() plt.axis('off') fig.add_subplot(1, 3, 1) plt.imshow(rgb_gl[:, :, [2, 1, 0]])
def main(camera_params, env_params): width = camera_params['camera_width'] height = camera_params['camera_height'] K = np.array([[camera_params['camera_fx'], 0, camera_params['camera_cx']], [0, camera_params['camera_fy'], camera_params['camera_cy']], [0, 0, 1]]) ZNEAR = camera_params['camera_znear'] ZFAR = camera_params['camera_zfar'] depth_factor = 1000 x_min = float(env_params['x_min']) x_max = float(env_params['x_max']); y_min = float(env_params['y_min']); y_max = float(env_params['y_max']); table_height = float(env_params['table_height']); gen_images = True pose_from_file = False print("Camera Matrix:") print(K) # camera_pose = np.array([ \ # [0.868216, 6.3268e-06, 0.496186, 0.436202], \ # [-5.49302e-06, 1, -3.13929e-06, 0.0174911], \ # [-0.496186, 2.74908e-11, 0.868216, 0.709983], \ # [0, 0, 0, 1]]) # Camera to world transform # camera_pose = np.array([ \ # [0.0068906 , -0.497786, 0.867272 , 0.435696], \ # [-0.999953, 0.0024452, 0.00934823, 0.0323318], \ # [-0.00677407, -0.867296, -0.497746, 0.710332], \ # [0, 0, 0, 1]]) camera_pose = np.array([ \ [0.00572327, -0.629604, 0.776895, 0.437408], \ [-0.999953, 0.00244603, 0.0093488, 0.0323317], \ [-0.00778635, -0.776912, -0.629561, 0.709281], \ [0, 0, 0, 1]]) # # camera_pose = np.array([ \ # [0.778076, 6.3268e-06, 0.628171, 0.43785], \ # [-4.92271e-06, 1, -3.97433e-06, 0.0174995], \ # [ -0.628171, 2.70497e-11, 0.778076, 0.708856], \ # [ 0, 0, 0, 1]]) # cam_to_body = np.array([[ 0, 0, 1, 0], # [-1, 0, 0, 0], # [0, -1, 0, 0], # [0, 0, 0, 1]]); for class_idx, class_name in idx2class.items(): print("start ", class_idx, class_name) if class_name in ["__back_ground__"]: continue if gen_images: # init render # model_dir = os.path.join(LM6d_root, "aligned_cm", class_name, "google_16k") model_dir = os.path.join(LM6d_root, "models", class_name) render_machine = Render_Py(model_dir, K, width, height, ZNEAR, ZFAR) for set_type in ["all"]: rendered_pose_list = [] # For reading in Perch rendered_pose_list_out = [] if pose_from_file: with open(rendered_pose_path.format(set_type, class_name)) as f: str_rendered_pose_list = [x.strip().split(" ") for x in f.readlines()] rendered_pose_list = np.array( [[float(x) for x in each_pose] for each_pose in str_rendered_pose_list] ) else: for x in np.arange(x_min, x_max, float(env_params['search_resolution_translation'])): for y in np.arange(y_min, y_max, float(env_params['search_resolution_translation'])): for theta in np.arange(0, 2 * np.pi, float(env_params['search_resolution_yaw'])): original_point = np.array([[x], [y], [table_height], [1]]) if class_name == "004_sugar_box": # Add half the height of box to shift it up point = np.array([[x], [y], [table_height+0.086], [1]]) if class_name == "035_power_drill": point = np.array([[x], [y], [table_height], [1]]) # transformed_point = np.matmul(np.linalg.inv(camera_pose), point) # transformed_rotation = np.matmul(np.linalg.inv(camera_pose[0:3, 0:3]), RT_transform.euler2mat(0,0,theta)) # transformed_rotation = np.linalg.inv(camera_pose)[0:3, 0:3] # transformed_rotation = RT_transform.euler2mat(0,0,0) # print(transformed_point) object_world_transform = np.zeros((4,4)) if class_name == "004_sugar_box": object_world_transform[:3,:3] = RT_transform.euler2mat(0,0,theta) if class_name == "035_power_drill": object_world_transform[:3,:3] = RT_transform.euler2mat(np.pi/2,0,theta) object_world_transform[:4,3] = point.flatten() # print(world_object_transform) # First apply world to object transform on the object and then take it to camera frame total_transform = np.matmul(np.linalg.inv(camera_pose), object_world_transform) print(total_transform) pose = RT_transform.mat2quat(total_transform[:3,:3]).tolist() + total_transform[:3,3].flatten().tolist() # pose = RT_transform.mat2quat(transformed_rotation).tolist() + transformed_point.flatten()[0:3].tolist() print(pose) rendered_pose_list.append(pose) # rendered_pose_list_out.append(point.flatten().tolist() + [0,0,theta]) rendered_pose_list_out.append(original_point.flatten().tolist() + [0,0,theta]) rendered_pose_list = np.array(rendered_pose_list) rendered_pose_list_out = np.array(rendered_pose_list_out) for idx, observed_pose in enumerate(tqdm(rendered_pose_list)): # print(idx) # print(observed_pose) rendered_dir = os.path.join(rendered_root_dir, class_name) mkdir_if_missing(rendered_dir) if gen_images: image_file = os.path.join( rendered_dir, "{}-color.png".format(idx), ) depth_file = os.path.join( rendered_dir, "{}-depth.png".format(idx), ) pose_rendered_q = observed_pose # print(pose_rendered_q[4:]) rgb_gl, depth_gl = render_machine.render( pose_rendered_q[:4], pose_rendered_q[4:] ) rgb_gl = rgb_gl.astype("uint8") depth_gl = (depth_gl * depth_factor).astype(np.uint16) cv2.imwrite(image_file, rgb_gl) cv2.imwrite(depth_file, depth_gl) # pose_rendered_file = os.path.join( # rendered_dir, # "{}-pose.txt".format(idx), # ) # text_file = open(pose_rendered_file, "w") # text_file.write("{}\n".format(class_idx)) # pose_rendered_m = np.zeros((3, 4)) # pose_rendered_m[:, :3] = RT_transform.quat2mat( # pose_rendered_q[:4] # ) # pose_rendered_m[:, 3] = pose_rendered_q[4:] # pose_ori_m = pose_rendered_m # pose_str = "{} {} {} {}\n{} {} {} {}\n{} {} {} {}".format( # pose_ori_m[0, 0], # pose_ori_m[0, 1], # pose_ori_m[0, 2], # pose_ori_m[0, 3], # pose_ori_m[1, 0], # pose_ori_m[1, 1], # pose_ori_m[1, 2], # pose_ori_m[1, 3], # pose_ori_m[2, 0], # pose_ori_m[2, 1], # pose_ori_m[2, 2], # pose_ori_m[2, 3], # ) # text_file.write(pose_str) pose_rendered_file = os.path.join( rendered_dir, "poses.txt", ) np.savetxt(pose_rendered_file, np.around(rendered_pose_list_out, 4)) # text_file = open(pose_rendered_file, "w") # text_file.write(rendered_pose_list) print(class_name, " done")
def main(): for cls_idx, cls_name in enumerate(tqdm(sel_classes)): print(cls_idx, cls_name) keyframe_path = os.path.join( observed_set_dir, "train_observed_{}.txt".format(cls_name) ) with open(keyframe_path) as f: observed_index_list = [x.strip() for x in f.readlines()] video_name_list = [x.split("/")[0] for x in observed_index_list] observed_prefix_list = [x.split("/")[1] for x in observed_index_list] # init renderer model_dir = os.path.join(model_root, cls_name) render_machine = Render_Py(model_dir, K, width, height, ZNEAR, ZFAR) for idx, observed_index in enumerate(tqdm(observed_index_list)): prefix = observed_prefix_list[idx] video_name = video_name_list[idx] gt_observed_dir = os.path.join(gt_observed_root_dir, cls_name) mkdir_if_missing(gt_observed_dir) gt_observed_dir = os.path.join(gt_observed_dir, video_name) # ./ mkdir_if_missing(gt_observed_dir) # to be written gt_observed_color_file = os.path.join( gt_observed_dir, prefix + "-color.png" ) gt_observed_depth_file = os.path.join( gt_observed_dir, prefix + "-depth.png" ) gt_observed_pose_file = os.path.join(gt_observed_dir, prefix + "-pose.txt") gt_observed_label_file = os.path.join( gt_observed_dir, prefix + "-label.png" ) observed_pose_file = os.path.join( observed_root_dir, video_name, prefix + "-poses.npy" ) observed_poses = np.load(observed_pose_file) observed_pose_dict = observed_poses.all() # pprint(observed_pose_dict) if cls_name not in observed_pose_dict.keys(): continue pose = observed_pose_dict[cls_name] rgb_gl, depth_gl = render_machine.render( RT_transform.mat2quat(pose[:3, :3]), pose[:, -1] ) rgb_gl = rgb_gl.astype("uint8") label_gl = np.zeros(depth_gl.shape) label_gl[depth_gl != 0] = 1 depth_gl = depth_gl * depth_factor depth_gl = depth_gl.astype("uint16") # write results cv2.imwrite(gt_observed_color_file, rgb_gl) cv2.imwrite(gt_observed_depth_file, depth_gl) cv2.imwrite(gt_observed_label_file, label_gl) text_file = open(gt_observed_pose_file, "w") text_file.write("{}\n".format(cls_idx)) pose_str = "{} {} {} {}\n{} {} {} {}\n{} {} {} {}".format( pose[0, 0], pose[0, 1], pose[0, 2], pose[0, 3], pose[1, 0], pose[1, 1], pose[1, 2], pose[1, 3], pose[2, 0], pose[2, 1], pose[2, 2], pose[2, 3], ) text_file.write(pose_str) print(cls_name, " done")
def gen_real(): syn_poses_dir = os.path.join( cur_path, '../data/LINEMOD_6D/LM6d_converted/LM6d_render_v1/syn_poses_single/') # output path real_root_dir = os.path.join(LINEMOD_root, 'LM6d_data_syn_light', 'data', 'real') image_set_dir = os.path.join(LINEMOD_root, 'LM6d_data_syn_light/image_set') mkdir_if_missing(real_root_dir) mkdir_if_missing(image_set_dir) syn_poses_path = os.path.join(syn_poses_dir, 'LM6d_ds_v1_all_syn_pose.pkl') with open(syn_poses_path, 'rb') as f: syn_pose_dict = cPickle.load(f) for class_idx, class_name in enumerate(tqdm(classes)): if class_name == '__back_ground__': continue if class_name not in ['ape']: continue # init render machines brightness_ratios = [0.2, 0.25, 0.3, 0.35, 0.4] ################### model_dir = os.path.join(LINEMOD_root, 'models', class_name) render_machine = Render_Py_Light(model_dir, K, width, height, ZNEAR, ZFAR, brightness_ratios) # syn_poses_path = os.path.join(syn_poses_dir, 'LM6d_v1_all_rendered_pose_{}.txt'.format(class_name)) # syn_poses = np.loadtxt(syn_poses_path) # print(syn_poses.shape) # nx7 syn_poses = syn_pose_dict[class_name] num_poses = syn_poses.shape[0] real_index_list = [ '{}/{:06d}'.format(class_name, i + 1) for i in range(num_poses) ] real_set_path = os.path.join( image_set_dir, 'real/LM6d_data_syn_train_real_{}.txt'.format(class_name)) mkdir_if_missing(os.path.join(image_set_dir, 'real')) f_real_set = open(real_set_path, 'w') all_pair = [] for idx, real_index in enumerate(real_index_list): f_real_set.write('{}\n'.format(real_index)) # continue # just generate real set file prefix = real_index.split('/')[1] video_name = real_index.split('/')[0] real_dir = os.path.join(real_root_dir, class_name) mkdir_if_missing(real_dir) real_color_file = os.path.join(real_dir, prefix + "-color.png") real_depth_file = os.path.join(real_dir, prefix + "-depth.png") real_pose_file = os.path.join(real_dir, prefix + "-pose.txt") # real_label_file = os.path.join(real_root_dir, video_name, prefix + "-label.png") real_label_file = os.path.join(real_dir, prefix + "-label.png") if idx % 500 == 0: print(' ', class_name, idx, '/', len(real_index_list), ' ', real_index) pose_quat = syn_poses[idx, :] pose = se3.se3_q2m(pose_quat) # generate random light_position if idx % 6 == 0: light_position = [1, 0, 1] elif idx % 6 == 1: light_position = [1, 1, 1] elif idx % 6 == 2: light_position = [0, 1, 1] elif idx % 6 == 3: light_position = [-1, 1, 1] elif idx % 6 == 4: light_position = [-1, 0, 1] elif idx % 6 == 5: light_position = [0, 0, 1] else: raise Exception("???") # print( "light_position a: {}".format(light_position)) light_position = np.array(light_position) * 0.5 # inverse yz light_position[0] += pose[0, 3] light_position[1] -= pose[1, 3] light_position[2] -= pose[2, 3] # print("light_position b: {}".format(light_position)) # randomly adjust color and intensity for light_intensity colors = np.array([[0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0], [1, 1, 1]]) intensity = np.random.uniform(0.9, 1.1, size=(3, )) colors_randk = random.randint(0, colors.shape[0] - 1) light_intensity = colors[colors_randk] * intensity # print('light intensity: ', light_intensity) # randomly choose a render machine rm_randk = random.randint(0, len(brightness_ratios) - 1) # print('brightness ratio:', brightness_ratios[rm_randk]) # get render result rgb_gl, depth_gl = render_machine.render(se3.mat2quat( pose[:3, :3]), pose[:, -1], light_position, light_intensity, brightness_k=rm_randk) rgb_gl = rgb_gl.astype('uint8') # render_real label label_gl = np.zeros(depth_gl.shape) # print('depth gl:', depth_gl.shape) label_gl[depth_gl != 0] = 1 # import matplotlib.pyplot as plt # fig = plt.figure() # plt.axis('off') # fig.add_subplot(1, 3, 1) # plt.imshow(rgb_gl[:, :, [2,1,0]]) # # fig.add_subplot(1, 3, 2) # plt.imshow(depth_gl) # # fig.add_subplot(1, 3, 3) # plt.imshow(label_gl) # # fig.suptitle('light position: {}\n light_intensity: {}\n brightness: {}'.format(light_position, light_intensity, brightness_ratios[rm_randk])) # plt.show() cv2.imwrite(real_color_file, rgb_gl) depth_gl = (depth_gl * depth_factor).astype(np.uint16) cv2.imwrite(real_depth_file, depth_gl) cv2.imwrite(real_label_file, label_gl) text_file = open(real_pose_file, 'w') text_file.write("{}\n".format(class_idx)) pose_str = "{} {} {} {}\n{} {} {} {}\n{} {} {} {}" \ .format(pose[0, 0], pose[0, 1], pose[0, 2], pose[0, 3], pose[1, 0], pose[1, 1], pose[1, 2], pose[1, 3], pose[2, 0], pose[2, 1], pose[2, 2], pose[2, 3]) text_file.write(pose_str) print(class_name, " done")
inner_id = np.where(np.squeeze(meta_data["cls_indexes"]) == class_idx) if len(meta_data["poses"].shape) == 2: pose = meta_data["poses"] else: pose = np.squeeze(meta_data["poses"][:, :, inner_id]) # adjust light position according to pose light_position = np.array(light_position) * 0.5 # inverse yz light_position[0] += pose[0, 3] light_position[1] -= pose[1, 3] light_position[2] -= pose[2, 3] rgb_gl, depth_gl = render_machine.render( RT_transform.mat2quat(pose[:3, :3]), pose[:, -1], light_position, light_intensity, class_name=cls_name, brightness_k=rm_randk, ) rgb_dict[class_idx] = rgb_gl real_color_img = cv2.imread(real_color_file, cv2.IMREAD_COLOR) import matplotlib.pyplot as plt fig = plt.figure() plt.axis("off")
def gen_observed(): gen_images = True if not gen_images: print("just generate observed set index files") model_root = os.path.join(cur_path, "../data/LINEMOD_6D/LM6d_converted/models") # output dir observed_root_dir = os.path.join(LM6d_occ_dsm_root, "data/observed") observed_set_dir = os.path.join(LM6d_occ_dsm_root, "image_set/observed") mkdir_if_missing(observed_root_dir) mkdir_if_missing(observed_set_dir) syn_pose_path = os.path.join(syn_pose_dir, "LM6d_occ_dsm_train_observed_pose_all.pkl") with open(syn_pose_path, "rb") as f: syn_pose_dict = cPickle.load(f) if gen_images: # init render machine brightness_ratios = [0.2, 0.25, 0.3, 0.35, 0.4] # ################## model_folder_dict = {} for class_name in sel_classes: if class_name == "__background__": continue model_folder_dict[class_name] = os.path.join( model_root, class_name) render_machine = Render_Py_Light_MultiProgram( sel_classes, model_folder_dict, K, width, height, ZNEAR, ZFAR, brightness_ratios, ) observed_set_path = os.path.join(observed_set_dir, "train_observed_{}.txt") train_idx_dict = {cls_name: [] for cls_name in classes} for idx in tqdm(range(num_images)): prefix = observed_prefix_list[idx] if len(syn_pose_dict[prefix].keys()) < 3: continue random.shuffle(class_indices) if idx % 500 == 0: print("idx: {}, prefix: {}".format(idx, prefix)) if gen_images: # generate random light_position if idx % 6 == 0: light_position = [1, 0, 1] elif idx % 6 == 1: light_position = [1, 1, 1] elif idx % 6 == 2: light_position = [0, 1, 1] elif idx % 6 == 3: light_position = [-1, 1, 1] elif idx % 6 == 4: light_position = [-1, 0, 1] elif idx % 6 == 5: light_position = [0, 0, 1] else: raise Exception("???") # randomly adjust color and intensity for light_intensity colors = np.array([ [0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0], [1, 1, 1], ]) intensity = np.random.uniform(0.8, 1.2, size=(3, )) colors_randk = random.randint(0, colors.shape[0] - 1) light_intensity = colors[colors_randk] * intensity # randomly choose a render machine(brightness_ratio) rm_randk = random.randint(0, len(brightness_ratios) - 1) rgb_dict = {} label_dict = {} depth_dict = {} pose_dict = {} depth_mean_dict = {} classes_in_pose = syn_pose_dict[prefix].keys() # print('num classes:', len(classes_in_pose)) pose_0 = pose_q2m( syn_pose_dict[prefix][classes_in_pose[0]]) # the first pose for cls_idx in class_indices: cls_name = sel_classes[cls_idx] if cls_name not in classes_in_pose: continue train_idx_dict[cls_name].append("./" + prefix) if gen_images: pose_q = syn_pose_dict[prefix][cls_name] # print(pose_q) pose = pose_q2m(pose_q) pose_dict[cls_name] = pose light_position_0 = np.array(light_position) * 0.5 # inverse yz # print('pose_0: ', pose_0) light_position_0[0] += pose_0[0, 3] light_position_0[1] -= pose_0[1, 3] light_position_0[2] -= pose_0[2, 3] rgb_gl, depth_gl = render_machine.render( RT_transform.mat2quat(pose[:3, :3]), pose[:, -1], light_position_0, light_intensity, class_name=cls_name, brightness_k=rm_randk, ) rgb_gl = rgb_gl.astype("uint8") rgb_dict[cls_name] = rgb_gl label_gl = np.zeros(depth_gl.shape) label_gl[depth_gl != 0] = 1 label_dict[cls_name] = label_gl depth_dict[cls_name] = depth_gl * depth_factor depth_mean_dict[cls_name] = np.mean(depth_gl[depth_gl != 0]) if gen_images: # get rendered results together res_img = np.zeros(rgb_gl.shape, dtype="uint8") res_depth = np.zeros(depth_gl.shape, dtype="uint16") res_label = np.zeros(label_gl.shape) means = depth_mean_dict.values() gen_indices = np.argsort(np.array(means)[::-1]) gen_classes = depth_mean_dict.keys() for cls_idx in gen_indices: cls_name = gen_classes[cls_idx] if cls_name not in classes_in_pose: continue tmp_rgb = rgb_dict[cls_name] tmp_label = label_dict[cls_name] for i in range(3): res_img[:, :, i][tmp_label == 1] = tmp_rgb[:, :, i][tmp_label == 1] # label res_label[tmp_label == 1] = class2idx(cls_name) # depth tmp_depth = depth_dict[cls_name] res_depth[tmp_label == 1] = tmp_depth[tmp_label == 1] res_depth = res_depth.astype("uint16") def vis_check(): fig = plt.figure(figsize=(8, 6), dpi=200) plt.axis("off") fig.add_subplot(1, 3, 1) plt.imshow(res_img[:, :, [2, 1, 0]]) plt.axis("off") plt.title("res_img") plt.subplot(1, 3, 2) plt.imshow(res_depth / depth_factor) plt.axis("off") plt.title("res_depth") plt.subplot(1, 3, 3) plt.imshow(res_label) plt.axis("off") plt.title("res_label") fig.suptitle( "light position_0: {}\n light_intensity: {}\n brightness: {}". format(light_position_0, light_intensity, brightness_ratios[rm_randk])) plt.show() # vis_check() if gen_images: # write results ------------------------------------- observed_color_file = os.path.join(observed_root_dir, prefix + "-color.png") observed_depth_file = os.path.join(observed_root_dir, prefix + "-depth.png") observed_label_file = os.path.join(observed_root_dir, prefix + "-label.png") poses_file = os.path.join(observed_root_dir, prefix + "-poses.npy") cv2.imwrite(observed_color_file, res_img) cv2.imwrite(observed_depth_file, res_depth) cv2.imwrite(observed_label_file, res_label) np.save(poses_file, pose_dict) # pass # write observed set index for cls_name in sel_classes: train_indices = train_idx_dict[cls_name] train_indices = sorted(train_indices) with open(observed_set_path.format(cls_name), "w") as f: for line in train_indices: f.write(line + "\n") print("done")