def rgbd2cloud(color, depth, pinhole_camera_intrinsic, input_img_format="open3d"): assert input_img_format in ["open3d", "cv", "cv2"] ''' 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) } ''' if input_img_format == "cv" or input_img_format == "cv2": rgbd_image = open3d.create_rgbd_image_from_color_and_depth( open3d.Image(cv2.cvtColor(color, cv2.COLOR_BGR2RGB)), open3d.Image(depth), convert_rgb_to_intensity=False) else: rgbd_image = open3d.create_rgbd_image_from_color_and_depth( color, depth, convert_rgb_to_intensity=False) cloud = open3d.create_point_cloud_from_rgbd_image( rgbd_image, pinhole_camera_intrinsic) return cloud
def compute_point_cloud(self, color_img, depth_img): rgbd_image = open3d.create_rgbd_image_from_color_and_depth( open3d.Image(cv2.cvtColor(color_img, cv2.COLOR_BGR2RGB)), open3d.Image(depth_img), convert_rgb_to_intensity=False) cloud = open3d.create_point_cloud_from_rgbd_image( rgbd_image, self.camera_intrinsics) return cloud
def visualise_rgbd_cloud(key_list, resolution_height, resolution_width, device_manager, coordinate_dimentions, transformation_devices): # enable visualiser align = rs.align(rs.stream.depth) vis = o3d.Visualizer() vis.create_window('PCD', width=1280, height=720) pointcloud = o3d.PointCloud() geom_added = False voxel_size = 0.005 while True: cloud_pcd = o3d.PointCloud() for (serial, device) in device_manager._enabled_devices.items(): frames = device.pipeline.wait_for_frames() frames = align.process(frames) profile = frames.get_profile() depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue # Convert images to numpy arrays depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) img_depth = o3d.Image(depth_image) img_color = o3d.Image(color_image) rgbd = o3d.create_rgbd_image_from_color_and_depth( img_color, img_depth, convert_rgb_to_intensity=False) intrinsics = profile.as_video_stream_profile().get_intrinsics() pinhole_camera_intrinsic = o3d.PinholeCameraIntrinsic( intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy) pcd = o3d.create_point_cloud_from_rgbd_image( rgbd, pinhole_camera_intrinsic) pcd.transform(transformation_devices[serial].pose_mat) pointcloud.points = pcd.points pointcloud.colors = pcd.colors cloud_pcd = cloud_pcd + pointcloud #pcd = o3d.geometry.voxel_down_sample(cloud_pcd, # voxel_size=voxel_size) while True: if geom_added == False: vis.add_geometry(cloud_pcd) geom_added = True vis.update_geometry() vis.poll_events() vis.update_renderer()
def pcl_from_open3d(color_image, depth_image, camera_matrix): color_o3d = o3d.Image(color_image.astype(np.uint8)) depth_o3d = o3d.Image(depth_image.astype(np.uint8)) intrinsics = o3d.camera.PinholeCameraIntrinsic() intrinsics.intrinsic_matrix = camera_matrix rgbd = o3d.create_rgbd_image_from_color_and_depth( color_o3d, depth_o3d, convert_rgb_to_intensity=False) pcl = o3d.create_point_cloud_from_rgbd_image(image=rgbd, intrinsic=intrinsics) pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) return pcl
def get_rgbd_image(align_color_img, depth_img, depth_scale, clipping_distance_in_meters): align_color_img = align_color_img[:, :, 0:3] # Only get the first three channel align_color_img = align_color_img[..., ::-1] # Convert opencv BGR to RGB rgbd_image = open3d.create_rgbd_image_from_color_and_depth( open3d.Image(align_color_img.copy()), open3d.Image(depth_img), depth_scale=1.0 / depth_scale, depth_trunc=clipping_distance_in_meters, convert_rgb_to_intensity=False) # rgbd_image = open3d.geometry.create_rgbd_image_from_color_and_depth(open3d.Image(align_color_img.copy()), # open3d.Image(depth_img),) return rgbd_image
def get_camera_pcd(self, i): img = self._get_image(i) depth = self._get_depth(i) _, H, W = img.shape fx, fy, cx, cy = self._get_intrinsic(i) intrinsic = PinholeCameraIntrinsic(W, H, fx, fy, cx, cy) img = open3d.Image(img.transpose((1, 2, 0)).astype(np.uint8)) depth = open3d.Image(depth) rgbd = create_rgbd_image_from_color_and_depth( img, depth, depth_scale=1.0, convert_rgb_to_intensity=False) pcd = create_point_cloud_from_rgbd_image(rgbd, intrinsic) return pcd
def point_cloud_open3d(index, camera_matrix): depth_image = load_depth_image(index) depth_o3d = o3d.Image(depth_image.astype(np.uint16)) intrinsics = o3d.camera.PinholeCameraIntrinsic() intrinsics.intrinsic_matrix = camera_matrix pcl = o3d.create_point_cloud_from_depth_image(depth=depth_o3d, intrinsic=intrinsics, depth_scale=5000.0) # pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) return pcl
def create_open3d_point_cloud_from_rgbd(color_img, depth_img, cam_info, depth_unit=0.001, depth_trunc=3.0): ''' Create pointcreate_open3dpoint_cloud_from_rgbd cloud of open3d format, given opencv rgbd images and camera info. Arguments: color_img {np.ndarry, np.uint8}: 3 channels of BGR. Undistorted. depth_img {np.ndarry, np.uint16}: Undistorted depth image that matches color_img. cam_info {CameraInfo} depth_unit {float}: if depth_img[i, j] is x, then the real depth is x*depth_unit meters. depth_trunc {float}: Depth value larger than ${depth_trunc} meters gets truncated to 0. Output: open3d_point_cloud {open3d.geometry.PointCloud} See: http://www.open3d.org/docs/release/python_api/open3d.geometry.PointCloud.html Reference: ''' # Create `open3d.geometry.RGBDImage` from color_img and depth_img. # http://www.open3d.org/docs/0.7.0/python_api/open3d.geometry.create_rgbd_image_from_color_and_depth.html#open3d.geometry.create_rgbd_image_from_color_and_depth rgbd_image = open3d.create_rgbd_image_from_color_and_depth( color=open3d.Image(cv2.cvtColor(color_img, cv2.COLOR_BGR2RGB)), depth=open3d.Image(depth_img), depth_scale=1.0 / depth_unit, convert_rgb_to_intensity=False) # Convert camera info to `class open3d.camera.PinholeCameraIntrinsic`. # http://www.open3d.org/docs/release/python_api/open3d.camera.PinholeCameraIntrinsic.html pinhole_camera_intrinsic = cam_info.to_open3d_format() # Project image pixels into 3D world points. # Output type: `class open3d.geometry.PointCloud`. # http://www.open3d.org/docs/0.6.0/python_api/open3d.geometry.create_point_cloud_from_rgbd_image.html#open3d.geometry.create_point_cloud_from_rgbd_image open3d_point_cloud = open3d.create_point_cloud_from_rgbd_image( image=rgbd_image, intrinsic=pinhole_camera_intrinsic) return open3d_point_cloud
def pcl_from_depth_image(depth_image, camera_matrix): far = 3.0 depth_image = depth_image[:, :, 0] / 255.0 depth_image = lerp(depth_image, 0.0, 1.0, 0.0, far) depth_o3d = o3d.Image(depth_image.astype(np.uint8)) intrinsics = o3d.camera.PinholeCameraIntrinsic() intrinsics.intrinsic_matrix = camera_matrix pcl = o3d.create_point_cloud_from_depth_image(depth=depth_o3d, intrinsic=intrinsics) pcl.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) return pcl
def _create_point_cloud(self, color_img_resized, depth_img_resized): ''' Create point cloud from color and depth image. Return: pcd {open3d.geometry.PointCloud} ''' rgbd_image = open3d.create_rgbd_image_from_color_and_depth( color=open3d.Image( cv2.cvtColor(color_img_resized, cv2.COLOR_BGR2RGB)), depth=open3d.Image(depth_img_resized), depth_scale=1.0 / self._cfg.depth_unit, depth_trunc=self._cfg.depth_trunc, convert_rgb_to_intensity=False) cam_intrin = self._cam_intrin_resized.to_open3d_format() pcd = open3d.create_point_cloud_from_rgbd_image(rgbd_image, cam_intrin) if self._cfg.cloud_downsample_voxel_size > 0: pcd = open3d.geometry.voxel_down_sample( pcd, voxel_size=self._cfg.cloud_downsample_voxel_size) return pcd
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)
color_image1 = np.asanyarray(color_frame1.get_data()) depth_image1 = np.asanyarray(depth_frame1.get_data()) color_frame2 = aligned_frames2.get_color_frame() depth_frame2 = aligned_frames2.get_depth_frame() color_image2 = np.asanyarray(color_frame2.get_data()) depth_image2 = np.asanyarray(depth_frame2.get_data()) depth_image11 = np.where( (depth_image1 > 1000) | (depth_image1 < 0), 0, depth_image1) depth_image22 = np.where( (depth_image2 > 1000) | (depth_image2 < 0), 0, depth_image2) depth1 = o3d.Image(depth_image11) color1 = o3d.Image(cv2.cvtColor(color_image1, cv2.COLOR_BGR2RGB)) depth2 = o3d.Image(depth_image22) color2 = o3d.Image(cv2.cvtColor(color_image2, cv2.COLOR_BGR2RGB)) rgbd1 = o3d.create_rgbd_image_from_color_and_depth( color1, depth1, convert_rgb_to_intensity=False) pcd1 = o3d.create_point_cloud_from_rgbd_image( rgbd1, pinhole_camera1_intrinsic) rgbd2 = o3d.create_rgbd_image_from_color_and_depth( color2, depth2, convert_rgb_to_intensity=False) pcd2 = o3d.create_point_cloud_from_rgbd_image( rgbd2, pinhole_camera2_intrinsic)
def track_view(vis): global frame, poses global num_frames, last_T global parameter ctr = vis.get_view_control() if frame == 0: params = ctr.convert_to_pinhole_camera_parameters() intrinsics = params.intrinsic extrinsics = params.extrinsic pose = np.array( [[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600], [-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143], [-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787], [0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]]) params = o3d.camera.PinholeCameraParameters() params.extrinsic = pose params.intrinsic = intrinsics ctr.convert_from_pinhole_camera_parameters(params) fid = frame % num_frames keyindex1 = 500 keyindex2 = 600 if True: intrinsics = o3d.read_pinhole_camera_intrinsic( "stream/%d.intrinsics.json" % fid) T = np.loadtxt('stream/%d.pose' % fid) params = o3d.camera.PinholeCameraParameters() params.extrinsic = T params.intrinsic = intrinsics ctr.convert_from_pinhole_camera_parameters(params) #很重要,不能删除 if (fid == keyindex1) | (fid == keyindex2): """ Generate Point Cloud """ if (last_T is None) or ((fid in frame_trace) or (AngularDistance(T, last_T))): print('%d/%d' % (fid, num_frames)) depth = vis.capture_depth_float_buffer(False) depth = np.array(depth) idx = np.where(depth > 30) depth[idx] = 0 depth = o3d.Image(depth) image = vis.capture_screen_float_buffer(False) image = o3d.Image( np.array(np.array(image) * 255).astype(np.uint8)) rgbd = o3d.create_rgbd_image_from_color_and_depth( image, depth, convert_rgb_to_intensity=False) rgbd.depth = o3d.Image(np.array(rgbd.depth) * 1000) pcd = o3d.create_point_cloud_from_rgbd_image( rgbd, intrinsics) pcd.transform(np.linalg.inv(T)) o3d.write_point_cloud("stream/%d.ply" % fid, pcd, write_ascii=True) cv2.imwrite("stream/%d.png" % fid, np.array(image)) depth = np.array(depth) #depth = np.array(depth)*1000 #depth.astype(np.uint16) #cv2.imwrite("stream/%d_depth.png" % fid, depth)## last_T = T if (fid == keyindex1): intrinsic_matrix = intrinsics.intrinsic_matrix pcd0, image, indexlist = see2pcd( fid, depth, np.array(image), pcd, intrinsics.intrinsic_matrix) pcd0.transform(np.linalg.inv(T)) #print(T) parameter['0'] = [pcd0, image, indexlist] if (fid == keyindex2): intrinsic_matrix = intrinsics.intrinsic_matrix pcd1, image, indexlist = see2pcd( fid, depth, np.array(image), pcd, intrinsics.intrinsic_matrix) pcd1.transform(np.linalg.inv(T)) #print(T) parameter['1'] = [pcd1, image, indexlist] vis.destroy_window() #o3d.draw_geometries([parameter['0'][0],pcd1]) compare(parameter) os.system("pause") if fid == num_frames - 1: exit() frame += 1
depth_image[depth_image > max_value] = max_value return depth_image, intrin data_path = '/home/spc/Desktop/DepthCodec/sample_data/*' files = glob.glob(data_path) assert files.__len__(), "no test data in " + data_path for example in files: depth, intrin = loadSampleFromFile(example, 20) plt.figure() plt.imshow(depth) plt.pause(0.1) plt.waitforbuttonpress() open3d_depth = open3d.Image(depth) open3d_intrin = open3d.PinholeCameraIntrinsic(intrin.width, intrin.height, intrin.fx, intrin.fy, intrin.ppx, intrin.ppy) open3d_pc1 = open3d.create_point_cloud_from_depth_image( open3d_depth, open3d_intrin) open3d.draw_geometries([open3d_pc1]) tc = DepthImageCodec() tc.compress(depth) bits = BitStream() bits = tc.encode(bits) print(depth.size * 16 / len(bits)) uncompressed = tc.uncompress() open3d_uncompressed = open3d.Image(uncompressed)
def to_rgbd(color, depth, depth_scale, depth_trunc): if color.shape[:2] != depth.shape[:2]: color = cv2.resize(color, (depth.shape[1], depth.shape[0])) rgbd = o3d.create_rgbd_image_from_color_and_depth(o3d.Image(color), o3d.Image(depth), convert_rgb_to_intensity=False, depth_scale=depth_scale, depth_trunc=depth_trunc) return rgbd
def track_view(vis): global frame, poses global num_frames, last_T #print('frame=%d' % frame) #import ipdb; ipdb.set_trace() ctr = vis.get_view_control() ######intrinsics = o3d.read_pinhole_camera_intrinsic('intrinsics.json') #intrinsics = o3d.PinholeCameraIntrinsic(o3d.PinholeCameraIntrinsicParameters.Kinect2ColorCameraDefault) ######intrinsics = o3d.PinholeCameraIntrinsic( ###### o3d.PinholeCameraIntrinsicParameters.PrimeSenseDefault) ######intrinsics_mat=np.array([[935.30743608719376, 0.0, 0.0], ###### [0.0, 935.30743608719376, 0.0], ###### [959.5, 539.5, 1.0]]) ######intrinsics.intrinsic_matrix = intrinsics_mat #import ipdb; ipdb.set_trace() ######print(intrinsics) #pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics) #o3d.write_point_cloud("stream/%d.ply" % frame, pcd) if frame == 0: #vis.get_render_option().load_from_json("render.json") intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters() pose = [[-0.8188861922, 0.3889273405, -0.4220911372, -14.6068376600], [-0.1157361687, -0.8321937190, -0.5422718444, 23.0477832143], [-0.5621659395, -0.3952077147, 0.7264849060, 4.1193224787], [0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000]] ctr.convert_from_pinhole_camera_parameters(intrinsics, pose) fid = frame % num_frames if not args.load: intrinsics, extrinsics = ctr.convert_to_pinhole_camera_parameters() o3d.write_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % frame, intrinsics) with open('stream/%d.pose' % frame, 'w') as fout: for i in range(4): for j in range(4): if j > 0: fout.write(' ') fout.write('%.10f' % extrinsics[i, j]) fout.write('\n') #o3d.write_pinhole_camera_intrinsics(intrinsics, "stream/%d.intrinsics.json" % frame) #pose_dict['intrinsics'].append(intrinsics) #pose_dict['extrinsics'].append(extrinsics) #if frame == 100: # import pickle # with open('stream/pose.p', 'wb') as fout: # pickle.dump(pose_dict, fout, protocol=pickle.HIGHEST_PROTOCOL) # exit() else: intrinsics = o3d.read_pinhole_camera_intrinsic("stream/%d.intrinsics.json" % fid) T = np.loadtxt('stream/%d.pose' % fid) ctr.convert_from_pinhole_camera_parameters(intrinsics, T) if args.generate: """ Generate Point Cloud """ if (last_T is None) or (fid in frame_trace) or (AngularDistance(T, last_T)): print('%d/%d' % (fid, num_frames)) #vis.update_geometry() #vis.poll_events() #vis.update_renderer() depth = vis.capture_depth_float_buffer(False) depth = np.array(depth) #print(depth.max(), depth.min()) idx = np.where(depth > 30) depth[idx] = 0 depth = o3d.Image(depth) image = vis.capture_screen_float_buffer(False) image = o3d.Image(np.array(np.array(image)*255).astype(np.uint8)) rgbd = o3d.create_rgbd_image_from_color_and_depth( image, depth, convert_rgb_to_intensity = False) rgbd.depth = o3d.Image(np.array(rgbd.depth)*1000) pcd = o3d.create_point_cloud_from_rgbd_image(rgbd, intrinsics) #pcd = o3d.voxel_down_sample(pcd, voxel_size=0.05) #pcd.transform(np.linalg.inv(T)) o3d.write_point_cloud("stream/%d.ply" % fid, pcd, write_ascii=True) cv2.imwrite("stream/%d.png" % fid, np.array(image)) last_T = T if fid == num_frames - 1: exit() frame += 1
depth_val = 255 * (norm) / 120 depth_val = 32 * np.log2(depth_val + 1) print('%s Norm %d' % (ply_file_path, max(norm[:]))) if np.isclose(max(norm[:]), 0): 'Error With File' continue thetaz = np.rad2deg(np.arcsin(xyz_load[:, 2] / norm)) thetax = (180 * (np.int64(xyz_load[:, 0] < 0))) + np.rad2deg( np.arctan(xyz_load[:, 1] / xyz_load[:, 0])) theta_x = thetax + 90 - 180 % 360 theta_z = thetaz + 90 img_cy = np.zeros((180 * res_scale, 360 * res_scale, 1)) img_cy[np.int64(theta_z * res_scale), np.int64(theta_x * res_scale)] = np.expand_dims( 255 - depth_val, 1) img_od_cyl = od.Image(img_cy.astype(np.uint8)) xyz_tmp = np.zeros_like(xyz_load) xyz_tmp[:, 1] = xyz_load[:, 0] xyz_tmp[:, 2] = xyz_load[:, 1] xyz_tmp[:, 0] = xyz_load[:, 2] ext = get_extrinsic(roll=0 * np.pi / 4, yaw=4 * np.pi / 4, pitch=2 * np.pi / 4, x=0, y=0, z=0) xyz_load = xyz_tmp K = np.identity(3) K[0, 2] = WINDOW_WIDTH / 2.0 K[1, 2] = WINDOW_HEIGHT / 2.0 K[0, 0] = K[1,