def create_pointcloud_from_depth( depth_path, # type: str, focal_x, # type: float, focal_y, # type: float, principal_x, # type: float, principal_y, # type: float ): depth_image = open3d.read_image(depth_path) intrinsic = open3d.camera.PinholeCameraIntrinsic() intrinsic.set_intrinsics(640, 480, focal_x, focal_y, principal_x, principal_y) return open3d.geometry.create_point_cloud_from_depth_image( depth_image, intrinsic)
def getPcRGBD(path, idx): frame = 'frame-000000' frame = frame[:-len(idx)] + idx depth = frame + '.depth.png' depth = os.path.join(path, depth) rgb = frame + '.color.png' rgb = os.path.join(path, rgb) im_depth = o3d.read_image(depth) im_color = o3d.read_image(rgb) # rgbd_image = o3d.create_rgbd_image_from_color_and_depth(im_color, im_depth) pcd = o3d.create_point_cloud_from_depth_image( im_depth, o3d.PinholeCameraIntrinsic( # pcd = o3d.create_point_cloud_from_rgbd_image(rgbd_image, o3d.PinholeCameraIntrinsic( o3d.PinholeCameraIntrinsicParameters.PrimeSenseDefault)) # o3d.draw_geometries([pcd]) return pcd
def read_color_and_depth_image( folder, ith_image, output_img_format="open3d", index_len=5, image_folder="image", # folder/image/image_ith_image image_name="image_", depth_doler="depth", # folder/depth/depth_ith_image depth_name="depth_", ): assert output_img_format in ["open3d", "cv2", "cv"] ''' test case{ images_folder = "/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/data/mydata" cam_param_file="/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/config/cam_params.json" color, depth = read_color_and_depth_image(images_folder, 1) pinhole_camera_intrinsic = open3d.io.read_pinhole_camera_intrinsic(cam_param_file) cloud = rgbd2cloud(color, depth, pinhole_camera_intrinsic) cloud = filtCloudByRange(cloud, zmin=0.2, zmax=0.8) open3d.io.draw_geometries([cloud]) } ''' cfolder = folder + "/" + image_folder + "/" + image_name dfolder = folder + "/" + depth_doler + "/" + depth_name str_idx = ("{:0" + str(index_len) + "d}").format(ith_image) suffix = str_idx + ".png" if output_img_format == "cv" or output_img_format == "cv2": color_image = cv2.imread(cfolder + suffix, cv2.IMREAD_UNCHANGED) depth_image = cv2.imread(dfolder + suffix, cv2.IMREAD_UNCHANGED) else: color_image = open3d.read_image(cfolder + suffix) depth_image = open3d.read_image(dfolder + suffix) return color_image, depth_image
def __init__(self, root, traj, subsample_rate=20, depth_scale=2000, trans_by_pose=None): self.root = root self.traj = traj self._trans_by_pose = trans_by_pose self.depth_scale = depth_scale traj_file = os.path.join(root, 'local_point_cloud', traj) depth_files = [line.rstrip('\n') for line in open(traj_file)] self.n_pc = len(depth_files) # load point cloud and gt image_structs = sio.loadmat(os.path.join(root, 'image_structs.mat')) image_structs = image_structs['image_structs'][0] image_files = [i[0][0] for i in image_structs] point_clouds = [] self.gt = np.zeros((self.n_pc, 6)) for index, depth_file in enumerate(depth_files): depth_file_full = os.path.join(root, 'high_res_depth', depth_file) depth_map = np.asarray(o3d.read_image(depth_file_full)) current_point_cloud = utils.convert_depth_map_to_pc( depth_map, self.focal_len, self.principal_pt, depth_scale=self.depth_scale) current_point_cloud = current_point_cloud[::subsample_rate, :: subsample_rate, :] point_clouds.append(current_point_cloud) image_file = depth_file[:14] + '1.jpg' idx = image_files.index(image_file) current_image_struct = image_structs[idx] current_pos = current_image_struct[6] current_direction = current_image_struct[4] current_gt = np.concatenate((current_pos, current_direction)).T self.gt[index, :] = current_gt point_clouds = np.asarray(point_clouds) self.point_clouds = torch.from_numpy(point_clouds) # <NxHxWx3> self.valid_points = find_valid_points(self.point_clouds)
def generate_pcd(path): setting = list( csv.reader(open(path + '/img.cfg', 'r'), delimiter=';', skipinitialspace=True))[1] h_start_angle = math.radians(float(setting[2])) h_end_angle = math.radians(float(setting[3])) v_angles = numpy.radians(numpy.float32(setting[4:])) image_files = sorted( [path + '/' + x for x in os.listdir(path) if '.png' in x]) vecs = [] for row in range(int(setting[1])): for col in range(int(setting[0])): yaw_step = (h_end_angle - h_start_angle) / int(setting[0]) yaw = h_start_angle + col * yaw_step pitch = v_angles[row] vecs.append(yawpitch2_vec(pitch, yaw)) vecs = numpy.float32(vecs) for image_file in image_files: print image_file image = numpy.asarray(open3d.read_image(image_file)).flatten() points = (vecs.T * (image / 500.0).flatten()).T cloud = open3d.geometry.PointCloud() cloud.points = open3d.Vector3dVector(points) open3d.estimate_normals(cloud, search_param=open3d.KDTreeSearchParamHybrid( radius=0.2, max_nn=30)) dst_filename = image_file[:-4] + '.pcd' open3d.write_point_cloud(dst_filename, cloud)
def generateImage(mapping, im_color): global CLOUD_ROT img_m = mapping.Cloud2Image(CLOUD_ROT) img_mapped = cv2.addWeighted(img_m, 0.5, im_color, 0.5, 0) cv2.imshow(window_name, img_mapped) if __name__ == "__main__": args = get_argumets() """Data loading""" print(":: Load two point clouds to be matched.") color_raw = o3.read_image(args.cimg) depth_raw = o3.read_image(args.dimg) camera_intrinsic = o3.read_pinhole_camera_intrinsic(args.intrin) im_color = np.asarray(color_raw) im_color = cv2.cvtColor(im_color, cv2.COLOR_BGR2RGB) im_depth = np.asarray(depth_raw) rgbd_image = o3.create_rgbd_image_from_color_and_depth( color_raw, depth_raw, 1000.0, 2.0) pcd = o3.create_point_cloud_from_rgbd_image(rgbd_image, camera_intrinsic) o3.write_point_cloud("cloud_in.ply", pcd) cloud_in_ds = o3.voxel_down_sample(pcd, 0.005) o3.write_point_cloud("cloud_in_ds.ply", cloud_in_ds) np_pcd = np.asarray(pcd.points)
os.path.join(path, "image/"), extension = ".jpg") if USE_Z_BUFFERING: depth_image_path = [osp.join('/tmp', fn.replace('jpg', 'png')) for fn in color_image_path] # generate depth maps save_depth_maps(mesh, camera.intrinsic, np.asarray(camera.extrinsic), depth_image_path) else: depth_filenames = get_file_list( os.path.join(path, "depth/"), extension = ".png") depth_image_path = [] # add artificial noise to the depth maps for i in range(len(depth_filenames)): depth = open3d.read_image(osp.join(path, 'depth', depth_filenames[i])) depth = np.asarray(depth, dtype=np.float) depth += 50*np.random.rand(*depth.shape) tmp_filename = osp.join('/tmp', 'depth_{:s}'.format(depth_filenames[i])) depth_image_path.append(tmp_filename) assert(len(depth_image_path) == len(color_image_path)) # Read RGBD images rgbd_images = [] for i in range(len(depth_image_path)): depth = open3d.read_image(depth_image_path[i]) color = open3d.read_image(os.path.join(path, 'image', color_image_path[i])) rgbd_image = open3d.create_rgbd_image_from_color_and_depth(color, depth, convert_rgb_to_intensity = False) if debug_mode:
args = get_argumets() green = np.array([0, 255, 0]) blue = np.array([255, 0, 0]) white = np.array([255, 255, 255]) img_list = glob.glob(args.img_folder + '/*rgb.png') img_list.sort() model_list = glob.glob(args.model_dir + '/*.pcd') for i, cimg in enumerate(img_list): """Data loading""" print(":: Load two point clouds to be matched.") color_raw = o3.read_image(cimg) dimg = cimg[:-7] + 'depth.png' depth_raw = o3.read_image(dimg) camera_intrinsic = o3.read_pinhole_camera_intrinsic(args.intrin) im_color = np.asarray(color_raw) im_color = cv2.cvtColor(im_color, cv2.COLOR_BGR2RGB) im_depth = np.asarray(depth_raw) rgbd_image = o3.create_rgbd_image_from_color_and_depth( color_raw, depth_raw, 1000.0, 2.0) pcd = o3.create_point_cloud_from_rgbd_image(rgbd_image, camera_intrinsic) o3.write_point_cloud("cloud_in.ply", pcd) cloud_in_ds = o3.voxel_down_sample(pcd, 0.005)
def map_texture(object_name, session_name, base_dir, models_dir, debug_mode=False, show_textured_mesh=False, depth_thresh_for_visibility=1e-2, depth_thresh_for_discontinuity=0.035, max_vertex_normal_angle=70, real_depth_maps=False): data_dir = osp.join(base_dir, session_name, object_name) try: with open(osp.join(data_dir, 'object_name.txt'), 'r') as f: object_name = f.readline().rstrip() except IOError: print('{:s} does not have object_name.txt, skipping'.format(data_dir)) return # read mesh file mesh_filename = osp.join(models_dir, '{:s}.ply'.format(object_name)) mesh = open3d.read_triangle_mesh(mesh_filename) if not mesh.has_vertex_normals(): mesh.compute_vertex_normals() # names of views rgb_im_dir = osp.join(data_dir, 'thermal_images') pose_dir = osp.join(data_dir, 'poses') names = [] for filename in os.listdir(pose_dir): if '.txt' not in filename: continue if 'camera_pose' not in filename: continue name = '_'.join(filename.split('.')[0].split('_')[2:]) names.append(name) names = sorted(names) # camera extrinsics Ts = [] for name in names: filename = osp.join(pose_dir, 'camera_pose_{:s}.txt'.format(name)) T = np.eye(4) with open(filename, 'r') as f: f.readline() T[0, 3] = float(f.readline().strip()) T[1, 3] = float(f.readline().strip()) T[2, 3] = float(f.readline().strip()) f.readline() f.readline() T[0, :3] = [float(v) for v in f.readline().strip().split()] T[1, :3] = [float(v) for v in f.readline().strip().split()] T[2, :3] = [float(v) for v in f.readline().strip().split()] Ts.append(np.linalg.inv(T)) # RGB images rgb_ims = [] im_intensities = [] for name in names: rgb_im = open3d.read_image(osp.join(rgb_im_dir, '{:s}.png'.format(name))) rgb_im = np.asarray(rgb_im) rgb_ims.append(rgb_im) im_shape = rgb_im.shape intensity = rgb_im[:200, :200, 0].mean() im_intensities.append(intensity) target_intensity = np.mean(im_intensities) for i, rgb_im in enumerate(rgb_ims): rgb_im = rgb_im * target_intensity / im_intensities[i] rgb_im = np.clip(rgb_im, a_min=0, a_max=255) rgb_ims[i] = open3d.Image(rgb_im.astype(np.uint8)) # camera intrinsic cinfo_filename = cinfo_manager.getPackageFileName( 'package://contactdb_utils/calibrations/boson.yaml') cinfo = cinfo_manager.loadCalibrationFile(cinfo_filename, 'boson') h_scaling = float(im_shape[0]) / cinfo.height w_scaling = float(im_shape[1]) / cinfo.width K = np.asarray(cinfo.K) K[0] *= w_scaling K[2] *= w_scaling K[4] *= h_scaling K[5] *= h_scaling K = K.reshape((3,3)) intrinsic = open3d.PinholeCameraIntrinsic(im_shape[1], im_shape[0], K[0,0], K[1,1], K[0,2], K[1,2]) if real_depth_maps: # use registered Kinect depthmaps depth_im_dir = osp.join(data_dir, 'depth_images') depth_ims = [] for name in names: depth_im_filename = osp.join(depth_im_dir, '{:s}.png'.format(name)) depth_im = open3d.read_image(depth_im_filename) depth_im = np.uint16(fill_holes(depth_im)) depth_ims.append(depth_im) else: # create depth maps by rendering depth_ims = render_depth_maps(mesh_filename, intrinsic, Ts) # create RGB-D images rgbds = [] for depth_im, rgb_im, T in zip(depth_ims, rgb_ims, Ts): depth_im = open3d.Image(depth_im) rgbds.append(open3d.create_rgbd_image_from_color_and_depth(rgb_im, depth_im, convert_rgb_to_intensity=False)) if debug_mode: pc = open3d.create_point_cloud_from_rgbd_image(rgbds[-1], intrinsic) tmesh = copy(mesh) tmesh.transform(T) geoms = [pc] if show_textured_mesh: geoms.append(tmesh) open3d.draw_geometries(geoms) # create trajectory for texture mapping traj = open3d.PinholeCameraTrajectory() traj.extrinsic = open3d.Matrix4dVector(np.asarray(Ts)) traj.intrinsic = intrinsic # do texture mapping! option = open3d.ColorMapOptmizationOption() option.maximum_iteration = 300 option.depth_threshold_for_visiblity_check = depth_thresh_for_visibility option.depth_threshold_for_discontinuity_check = \ depth_thresh_for_discontinuity option.half_dilation_kernel_size_for_discontinuity_map = 0 option.max_angle_vertex_normal_camera_ray = max_vertex_normal_angle open3d.color_map_optimization(mesh, rgbds, traj, option) if not debug_mode: # write result as a PLY file mesh_filename = osp.join(data_dir, 'thermal_images', '{:s}_textured.ply'.format(object_name)) open3d.write_triangle_mesh(mesh_filename, mesh) print('Written {:s}'.format(mesh_filename)) if show_textured_mesh: show_object_mesh(data_dir)
def reconstruct(room_name, cam_poses): # prepare the result file result_file = os.path.join( proj_path, "result", "{}_{}_{}.json".format(dataset_name, room_name, vox_num_option)) print(result_file) if ft.file_exist(result_file): # load data print("load result json file ...") data_result = io.load_json(result_file) else: print("No result file, please check your folder again!") return if VIS_ALL or VIS_FINAL: vis = o3d.Visualizer() vis.create_window() print("created a window") for start_index_key in start_key_list: print("start index", start_index_key) result_pcd_folder = os.path.join(proj_path, "result", "pcd", start_index_key) if not os.path.exists(result_pcd_folder): os.makedirs(result_pcd_folder) # regarding the view point for the visualiser viewpoint_file = os.path.join(result_pcd_folder, "view_point.json") for method in NBV_strategy: print("method", method) result_pcd_file = os.path.join( result_pcd_folder, "{}_{}_{}.ply".format(dataset_name, room_name, method)) result_pcd_folder_method = os.path.join(proj_path, "result", start_index_key, method) if (not os.path.exists(result_pcd_file)) or ( VIS_ALL and (not os.path.exists(result_pcd_folder_method))): if VIS_ALL: trajectory = o3d.read_pinhole_camera_trajectory( viewpoint_file) print("created a window") if VIS_ALL and (not os.path.exists(result_pcd_folder_method)): os.makedirs(os.path.join(result_pcd_folder_method, "pcd")) os.makedirs(os.path.join(result_pcd_folder_method, "depth")) os.makedirs(os.path.join(result_pcd_folder_method, "rgb")) print("created folders") nbv_ids = data_result[start_index_key][method]["NBV_ids"] volume = o3d.ScalableTSDFVolume( voxel_length=0.01, sdf_trunc=0.03, color_type=o3d.TSDFVolumeColorType.RGB8) # 4.0 / 512.0 count = 0 for pose_id in range(len(nbv_ids)): print("Integrate frame at {:d}".format(pose_id)) current_pose = cam_poses[nbv_ids[pose_id]] rgb_path = os.path.join( proj_path, config["dataset_folder"], dataset_name, room_name, "image", "rgb{:d}.png".format(nbv_ids[pose_id])) depth_path = os.path.join( proj_path, config["dataset_folder"], dataset_name, room_name, "depth", "depth{:d}.png".format(nbv_ids[pose_id])) color = o3d.read_image(rgb_path) depth = o3d.read_image(depth_path) rgbd = o3d.create_rgbd_image_from_color_and_depth( color, depth, depth_trunc=4.5, convert_rgb_to_intensity=False) volume.integrate(rgbd, intrinsic, np.linalg.inv(current_pose)) pcd_scene = volume.extract_point_cloud( ) # note the points are not isometric if VIS_ALL: print("Visualise each frame") result_pcd_file_temp = os.path.join( result_pcd_folder_method, "pcd", "{}_{}_{:03d}.png".format(dataset_name, room_name, count)) result_depth_file_temp = os.path.join( result_pcd_folder_method, "depth", "{}_{}_{:03d}.png".format(dataset_name, room_name, count)) result_rgb_file_temp = os.path.join( result_pcd_folder_method, "rgb", "{}_{}_{:03d}.png".format(dataset_name, room_name, count)) copyfile(depth_path, result_depth_file_temp) copyfile(rgb_path, result_rgb_file_temp) vis.add_geometry(pcd_scene) mesh_frame = local_plt.o3d_coordinate(current_pose) vis.add_geometry(mesh_frame) ctr = vis.get_view_control() #if count == 0: ctr.convert_from_pinhole_camera_parameters( trajectory.intrinsic, trajectory.extrinsic[0]) vis.update_geometry() vis.poll_events() vis.update_renderer() vis.capture_screen_image(result_pcd_file_temp) # crop border img = improc.crop_border(result_pcd_file_temp, 0.08, 0.4) # img = improc.imscale(img, 1.5) improc.imwrite(result_pcd_file_temp, img) time.sleep(0.5) pcd_scene.points = o3d.Vector3dVector([]) pcd_scene.colors = o3d.Vector3dVector([]) vis.update_geometry() vis.poll_events() vis.update_renderer() count = count + 1 if not os.path.exists(result_pcd_file): pcd_scene = volume.extract_point_cloud( ) #note the points are not isometric o3d.write_point_cloud(result_pcd_file, pcd_scene) else: if VIS_FINAL: print("created a visualiser") pcd_scene = o3d.read_point_cloud(result_pcd_file) # only trigger the view point set when there is no tarjectory file if not os.path.exists(viewpoint_file): print( "Please manipulate the visualiser for a better view point, close to save!" ) save_view_point(vis, pcd_scene, viewpoint_file) return image_name = os.path.join(result_pcd_folder, "{}_pcd.png".format(method)) trajectory = o3d.read_pinhole_camera_trajectory( viewpoint_file) print("read the view point") vis.add_geometry(pcd_scene) print("Added the view point") ctr = vis.get_view_control() print("obtained view control") ctr.convert_from_pinhole_camera_parameters( trajectory.intrinsic, trajectory.extrinsic[0]) print("changed the view point") vis.update_geometry() vis.poll_events() vis.update_renderer() print("updated the rendering") vis.capture_screen_image(image_name) print("captured the screenshot") # crop border img = improc.crop_border(image_name, 0.08, 0.4) #img = improc.imscale(img, 1.5) improc.imwrite(image_name, img) time.sleep(2) pcd_scene.points = o3d.Vector3dVector([]) pcd_scene.colors = o3d.Vector3dVector([]) vis.update_geometry() vis.poll_events() vis.update_renderer() print("reset the point to empty") if VIS_ALL or VIS_FINAL: vis.destroy_window() print("closed window and visualiser")
import transforms3d as t3d # pip install transform3d import open3d as o3d # pip install open3d-python==0.5 import numpy as np import math # load point cloud i = 4 depth_image_path = "../output/bop_data/lmo/train_pbr/000000/depth/000000.png" depth_raw = o3d.read_image(depth_image_path) image_width = 671 image_height = 502 fx = 1122.375 fy = 1122.375 cx = 334.4445 cy = 264.443075 intrinsic = o3d.PinholeCameraIntrinsic(image_width, image_height, fx, fy, cx, cy) extrinsic = np.identity(4) print(extrinsic) depth_scale = 10 depth_trunc = 1500.0 np_depth = np.array(depth_raw) depth_raw = o3d.geometry.Image(np_depth) print(depth_raw) print(type(depth_raw)) print(type(intrinsic)) color = True
def read_image(self, path): return o3.read_image(path)
import open3d as o3d import matplotlib.pyplot as plt if __name__ == "__main__": print("Read Redwood dataset") color_raw = o3d.read_image("img3.png") depth_raw = o3d.io.read_image("imgDepth3.png") rgbd_image = o3d.geometry.create_rgbd_image_from_color_and_depth( color_raw, depth_raw) print(rgbd_image) plt.subplot(1, 2, 1) plt.title('Redwood grayscale image') plt.imshow(rgbd_image.color) plt.subplot(1, 2, 2) plt.title('Redwood depth image') plt.imshow(rgbd_image.depth) plt.show() pcd = o3d.geometry.create_point_cloud_from_rgbd_image( rgbd_image, o3d.camera.PinholeCameraIntrinsic( o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)) # Flip it, otherwise the pointcloud will be upside down pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) o3d.visualization.draw_geometries([pcd])