Created
August 21, 2024 13:42
-
-
Save kenomo/bafd51cbc03271ce79e4cd79bd8e2279 to your computer and use it in GitHub Desktop.
Read e57 (point clouds + images) files exported with Leica Cyclone FIELD 360 (tested with Leica BLK360 G2)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import numpy as np | |
| import pye57 | |
| import cv2 | |
| import open3d as o3d | |
| from pyquaternion import Quaternion | |
| import matplotlib.pyplot as plt | |
| ########################################## | |
| class Image: | |
| def __init__(self, image2D): | |
| self.name = image2D["name"].value() | |
| self.rotation = Quaternion([e.value() for e in image2D["pose"]["rotation"]]) | |
| # Rotate extrinsic matrix, such that camera's forward direction is along the positive z-axis | |
| rotation_180_x = Quaternion(axis=[1, 0, 0], angle=np.pi) | |
| self.rotation = self.rotation * rotation_180_x | |
| self.rotation_matrix = self.rotation.rotation_matrix | |
| self.translation = [e.value() for e in image2D["pose"]["translation"]] | |
| self.extrinsic = np.eye(4) | |
| self.extrinsic[:3, :3] = self.rotation_matrix | |
| self.extrinsic[:3, 3] = self.translation | |
| pinhole = image2D["pinholeRepresentation"] | |
| focal_length_mm = pinhole["focalLength"].value() * 1000.0 | |
| self.principal_point_x = pinhole["principalPointX"].value() | |
| self.principal_point_y = pinhole["principalPointY"].value() | |
| pixel_width_mm = pinhole["pixelWidth"].value() * 1000.0 | |
| pixel_height_mm = pinhole["pixelHeight"].value() * 1000.0 | |
| self.image_width = pinhole["imageWidth"].value() | |
| self.image_height = pinhole["imageHeight"].value() | |
| self.focal_length_x = focal_length_mm / pixel_width_mm | |
| self.focal_length_y = focal_length_mm / pixel_height_mm | |
| jpeg_image = pinhole["jpegImage"] | |
| jpeg_image_data = np.zeros(shape=jpeg_image.byteCount(), dtype=np.uint8) | |
| jpeg_image.read(jpeg_image_data, 0, jpeg_image.byteCount()) | |
| image_bgr = cv2.imdecode(jpeg_image_data, cv2.IMREAD_COLOR) | |
| self.image_rgb = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2RGB) | |
| self.intrinsic = o3d.camera.PinholeCameraIntrinsic( | |
| self.image_width, self.image_height, | |
| self.focal_length_x, self.focal_length_y, | |
| self.principal_point_x, self.principal_point_y | |
| ) | |
| self.camera_frustum = o3d.geometry.LineSet.create_camera_visualization( | |
| self.image_width, self.image_height, | |
| self.intrinsic.intrinsic_matrix, | |
| np.linalg.inv(self.extrinsic), | |
| 1.0 | |
| ) | |
| self.coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) | |
| self.coordinate_frame.transform(self.extrinsic) | |
| class Scan: | |
| def __init__(self, e57, scan_idx): | |
| imf = e57.image_file | |
| root = imf.root() | |
| # General information | |
| header = e57.get_header(scan_idx) | |
| scan = e57.read_scan(scan_idx, intensity=True, colors=True, transform=False) | |
| self.point_count = header.point_count | |
| self.rotation = header.rotation | |
| self.rotation_matrix = header.rotation_matrix | |
| self.translation = header.translation | |
| self.pose = np.eye(4) | |
| self.pose[:3, :3] = self.rotation_matrix | |
| self.pose[:3, 3] = self.translation | |
| # frame of scan in global system | |
| self.coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) | |
| self.coordinate_frame.transform(self.pose) | |
| self.guid = header.guid | |
| self.name = root["data3D"][scan_idx]["name"].value() | |
| self.intensity_limits_min = header.intensityMinimum | |
| self.intensity_limits_max = header.intensityMaximum | |
| self.color_limits_min = [0, 0, 0] | |
| self.color_limits_max = [1, 1, 1] | |
| self.color_limits_min[0] = root["data3D"][scan_idx]["colorLimits"]["colorRedMinimum"].value() | |
| self.color_limits_max[0] = root["data3D"][scan_idx]["colorLimits"]["colorRedMaximum"].value() | |
| self.color_limits_min[1] = root["data3D"][scan_idx]["colorLimits"]["colorGreenMinimum"].value() | |
| self.color_limits_max[1] = root["data3D"][scan_idx]["colorLimits"]["colorGreenMaximum"].value() | |
| self.color_limits_min[2] = root["data3D"][scan_idx]["colorLimits"]["colorBlueMinimum"].value() | |
| self.color_limits_max[2] = root["data3D"][scan_idx]["colorLimits"]["colorBlueMaximum"].value() | |
| self.cartesian_bounds_min = [header.xMinimum, header.yMinimum, header.zMinimum] | |
| self.cartesian_bounds_max = [header.xMaximum, header.yMaximum, header.zMaximum] | |
| # Scan data | |
| self.x = scan["cartesianX"] | |
| self.y = scan["cartesianY"] | |
| self.z = scan["cartesianZ"] | |
| self.i = scan["intensity"] | |
| self.r = scan["colorRed"] | |
| self.g = scan["colorGreen"] | |
| self.b = scan["colorBlue"] | |
| self.points = np.vstack((self.x, self.y, self.z)).T | |
| self.colors = np.vstack((self.r, self.g, self.b)).T / 255.0 | |
| self.point_cloud = o3d.geometry.PointCloud() | |
| self.point_cloud.points = o3d.utility.Vector3dVector(self.points) | |
| self.point_cloud.colors = o3d.utility.Vector3dVector(self.colors) | |
| self.point_cloud.transform(self.pose) | |
| # Read images | |
| self.images = [] | |
| for image2D in root["images2D"]: | |
| if image2D["associatedData3DGuid"].value() != self.guid: | |
| continue | |
| self.images.append(Image(image2D)) | |
| ########################################## | |
| e57 = pye57.E57("scan.e57") | |
| number_of_scans = e57.scan_count | |
| print("Number of scans: ", number_of_scans) | |
| # Read scans | |
| scans = [] | |
| for scan_idx in range(number_of_scans): | |
| scans.append(Scan(e57, scan_idx)) | |
| # Visualize all scans | |
| print("Visualizing all scans") | |
| o3d.visualization.draw_geometries([scan.point_cloud for scan in scans] + [scan.coordinate_frame for scan in scans]) | |
| # Visualize individual scans | |
| print("Visualizing individual scans") | |
| for scan in scans: | |
| o3d.visualization.draw_geometries([scan.point_cloud] + [scan.coordinate_frame]) | |
| # Visualize all frustums of images | |
| print("Visualizing all frustums of images per scan") | |
| for scan in scans: | |
| o3d.visualization.draw_geometries([scan.point_cloud] + [image.camera_frustum for image in scan.images]) | |
| # Visualize individual images and frustums | |
| print("Visualizing individual images and frustums per scan") | |
| for scan in scans: | |
| for image in scan.images: | |
| o3d_image = o3d.geometry.Image(image.image_rgb) | |
| o3d.visualization.draw_geometries([scan.point_cloud, image.coordinate_frame, image.camera_frustum, o3d_image]) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment