# Copyright © Niantic, Inc. 2022. import os import logging import numpy as np import trimesh import pyrender from PIL import Image, ImageOps from scipy.linalg import svd from matplotlib.colors import LinearSegmentedColormap import torch from torch.cuda.amp import autocast from skimage import io, color from skimage.transform import resize from ace_util import get_pixel_grid, to_homogeneous from bisect import insort logging.getLogger('trimesh').setLevel(level=logging.WARNING) _logger = logging.getLogger(__name__) THICKNESS = 0.005 * 50 # controls how thick the frustum's 'bars' are # define camera frustum geometry origin_frustum_verts = np.array([ (0., 0., 0.), (0.375, -0.375, -1.0), (0.375, 0.375, -1.0), (-0.375, 0.375, -1.0), (-0.375, -0.375, -1.0), ]) frustum_edges = np.array([ (1, 2), (1, 3), (1, 4), (1, 5), (2, 3), (3, 4), (4, 5), (5, 2), ]) - 1 def normalise_vector(vect): """ Returns vector with unit length. @param vect: Vector to be normalised. @return: Normalised vector. """ length = np.sqrt((vect ** 2).sum()) return vect / length def cuboid_from_line(line_start, line_end, color=(255, 0, 255)): """Approximates a line with a long cuboid color is a 3-element RGB tuple, with each element a uint8 value """ # create two vectors which are both (a) perpendicular to the direction of the line and # (b) perpendicular to each other. direction = normalise_vector(line_end - line_start) random_dir = normalise_vector(np.random.rand(3)) perpendicular_x = normalise_vector(np.cross(direction, random_dir)) perpendicular_y = normalise_vector(np.cross(direction, perpendicular_x)) vertices = [] for node in (line_start, line_end): for x_offset in (-1, 1): for y_offset in (-1, 1): vert = node + THICKNESS * (perpendicular_y * y_offset + perpendicular_x * x_offset) vertices.append(vert) faces = [ (4, 5, 1, 0), (5, 7, 3, 1), (7, 6, 2, 3), (6, 4, 0, 2), (0, 1, 3, 2), # end of tube (6, 7, 5, 4), # other end of tube ] mesh = trimesh.Trimesh(vertices=np.array(vertices), faces=np.array(faces)) for c in (0, 1, 2): mesh.visual.vertex_colors[:, c] = color[c] return mesh def generate_frustum_marker(pose, color=(255, 0, 255), size=1.): frustum_vertices = np.array([ [0., 0., 0., 1.], [1., 1., 3., 1.], [-1., 1., 3., 1.], [-1., -1., 3., 1.], [1., -1., 3., 1.] ]).T frustum_vertices[:3] *= size frustum_vertices[2, :] *= -1 # OpenCV to OpenGL frustum_vertices = pose @ frustum_vertices frustum_vertices = frustum_vertices[:3].T frustum_faces = np.array([ [0, 4, 1], [0, 1, 2], [0, 2, 3], [0, 3, 4], [4, 2, 1], [4, 3, 2], ]) mesh = trimesh.Trimesh(vertices=frustum_vertices, faces=frustum_faces) for c in (0, 1, 2): mesh.visual.vertex_colors[:, c] = color[c] return mesh def get_image_box( image_path, frustum_pose, cam_marker_size=1.0, flip=False ): """ Gets a textured mesh of an image. @param image_path: File path of the image to be rendered. @param frustum_pose: 4x4 camera pose, OpenGL convention @param cam_marker_size: Scaling factor for the image object @param flip: flag whether to flip the image left/right @return: duple, trimesh mesh of the image and aspect ratio of the image """ pil_image = Image.open(image_path) pil_image = ImageOps.flip(pil_image) # flip top/bottom to align with scene space pil_image_w, pil_image_h = pil_image.size aspect_ratio = pil_image_w / pil_image_h # import pdb; pdb.set_trace() height = 0.75 width = height * aspect_ratio width *= cam_marker_size height *= cam_marker_size if flip: pil_image = ImageOps.mirror(pil_image) # flips left/right width = -width vertices = np.zeros((4, 3)) vertices[0, :] = [width / 2, height / 2, -cam_marker_size] vertices[1, :] = [width / 2, -height / 2, -cam_marker_size] vertices[2, :] = [-width / 2, -height / 2, -cam_marker_size] vertices[3, :] = [-width / 2, height / 2, -cam_marker_size] faces = np.zeros((2, 3)) faces[0, :] = [0, 1, 2] faces[1, :] = [2, 3, 0] # faces[2,:] = [2,3] # faces[3,:] = [3,0] uvs = np.zeros((4, 2)) uvs[0, :] = [1.0, 0] uvs[1, :] = [1.0, 1.0] uvs[2, :] = [0, 1.0] uvs[3, :] = [0, 0] face_normals = np.zeros((2, 3)) face_normals[0, :] = [0.0, 0.0, 1.0] face_normals[1, :] = [0.0, 0.0, 1.0] material = trimesh.visual.texture.SimpleMaterial( image=pil_image, ambient=(1.0, 1.0, 1.0, 1.0), diffuse=(1.0, 1.0, 1.0, 1.0), ) texture = trimesh.visual.TextureVisuals( uv=uvs, image=pil_image, material=material, ) mesh = trimesh.Trimesh( vertices=vertices, faces=faces, face_normals=face_normals, visual=texture, validate=True, process=False ) # from simple recon code def transform_trimesh(mesh, transform): """ Applies a transform to a trimesh. """ np_vertices = np.array(mesh.vertices) np_vertices = (transform @ np.concatenate([np_vertices, np.ones((np_vertices.shape[0], 1))], 1).T).T np_vertices = np_vertices / np_vertices[:, 3][:, None] mesh.vertices[:, 0] = np_vertices[:, 0] mesh.vertices[:, 1] = np_vertices[:, 1] mesh.vertices[:, 2] = np_vertices[:, 2] return mesh return transform_trimesh(mesh, frustum_pose), aspect_ratio def generate_frustum_at_position(rotation, translation, color, size, aspect_ratio): """Generates a frustum mesh at a specified (rotation, translation), with optional color : rotation is a 3x3 numpy array : translation is a 3-long numpy vector : color is a 3-long numpy vector or tuple or list; each element is a uint8 RGB value : aspect_ratio is a float of width/height """ frustum_verts = origin_frustum_verts.copy() frustum_verts[:, 0] *= aspect_ratio transformed_frustum_verts = \ size * rotation.dot(frustum_verts.T).T + translation[None, :] cuboids = [] for edge in frustum_edges: line_cuboid = cuboid_from_line(line_start=transformed_frustum_verts[edge[0]], line_end=transformed_frustum_verts[edge[1]], color=color) cuboids.append(line_cuboid) return trimesh.util.concatenate(cuboids) class LazyCamera: """Smooth and slightly delayed scene camera. Implements a rolling average of last few camera positions. Also zooms out to display the whole scene. """ def __init__(self, camera_buffer_size=40, backwards_offset=4, camera_buffer=None): """Constructor. Parameters: camera_buffer_size: Number of last few cameras to consider backwards_offset: Move observing camera backwards from current view, in meters camera_buffer: Optional array of camera positions to pre-fill the buffer """ # buffer holding last m camera positions if camera_buffer is None: self.m_camera_buffer = [] else: self.m_camera_buffer = camera_buffer self.m_camera_buffer_size = camera_buffer_size self.m_backwards_offset = backwards_offset def _orthonormalize_rotation(self, T): """Takes a 4x4 matrix and orthonormalizes the upper left 3x3 using SVD Returns: T with orthonormalized upper 3x3 """ R = T[:3, :3] t = T[:3, 3] # see https://arxiv.org/pdf/2006.14616.pdf Eq.2 U, S, Vt = svd(R) Z = np.eye(3) Z[-1, -1] = np.sign(np.linalg.det(U @ Vt)) R = U @ Z @ Vt T = np.eye(4) # recreate the matrix to make sure that the forth row is [0 0 0 1] T[:3, :3] = R T[:3, 3] = t return T def update_camera(self, view): """Update lazy camera with new view. Parameters: view: New camera view, 4x4 matrix """ observing_camera = view.copy() # push observing camera back in z-direction in camera space z_vec = np.zeros((3,)) z_vec[2] = 1 offset_vector = view[:3, :3] @ z_vec observing_camera[:3, 3] += offset_vector * self.m_backwards_offset # use moving avage of last X cameras (so that observing camera is smooth and follows with slight delay) self.m_camera_buffer.append(observing_camera) if len(self.m_camera_buffer) > self.m_camera_buffer_size: self.m_camera_buffer = self.m_camera_buffer[1:] def get_current_view(self): """Get current lazy camera view for rendering. Returns: 4x4 matrix """ # naive average of camera pose matrices smooth_camera_pose = np.zeros((4, 4)) for camera_pose in self.m_camera_buffer: smooth_camera_pose += camera_pose smooth_camera_pose /= len(self.m_camera_buffer) return self._orthonormalize_rotation(smooth_camera_pose) def get_camera_buffer(self): """ Return buffered camera views, e.g. for storing state. """ return self.m_camera_buffer class PointCloudBuffer: """Holds last N point clouds.""" def __init__(self, pc_buffer_size=500, use_mask=True): """Constructor. Parameters: pc_buffer_size: Number of last N point clouds to hold """ self.pc_buffer_size = pc_buffer_size self.use_mask = use_mask self.pc_xyz_buffer = [] self.pc_clr_buffer = [] self.pc_err_buffer = [] self.pc_mask_buffer = [] def set_mask_buffer(self, mask_buffer): self.pc_mask_buffer = mask_buffer def update_buffer(self, pc_xyz, pc_clr, pc_errs=None, pc_mask=None): """ Add a new (partial) point cloud to the buffer. @param pc_xyz: N3, coordinates of points @param pc_clr: N3, RGB colors of points @param pc_errs: N1, scalar errors of points """ self.pc_xyz_buffer.append(pc_xyz) self.pc_clr_buffer.append(pc_clr) self.pc_mask_buffer.append(pc_mask) # if self.use_mask and pc_mask is not None: # self.pc_mask_buffer.append(pc_mask) # print(f'current idx {len(self.pc_xyz_buffer)}, current shape {self.pc_xyz_buffer[-1].shape}') if pc_errs is not None: self.pc_err_buffer.append(pc_errs) # remove oldest xyz and clr entries in the buffer if buffer is full if 0 < self.pc_buffer_size < len(self.pc_xyz_buffer): self.pc_xyz_buffer = self.pc_xyz_buffer[1:] self.pc_clr_buffer = self.pc_clr_buffer[1:] # errs handled separately, because optional if 0 < self.pc_buffer_size < len(self.pc_err_buffer): self.pc_err_buffer = self.pc_err_buffer[1:] def clear_buffer(self): """ Clear the buffer. """ self.pc_xyz_buffer = [] self.pc_clr_buffer = [] self.pc_mask_buffer = [] # self.pc_err_buffer = [] def get_point_cloud(self): """ Merges and returns all point clouds in the buffer. @return: triple, N3 xyz + N3 colors + N1 errors """ # combine PC chunks of current frame to single PC merged_xyz = np.concatenate(self.pc_xyz_buffer) merged_clr = np.concatenate(self.pc_clr_buffer) merged_mask = np.concatenate(self.pc_mask_buffer) if len(self.pc_err_buffer) > 0: merged_errs = np.concatenate(self.pc_err_buffer) else: merged_errs = None masked_xyz = merged_xyz[merged_mask.astype(bool)] masked_clr = merged_clr[merged_mask.astype(bool)] masked_errs = merged_errs[merged_mask.astype(bool)] if merged_errs is not None else None # return merged_xyz, merged_clr, merged_errs return masked_xyz, masked_clr, masked_errs def disable_buffer_cap(self): """ Switch rolling buffer of fixed size to unconstrained buffer. """ self.pc_buffer_size = -1 def get_retro_colors(): """ Create custom color map, dark magenta to bright cyan. if you like this color map and use it in your own work, let me know https://twitter.com/eric_brachmann looking forward to seeing what you do with it :) -- Eric @return: Color lookup table, 256x3 """ cdict = {'red': [ [0.0, 0.073, 0.073], [0.4, 0.325, 0.325], [0.7, 0.286, 0.286], [0.85, 0.266, 0.266], [0.95, 0, 0], [1, 1, 1], ], 'green': [ [0.0, 0.0, 0.0], [0.4, 0.058, 0.058], [0.7, 0.470, 0.470], [0.85, 0.827, 0.827], [0.95, 1, 1], [1, 1, 1], ], 'blue': [ [0.0, 0.057, 0.057], [0.4, 0.223, 0.223], [0.7, 0.752, 0.752], [0.85, 0.988, 0.988], [0.95, 1, 1], [1, 1, 1], ]} retroColorMap = LinearSegmentedColormap('retroColors', segmentdata=cdict, N=256) return retroColorMap(np.linspace(0, 1, 257))[1:, :3] def get_point_cloud_from_network(network, data_loader, filter_depth, dense_cloud=False): """ Extract a point cloud from a fully trained network. @param network: scene coordinate regression network @param data_loader: loader for the mapping sequence @param filter_depth: in meters, remove points further from the camera @param dense_cloud: if True, return all points (good to initialise splats), otherwise filter based on repro error @return: tuple, N3 coordinates + N3 RGB colors """ # remove points where scene coordinates change more than this threshold from one pixel to the next (in meters) # since scene can have vastly different scales, and scales are estimates, we try increasingly relaxed thresholds grad_thresholds = [0.1, 0.5, 1.0, torch.inf] # total number of points in the point cloud, at least min even with large re-projection errors # at most max, even if more points have small re-projection errors pc_points_min = 100000 pc_points_max = 1000000 # remove points with re-projection larger than threshold (in px) as long as we keep a min number of points repro_threshold = 1 if dense_cloud: # disable checks to return random points per image grad_thresholds = [torch.inf] repro_threshold = torch.inf pc_points_per_image_min = int(pc_points_min / len(data_loader)) pc_points_per_image_max = int(pc_points_max / len(data_loader)) pixel_grid = get_pixel_grid(network.OUTPUT_SUBSAMPLE) # Shape: 2x5000x5000 pc_xyz = [] pc_clr = [] pc_mask = [] file_list = [] with torch.no_grad(): # iterate over mapping sequence for image, _, gt_inv_pose, _, K, _, _, file, _ in data_loader: # predict scene coordinate image = image.cuda(non_blocking=True) gt_inv_pose = gt_inv_pose.cuda(non_blocking=True) K = K.cuda(non_blocking=True) with autocast(): scene_coords = network(image) B, C, H, W = scene_coords.shape assert B == 1, "Batch size must be 1 for point cloud extraction." # scene coordinate to camera coordinates pred_scene_coords_B3HW = scene_coords.float() pred_scene_coords_B4N = to_homogeneous(pred_scene_coords_B3HW.flatten(2)) pred_cam_coords_B3N = torch.matmul(gt_inv_pose[:, :3], pred_scene_coords_B4N) # project scene coordinates pred_px_B3N = torch.matmul(K, pred_cam_coords_B3N) pred_px_B3N[:, 2].clamp_(min=0.1) # avoid division by zero pred_px_B2N = pred_px_B3N[:, :2] / pred_px_B3N[:, 2, None] # measure reprojection error pixel_positions_2HW = pixel_grid[:, :H, :W].clone() # Crop to actual size pixel_positions_2N = pixel_positions_2HW.view(2, -1) reprojection_error_2N = pred_px_B2N.squeeze() - pixel_positions_2N.cuda() reprojection_error_1N = torch.norm(reprojection_error_2N, dim=0, keepdim=True, p=1) # filter based on gradient of scene coordinates grad_x_BHW = torch.linalg.norm(pred_scene_coords_B3HW[:, :, :, 1:] - pred_scene_coords_B3HW[:, :, :, :-1], dim=1) grad_x_BHW = torch.nn.functional.pad(grad_x_BHW, (1, 0), mode='reflect') grad_y_BHW = torch.linalg.norm(pred_scene_coords_B3HW[:, :, 1:, :] - pred_scene_coords_B3HW[:, :, :-1, :], dim=1) grad_y_BHW = torch.nn.functional.pad(grad_y_BHW, (0, 0, 1, 0), mode='reflect') grad_BHW = torch.max(grad_x_BHW, grad_y_BHW) grad_1N = grad_BHW.view(B, -1) # try different grad thresholds, keep the tightest one that still has enough points for grad_threshold in grad_thresholds: sc_grad_mask = grad_1N.squeeze() < grad_threshold if sc_grad_mask.sum() > pc_points_per_image_min: break # filter predictions based on depth sc_depth_mask = pred_cam_coords_B3N[0, 2] < filter_depth sc_grad_and_depth_mask = torch.logical_and(sc_grad_mask, sc_depth_mask) # if no points survive, keep all if sc_grad_and_depth_mask.sum() == 0: sc_grad_and_depth_mask[:] = True # apply reprojection error sc_err_mask = reprojection_error_1N.squeeze() < repro_threshold sc_err_mask = torch.logical_and(sc_err_mask, sc_grad_and_depth_mask) # check whether enough point survive num_valid_points = int(sc_err_mask.sum()) if num_valid_points < pc_points_per_image_min: # take min points with lowest reprojection error reprojection_error_within_range_and_smooth_1N = reprojection_error_1N.squeeze()[sc_grad_and_depth_mask] sorted_errors, _ = torch.sort(reprojection_error_within_range_and_smooth_1N) relaxed_filter_repro_error = sorted_errors[min(pc_points_per_image_min, sorted_errors.shape[0] - 1)] sc_err_mask = reprojection_error_1N.squeeze() < relaxed_filter_repro_error sc_err_mask = torch.logical_and(sc_grad_and_depth_mask, sc_err_mask) elif num_valid_points > pc_points_per_image_max: # sub-sample points keep_ratio = pc_points_per_image_max / num_valid_points sub_sample_mask = torch.randperm(num_valid_points) < int(keep_ratio * num_valid_points) sc_err_mask_subsampled = sc_err_mask.clone() sc_err_mask_subsampled[sc_err_mask] = sub_sample_mask.cuda() sc_err_mask = sc_err_mask_subsampled # load image file to extract colors rgb = io.imread(file[0]) if len(rgb.shape) < 3: rgb = color.gray2rgb(rgb) # align RGB values with scene coordinate prediction rgb = rgb.astype('float64') # firstly, resize image to network input resolution rgb = resize(rgb, image.shape[2:]) # secondly, sub-sampling to network output resolution # using nearest neighbour subsampling results in slightly crisper colors nn_stride = network.OUTPUT_SUBSAMPLE nn_offset = network.OUTPUT_SUBSAMPLE // 2 rgb = rgb[nn_offset::nn_stride, nn_offset::nn_stride, :] # make sure the resolution fits (catch any striding mismatches) rgb = resize(rgb, scene_coords.shape[2:]) rgb = torch.from_numpy(rgb).permute(2, 0, 1) rgb = rgb.contiguous().view(3, -1) # remove invalid map points rgb = rgb[:, sc_err_mask.cpu()] xyz = pred_scene_coords_B4N[0, :3, sc_err_mask].cpu() print('pred_scene_coords_B4N_shape', pred_scene_coords_B4N.shape) pc_xyz.append(xyz.numpy()) pc_clr.append(rgb.numpy()) pc_mask.append(sc_err_mask.cpu()) file_list.append(file[0]) # merge points pc_xyz = np.concatenate(pc_xyz, axis=1) pc_clr = np.concatenate(pc_clr, axis=1) # import pdb; pdb.set_trace() # 3N to N3 # import ipdb; ipdb.set_trace() pc_xyz = np.transpose(pc_xyz) pc_clr = np.transpose(pc_clr) # OpenCV to OpenGL convention pc_xyz[:, 1] = -pc_xyz[:, 1] pc_xyz[:, 2] = -pc_xyz[:, 2] # return merged frame points return pc_xyz, pc_clr, pc_mask, file_list, image.shape[2:], scene_coords.shape[2:] # return pc_xyz, pc_clr def get_rendering_target_path(target_base_path, map_file_name): """ Infer a folder for renderings from a base path and a map name. Creates target folder if it does not exist. @param target_base_path: Base path for all renderings. @param map_file_name: Map file name to infer folder name for renderings of this mapping run. @return: path to store renderings """ target_path = map_file_name # infer rendering folder from map file name target_path = os.path.basename(target_path) # extract file name target_path = os.path.splitext(target_path)[0] # remove extension target_path = target_base_path / target_path os.makedirs(target_path, exist_ok=True) return target_path class CameraTrajectoryBuffer: """Incrementally builds a camera trajectory mesh.""" def __init__(self, frustum_skip, frustum_scale): """ Constructor. Initialises standard values. @param frustum_skip: minimum distance between placing frustums, in meters @param frustum_scale: Scaling factor for camera frustums """ self.frustum_skip = frustum_skip self.frustum_scale = frustum_scale self.trajectory = [] # holds line segments to render the camera path of the mapping sequence self.frustums = [] # holds frustum geometry for the trajectory self.frustum_images = [] # frustum images need to be kept extra due to image texture self.trajectory_previous = None # holds last camera position to skip segments if camera jumps self.frustum_positions = [] # holds accepted frustum placement positions to sparsify them self.trajectory_distances = [] # holds all previous distances in the trajectory to detect jumps self.trajectory_color = (255, 255, 255) self.aspect_ratio_buffer = 4 / 3 # default aspect ratio, overwritten as soon as a acutal image is loaded global THICKNESS if frustum_scale < 10: THICKNESS = 0.005 def grow_camera_path(self, new_camera): """ Expand the camera trajectory line wrt new camera. Keeps track of camera movement statistics and skips the line if a camera jump is detected. @param new_camera: 4x4 camera pose, OpenGL convention """ # get position of mapping camera current_pos = new_camera[:3, 3] # draw line from previous position to current position if self.trajectory_previous is not None: current_dist = np.linalg.norm(current_pos - self.trajectory_previous) # keep sorted list of previous camera distance insort(self.trajectory_distances, current_dist) # detect jump if current dist is more than X times the median line_skip = 10 * self.trajectory_distances[len(self.trajectory_distances) // 2] if 0.0001 < current_dist < line_skip: line_cuboid = cuboid_from_line(line_start=self.trajectory_previous, line_end=current_pos, color=self.trajectory_color) self.trajectory.append(line_cuboid) else: if current_dist > line_skip: _logger.info(f"Detected jump: camera dist={current_dist:.3f}, threshold={line_skip:.3f}, " f"threshold estimated from {len(self.trajectory_distances)} estimates.") # update previous position for next iteration self.trajectory_previous = current_pos def add_position_marker(self, marker_pose, marker_color, marker_extent=0.015, frustum_maker=False): """ Adds a cube to the trajectory mesh to signify a singular camera position. @param marker_pose: 4x4 camera pose, OpenGL convention @param marker_color: RGB color of the marker @param marker_extent: size of the marker, marker is a cube of this side length """ # import pdb; pdb.set_trace() if frustum_maker: current_pos_marker = generate_frustum_marker(marker_pose, marker_color, marker_extent) else: current_pos_marker = trimesh.primitives.Box( extents=(marker_extent, marker_extent, marker_extent), transform=marker_pose) for c in (0, 1, 2): # import pdb; pdb.set_trace() current_pos_marker.visual.vertex_colors[:, c] = marker_color[c] self.trajectory.append(current_pos_marker) def _get_closest_frustum_distance(self, new_camera): """ Calculate distance to the closest, previously placed frustum in the trajectory so far. @param new_camera: 4x4 camera, OpenGL convention @return: distance to the closest frustum in the trajectory """ if len(self.frustum_positions) == 0: return self.frustum_skip + 1 # hack, return a distance that always accepts the new camera else: distances = [np.linalg.norm(pos - new_camera[:3, 3]) for pos in self.frustum_positions] return min(distances) def add_camera_frustum(self, camera, image_file=None, sparse=True, frustum_color=None): """ Add a camera frustum object to the trajectory, minding distance to existing frustums. @param camera: 4x4 camera pose, OpenGL convention @param image_file: path to image to be displayed in frustum @param sparse: flag, if true a frustum is not placed if too close to existing frustums @param frustum_color: RGB color, if none default color is used """ new_camera = camera.copy() if frustum_color is None: frustum_color = self.trajectory_color # place camera frustum all X centimeters (or overwrite via sparse flag) if (sparse == False) or (self._get_closest_frustum_distance(new_camera) > self.frustum_skip): if image_file is not None: image_mesh, self.aspect_ratio_buffer = get_image_box(image_path=image_file, frustum_pose=new_camera, flip=True, cam_marker_size=self.frustum_scale) image_mesh = pyrender.Mesh.from_trimesh(image_mesh) self.frustum_images.append(image_mesh) # print(f'generate_frustum_at_position {new_camera}') frustum = generate_frustum_at_position(rotation=new_camera[:3, :3], translation=new_camera[:3, 3], color=frustum_color, size=self.frustum_scale, aspect_ratio=self.aspect_ratio_buffer) # print(f'self.frustum_scale {self.frustum_scale}') self.frustums.append(frustum) self.frustum_positions.append(new_camera[:3, 3]) def clear_frustums(self): """ Clear all existing frustums in the trajectory. """ self.frustums.clear() self.frustum_images.clear() self.frustum_previous = None def get_mesh(self): """ Turn trajectory into pyrender mesh. Frustum images are returned separately since merging textured and non-textured objects creates artifacts. @return: tuple, trajectory mesh + list of frustum image objects """ # concatenate line segments and frustums into a single mapping trajectory mesh trajectory_mesh = self.trajectory + self.frustums trajectory_mesh = trimesh.util.concatenate(trajectory_mesh) trajectory_mesh = pyrender.Mesh.from_trimesh(trajectory_mesh) return trajectory_mesh, self.frustum_images