def _read_txt(self, stream): ''' Load a PLY element from an ASCII-format PLY file. The element may contain list properties. ''' self._data = _np.empty(self.count, dtype=self.dtype()) k = 0 for line in _islice(iter(stream.readline, b''), self.count): fields = iter(line.strip().split()) for prop in self.properties: try: self._data[prop.name][k] = prop._from_fields(fields) except StopIteration: raise PlyElementParseError("early end-of-line", self, k, prop) except ValueError: raise PlyElementParseError("malformed input", self, k, prop) try: next(fields) except StopIteration: pass else: from Utility.Logging_Extension import logger logger.vinfo('fields', ' '.join(fields)) logger.vinfo('k', ' '.join(k)) raise PlyElementParseError("expected end-of-line", self, k) k += 1 if k < self.count: del self._data raise PlyElementParseError("early end-of-file", self, k)
def copy_vehicle_for_each_time_step(gt_general_idp, gt_specific_dp, lazy): vehicle_model_name = os.path.basename( os.path.dirname(gt_specific_dp)) vehicle_model_ply_ifp = os.path.join( gt_general_idp, vehicle_model_name, 'CarBody.ply') trajectory_ifp = os.path.join( gt_specific_dp, 'animation_transformations.txt') camera_object_trajectory = TrajectoryFileHandler.parse_camera_and_object_trajectory_file( trajectory_ifp) vehicle_model_odp = os.path.join( gt_specific_dp, 'object_ground_truth_in_world_ground_truth_coordinates') mkdir_safely(vehicle_model_odp) for frame_name in camera_object_trajectory.get_frame_names_sorted(): logger.vinfo('frame_name', frame_name) obj_matrix_world = camera_object_trajectory.get_object_matrix_world(frame_name) vehicle_model_ply_ofp = os.path.join(vehicle_model_odp, frame_name + '.ply') CloudCompare.apply_transformation( vehicle_model_ply_ifp, obj_matrix_world, vehicle_model_ply_ofp, save_point_clouds=False, save_meshes=True, lazy=True)
def write_MVS_colmap_file(points, dense_id_to_file_name, mvs_colmap_ofp): logger.info('write_MVS_colmap_file: ...') logger.vinfo('mvs_colmap_ofp', mvs_colmap_ofp) mvs_content = [] mvs_content.append('# DENSE_IMAGE_ID to image_name') mvs_content.append(dense_id_to_file_name.size()) for dense_id, file_name in dense_id_to_file_name.items(): mvs_content.append(str(dense_id) + ' ' + file_name) mvs_content.append( '# 3D point list with one line of data per point:\n') mvs_content.append( '# POINT3D_ID, X, Y, Z, NX, NY, NZ, R, G, B, TRACK[] as (DENSE_ID, DENSE_COL, DENSE_ROW)\n' ) mvs_content.append('# Number of points: x, mean track length: 0\n') for point in points: # From the docs: # <Point> = <XYZ> <RGB> <number of measurements> <List of Measurements> current_line = ' '.join(list(map(str, point.coord))) current_line += ' ' + ' '.join(list(map(str, point.normal))) current_line += ' ' + ' '.join(list(map(str, point.color))) if point.measurements is not None: for measurement in point.measurements: current_line += ' ' + str(measurement.camera_index) current_line += ' ' + str(measurement.x) # x = col current_line += ' ' + str(measurement.y) # y = row mvs_content.append(current_line + ' ' + '\n') with open(mvs_colmap_ofp, 'wb') as output_file: output_file.writelines([item.encode() for item in mvs_content]) logger.info('write_MVS_colmap_file: Done')
def compute_gt_lines_per_frame(content_list): logger.info('compute_gt_lines_per_frame: ...') # logger.vinfo('content_list', content_list) content_list = [x.strip() for x in content_list] frame_idx_list = [] for idx, content_line in enumerate(content_list): if 'frame' in content_line: # DONT test for 'jpg' in content_line, since that is not true for the legacy data structure frame_idx_list.append(idx) # logger.vinfo('frame_idx_list', frame_idx_list) num_lines_per_frame = None for idx in range(len(frame_idx_list))[1:]: gt_lines_per_frame_tmp = frame_idx_list[idx] - frame_idx_list[idx - 1] if num_lines_per_frame is not None: assert num_lines_per_frame == gt_lines_per_frame_tmp num_lines_per_frame = gt_lines_per_frame_tmp # logger.vinfo('num_lines_per_frame', num_lines_per_frame) assert num_lines_per_frame is not None if not num_lines_per_frame in TrajectoryFileHandler.gt_lines_per_frame_values: logger.vinfo('num_lines_per_frame', num_lines_per_frame) assert False # split content per frame (index) lines_per_frame = list( TrajectoryFileHandler._chunks(content_list, num_lines_per_frame)) logger.info('compute_gt_lines_per_frame: Done') return lines_per_frame, num_lines_per_frame
def create_object_to_world_transformation_files( rec_method_transf_dir, camera_object_trajectory, world_to_world_transf=np.identity(4)): logger.info('create_object_to_world_transformation_files: ...') logger.vinfo( 'path_to_reconstruction_method_transformation_file_folder', rec_method_transf_dir) if not os.path.isdir(rec_method_transf_dir): os.mkdir(rec_method_transf_dir) for frame_name in sorted( camera_object_trajectory.get_frame_names_sorted()): transformation_file_path = os.path.join(rec_method_transf_dir, str(frame_name) + '.txt') logger.vinfo('frame_name', frame_name) #object_pose = camera_object_trajectory[frame_name]['object_pose'] object_matrix_world = camera_object_trajectory.get_object_matrix_world( frame_name) np.savetxt(transformation_file_path, world_to_world_transf.dot(object_matrix_world)) # with open(transformation_file_path, 'w') as output_file: # output_file.writelines([item for item in file_content]) logger.info('create_object_to_world_transformation_files: Done')
def read_image_from_path_as_rgb(path_to_image): if not os.path.isfile(path_to_image): logger.vinfo('path_to_image', path_to_image) assert False grb_image = None bgr_image = cv2.imread(path_to_image, cv2.IMREAD_COLOR) if bgr_image is not None: grb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) return grb_image
def parse_ply_file(path_to_file): logger.info('Parse PLY File: ...') logger.vinfo('path_to_file', path_to_file) ply_data = PlyData.read(path_to_file) vertices, _, _ = PLYFileHandler.__ply_data_vertices_to_vetex_list( ply_data) faces, _, _ = PLYFileHandler.__ply_data_faces_to_face_list(ply_data) logger.info('Parse PLY File: Done') return vertices, faces
def parse_reconstruction_file(ifp): if os.path.splitext(ifp)[1] == '.nvm': cameras, points = NVMFileHandler.parse_nvm_file(ifp) elif os.path.splitext(ifp)[1] == '.ply': points, faces = PLYFileHandler.parse_ply_file(ifp) cameras = [] else: logger.vinfo('ext', os.path.splitext(ifp)[1]) logger.info('Input file type Not implemented yet') assert False return cameras, points
def set_active_cam_intrinsics(self, calibration_np_mat, width, height, max_clipping_range=sys.float_info.max): active_vtk_camera = self.vtk_renderer.GetActiveCamera() focal_length = calibration_np_mat[0][0] if not calibration_np_mat[0][0] == calibration_np_mat[1][1]: logger.vinfo('calibration_mat', calibration_np_mat) assert False view_angle = Camera.compute_view_angle(focal_length, width, height) active_vtk_camera.SetViewAngle(view_angle) active_vtk_camera.SetClippingRange(0.0, max_clipping_range)
def parse_optical_flow_exr_file(path_to_exr_file): logger.info('parse_optical_flow_exr_file: ...') logger.vinfo('path_to_exr_file', path_to_exr_file) pt = Imath.PixelType(Imath.PixelType.FLOAT) exr_file = OpenEXR.InputFile(path_to_exr_file) # To examine the content of the exr file use: logger.vinfo('exr_file.header()', exr_file.header()) dw = exr_file.header()['dataWindow'] size = (dw.max.x - dw.min.x + 1, dw.max.y - dw.min.y + 1) logger.vinfo('size', size) red_channel_str = exr_file.channel('R', pt) red_exr_data_as_np = np.fromstring(red_channel_str, dtype=np.float32) red_exr_data_as_np.shape = (size[1], size[0] ) # Numpy arrays are (row, col) green_channel_str = exr_file.channel('G', pt) green_exr_data_as_np = np.fromstring(green_channel_str, dtype=np.float32) green_exr_data_as_np.shape = (size[1], size[0] ) # Numpy arrays are (row, col) exr_data_as_np = np.dstack((red_exr_data_as_np, green_exr_data_as_np)) logger.vinfo('shape', exr_data_as_np.shape) logger.info('parse_optical_flow_exr_file: Done') return exr_data_as_np
def recenter_objects_if_necessary(tire_fl_name, tire_fr_name, tire_bl_name, tire_br_name): logger.info('recenter_objects_if_necessary: ...') front_axle_center = compute_xy_axle_center(tire_fl_name, tire_fr_name) logger.vinfo('front_axle_center', front_axle_center) front_axle_center_xy0 = front_axle_center.xyz front_axle_center_xy0.z = 0 back_axle_center = compute_xy_axle_center(tire_bl_name, tire_br_name) logger.vinfo('back_axle_center', back_axle_center) back_axle_center_xy0 = back_axle_center.xyz back_axle_center_xy0.z = 0 axle_diff = front_axle_center_xy0 - back_axle_center_xy0 y_is_dir_axis = abs(axle_diff.x) < eps x_is_dir_axis = abs(axle_diff.y) < eps if not (y_is_dir_axis is not x_is_dir_axis): logger.vinfo('y_is_dir_axis', y_is_dir_axis) logger.vinfo('x_is_dir_axis', x_is_dir_axis) assert False if y_is_dir_axis: if abs(front_axle_center.x) > eps: for obj in bpy.data.objects: obj.location.x -= front_axle_center.x elif x_is_dir_axis: if (front_axle_center.y) > eps: for obj in bpy.data.objects: obj.location.y -= front_axle_center.y else: assert False logger.info('recenter_objects_if_necessary: Done')
def merge_files(list_w_cam_ifp, list_w_cam_colors, list_wo_cam_ifp, list_wo_cam_colors, ofp_or_odp, overwrite=False): """ :param list_w_cam_ifp: use camera information contained in these files :param list_w_cam_colors: :param list_wo_cam_ifp: ignore camera information contained in these files :param list_wo_cam_colors: :param ofp_or_odp: output file extension :return: """ if overwrite is False: if os.path.isfile(ofp_or_odp): assert False if os.path.isdir(ofp_or_odp) and os.listdir(ofp_or_odp) != []: assert False assert len(list_w_cam_ifp) == len(list_w_cam_colors) assert len(list_wo_cam_ifp) == len(list_wo_cam_colors) cams = [] points = [] for ifp, color in zip(list_w_cam_ifp, list_w_cam_colors): c_cams, c_points = ReconstructionFileConverter.parse_reconstruction_file(ifp) cams += c_cams if color is not None: for point in c_points: point.color = color points += c_points for ifp, color in zip(list_wo_cam_ifp, list_wo_cam_colors): _, c_points = ReconstructionFileConverter.parse_reconstruction_file(ifp) if color is not None: for point in c_points: point.set_color(color) points += c_points # adjust the point ids (after parsing duplicates exists) for index, point in enumerate(points): point.id = index logger.vinfo('len(points)', len(points)) ReconstructionFileConverter.write_reconstruction_file( cams, points, ofp_or_odp)
def perform_post_processing(directory_path, remove_stereo_files, create_object_files, create_background_files, create_ground_files, create_depth_files, create_videos, lazy): logger.info('perform_post_processing: ...') logger.vinfo('directory_path: ', directory_path) perform_image_post_processing(directory_path, remove_stereo_files, create_object_files, create_background_files, create_ground_files, create_depth_files, create_videos, lazy) perform_gt_post_processing(directory_path) logger.info('perform_post_processing: Done')
def high_level_object_import_from_other_blend_file(blend_file, folder_name, target_name): # Several Possibilities # Option: bpy.ops.wm.link() # the object can only be edited in the original file # Option: bpy.ops.wm.append() # the object is copied and can be edited if not os.path.isfile(blend_file): logger.vinfo('blend_file', blend_file) assert False # Invalid Input Path file_path = os.path.join(blend_file, folder_name, target_name) directory = ensure_trailing_slash(os.path.join(blend_file, folder_name)) bpy.ops.wm.append( filepath=file_path, filename=target_name, directory=directory)
def post_process_ground_truth(gt_specific_dp, gt_general_idp): logger.info('create_ground_truth: ...') logger.vinfo('gt_specific_idp', gt_specific_dp) logger.vinfo('gt_general_idp', gt_general_idp) copy_ground( gt_general_idp, gt_specific_dp, lazy=True) copy_vehicle_for_each_time_step( gt_general_idp, gt_specific_dp, lazy=True) logger.info('create_ground_truth: Done')
def __init__(self, path_to_config_file, working_file_suffix=None): self.path_to_config_file = path_to_config_file self.config = configparser.RawConfigParser() if not os.path.isfile(self.path_to_config_file): abs_path = os.path.abspath( os.path.dirname(self.path_to_config_file)) if not os.path.isdir(abs_path): logger.vinfo('abs_path', abs_path) assert False # config folder missing open(self.path_to_config_file, 'a').close() else: self.config.read(self.path_to_config_file) if working_file_suffix is not None: self.path_to_working_copy = self.path_to_config_file + working_file_suffix else: self.path_to_working_copy = self.path_to_config_file
def get_cached_result(self, callback, params, unique_id_or_path): if type(unique_id_or_path) == int: cache_fp = os.path.join(self.tmp_dir, str(unique_id_or_path) + '.cache') elif type(unique_id_or_path) == str: cache_fp = unique_id_or_path else: assert False if os.path.isfile(cache_fp): logger.vinfo('Reading data from dill cache: ', cache_fp) with open(cache_fp, 'rb') as file: result = dill.load(file) else: logger.info('Reading data from txt reconstruction') with open(cache_fp, 'wb') as file: result = callback(*params) dill.dump(result, file) return result
def update_measurements_with_point_projections_debug( self, points_background, show_points_in_fov=False, show_non_occluded_points=False, debug_offset=0): logger.info('update_measurements_with_point_projections: ...') from Utility.Image.Image_Drawing_Interface import ImageDrawingInterface for camera_idx in sorted( self.camera_index_to_camera.keys()[debug_offset:]): logger.vinfo('camera_idx', camera_idx) camera = self.camera_index_to_camera[camera_idx] #camera.set_principal_point([1024, 512]) logger.info('principal point: ' + str(camera.get_principal_point())) image_like_path = os.path.join(self.image_folder_path, camera.file_name) non_occluded_pixels = [] for point in points_background: visible_camera_ids = [ measurement.camera_index for measurement in point.measurements ] if camera_idx in visible_camera_ids: image_point, visible = camera.project_single_woorld_coord_into_camera_image_as_image_coord( point.coord) if visible: non_occluded_pixels.append(image_point) if show_non_occluded_points: ImageDrawingInterface.show_pixel_positions_in_image( non_occluded_pixels, image_like_path, title='non occluded points', single_color=(255, 0, 0)) ImageDrawingInterface.show_pixel_positions_in_image( non_occluded_pixels, image_like_path)
def create_poly_data_from_ply(ply_ifp, report_statistics=False): # The color information is stored in # vtk_colors = poly_data.GetPointData().GetScalars() # In order to modify the colors use # poly_data.GetPointData().SetScalars(vtk_colors) reader_ply = vtk.vtkPLYReader() # Returns vtkPolyData reader_ply.SetFileName(ply_ifp) reader_ply.Update() poly_data = reader_ply.GetOutput() if report_statistics: logger.vinfo('Number of points', poly_data.GetNumberOfPoints()) logger.vinfo('Number of polys', poly_data.GetNumberOfPolys()) if poly_data.GetNumberOfPoints() == 0: raise ValueError("No point data could be loaded from '" + ply_ifp) return None return poly_data
def apply_single_transformation_to_single_model(ifp_model, ifp_transformation, ofp_model, save_point_clouds=False, save_meshes=False, lazy=True, log_info=True): if log_info: logger.info('apply_single_transformation_to_model: ...') logger.vinfo('output_path_to_transformed_model_file', ofp_model) CloudCompare.apply_transformation_from_file(ifp_model, ifp_transformation, ofp_model, save_point_clouds, save_meshes, lazy) if log_info: logger.info('apply_single_transformation_to_model: Done')
def parse_openmvg_file(input_openMVG_file_path): """ The path_to_input_files parameter is optional, if provided the returned points carry also color information :param input_openMVG_file_path: :param path_to_input_files: Path to the input images (used to infer the color of the structural points) :return: """ logger.info('parse_openmvg_file: ...') logger.vinfo('input_openMVG_file_path', input_openMVG_file_path) input_file = open(input_openMVG_file_path, 'r') json_data = json.load(input_file) path_to_input_files = os.path.abspath(json_data['root_path']) cams, image_index_to_camera_index = OpenMVGFileHandler.parse_cameras(json_data) view_index_to_file_name = {cam.view_index: cam.file_name for cam in cams} points = OpenMVGFileHandler.parse_points( json_data, image_index_to_camera_index, path_to_input_files, view_index_to_file_name) logger.info('parse_openmvg_file: Done') return cams, points
def add_bone_constraint_keyframe(object_name, bone_name, constraint_name, frame_number, constraint_attribute_name, constraint_attribute_value): """ https://docs.blender.org/api/blender_python_api_current/bpy.ops.ui.html#bpy.ops.ui.copy_data_path_button Copy the RNA data path for this property to the clipboard Adds a Keyframe to an ARBITRARY constraint attribute (-> constraint_attribute_name) :param object_name: :param bone_name: :param constraint_name: :param frame_number: :param constraint_attribute_name :param constraint_attribute_value: :return: """ logger.info('add_bone_constraint_keyframe: ...') logger.vinfo('constraint_attribute_name', constraint_attribute_name) logger.vinfo('constraint_attribute_value', constraint_attribute_value) bpy.ops.object.mode_set(mode='POSE') # set_mode(active_object_name=object_name, mode='POSE', configure_scene_for_basic_ops=False) keyframe_object = bpy.data.objects[object_name] # bpy.ops.anim.change_frame(frame=frame_number) # bpy.context.object.pose.bones["MainControl"].constraints["Follow Path"].offset_factor = 0.01 # bpy.context.object.keyframe_insert( # data_path='pose.bones["MainControl"].constraints["Follow Path"].offset_factor', frame=1) # set the attribute to value setattr(keyframe_object.pose.bones[bone_name].constraints[constraint_name], constraint_attribute_name, constraint_attribute_value) keyframe_object.keyframe_insert( data_path='pose.bones[' + _enclose_with_quotation_mark(bone_name) + ']' + '.constraints[' + _enclose_with_quotation_mark(constraint_name) + '].' + constraint_attribute_name, frame=frame_number) bpy.ops.object.mode_set(mode='OBJECT') logger.info('add_bone_constraint_keyframe: Done')
def get_computer_vision_camera_matrix(blender_camera): """ Blender and Computer Vision Camera Coordinate Frame Systems (like VisualSfM, Bundler) differ by their y and z axis :param blender_camera: :return: """ # Only if the objects have a scale of 1, # the 3x3 part of the corresponding matrix_world contains a pure rotation # Otherwise it also contains scale or shear information if tuple(blender_camera.scale) != (1, 1, 1): logger.vinfo('blender_camera.scale', blender_camera.scale) assert False opengl_cam_mat = np.array(blender_camera.matrix_world) computer_vision_cam_mat = convert_opengl_to_computer_vision_camera( opengl_cam_mat) return computer_vision_cam_mat
def update_measurements_with_point_projections_efficient( self, points_background_original): import copy points_background_mod = copy.deepcopy(points_background_original) num_invalid_projections = 0 repr_errors = [] for index, point in enumerate(points_background_mod): if not index % 10000: logger.vinfo('index', index) for measurement in point.measurements: camera = self.camera_index_to_camera[measurement.camera_index] image_point, visible = \ camera.project_single_woorld_coord_into_camera_image_as_image_coord( point.coord) if visible: #repr_error = camera.compute_reprojection_error_single_point(point) #repr_errors.append(repr_error) # Replace previous measurements and clip them if necessary measurement.x = min(camera.width - 1, image_point[0]) measurement.y = min(camera.height - 1, image_point[1]) else: num_invalid_projections += 1 if len(repr_errors) > 0: mean_repr_error = np.mean(repr_errors) logger.vinfo('mean_repr_error', mean_repr_error) logger.vinfo('num_invalid_projections', num_invalid_projections) return points_background_mod
def compute_reprojection_errors(cams, points, image_path, max_allowed_mean_projection_error_in_pix, sparse_reconstruction_type): """ All points with a measurement in a certain image must be visible in the corresponding image :param cams: :param points: :return: """ logger.info('verify_cams_and_points: ...') rec = Reconstruction(cams, points, image_path, sparse_reconstruction_type) logger.vinfo('len(points)', len(points)) repr_errors = [] for iteration, point in enumerate(points): for measurement in point.measurements: cam = rec.get_camera_with_camera_index( measurement.camera_index) repro_error = cam.compute_reprojection_error_single_point( point) repr_errors.append(repro_error) logger.vinfo('max_repr_error', max(repr_errors)) mean_repr_error = np.mean(repr_errors) logger.vinfo('mean_repr_error', mean_repr_error) assert mean_repr_error < max_allowed_mean_projection_error_in_pix logger.info('verify_cams_and_points: Done')
def write_ply_file(ofp, vertices, with_colors=True, with_normals=False, faces=None, plain_text_output=False, with_measurements=False): logger.info('write_ply_file: ' + ofp) ply_data_vertex_data_dtype_list = PLYFileHandler.build_type_list( vertices, with_colors, with_normals, with_measurements) logger.vinfo('ply_data_vertex_data_dtype_list', ply_data_vertex_data_dtype_list) # PRINTING output_ply_data_vertex_element SHOWS ONLY THE HEADER output_ply_data_vertex_element = PLYFileHandler.__vertices_to_ply_vertex_element( vertices, ply_data_vertex_data_dtype_list) if faces is None or len(faces) == 0: logger.info('Write File With Vertices Only (no faces)') output_data = PlyData([output_ply_data_vertex_element], text=plain_text_output) else: logger.info('Write File With Faces') logger.info('Number faces' + str(len(faces))) ply_data_face_data_type = [('vertex_indices', 'i4', (3, ))] # we do not define colors for faces, # since we use the vertex colors to colorize the face output_ply_data_face_element = PLYFileHandler.__faces_to_ply_face_element( faces, ply_data_face_data_type) output_data = PlyData( [output_ply_data_vertex_element, output_ply_data_face_element], text=plain_text_output) output_data.write(ofp)
def apply_transformation(ifp_model, transformation_mat, ofp_model, save_point_clouds, save_meshes, lazy=False): if not isinstance(transformation_mat, np.ndarray): logger.vinfo('type(transformation_mat)', type(transformation_mat)) assert False unique_filename = uuid.uuid4().hex transformation_fp = os.path.join( os.path.dirname(CloudCompare.path_to_executable), unique_filename) np.savetxt(transformation_fp, transformation_mat) CloudCompare.apply_transformation_from_file(ifp_model, transformation_fp, ofp_model, save_point_clouds, save_meshes, lazy) os.remove(transformation_fp) assert not os.path.isfile(transformation_fp)
def check_pixel_mat(self, occupied_pixel_mask): occupied_pixel_flags = occupied_pixel_mask > 0 unique_values = set(occupied_pixel_flags.flatten().tolist()) if not True in unique_values: logger.vinfo('self.file_name', self.file_name) logger.vinfo('self.id', self.id) logger.vinfo('occupied_pixel_mask.shape', str(occupied_pixel_mask.shape)) assert False
def create_depth_viewer_node(scene): """ This will save the z buffer in the Viewer Node after rendering bpy.ops.render.render() rendered_image = bpy.data.images['Viewer Node'] pixels = rendered_image.pixels :param scene: :return: """ logger.info('create_depth_output_nodes: ...') scene.use_nodes = True scene_nodes = scene.node_tree.nodes scene_links = scene.node_tree.links default_render_layers_node = scene_nodes.get('Render Layers') # output_value = default_render_layers_node.outputs[output_type] # print(type(output_value)) viewer_node = scene_nodes.get('Depth Viewer') if viewer_node is None: viewer_node = scene_nodes.new('CompositorNodeViewer') viewer_node.name = 'Depth Viewer' logger.vinfo('viewer_node.name', viewer_node.name) viewer_node.use_alpha = False output_type = 'Depth' scene_links.new(default_render_layers_node.outputs[output_type], viewer_node.inputs[0]) # link Z to output logger.info('create_depth_output_nodes: Done')
def compute_intrinsic_skew_decomposition(intrinsic_mat): f_x, f_y, skew, p_x, p_y = Intrinsics.split_intrinsic_mat( intrinsic_mat) print("f_x", f_x) print("f_y", f_y) print("skew", skew) print("p_x", p_x) print("p_y", p_y) intrinsic_mat_wo_skew = np.array( [[f_x, 0, p_x - skew * p_y / f_y], [0, f_y, p_y], [0, 0, 1]], dtype=float) skew_mat = np.array([[1, skew / f_y, 0], [0, 1, 0], [0, 0, 1]], dtype=float) if not np.allclose(intrinsic_mat, skew_mat @ intrinsic_mat_wo_skew): err_mat = intrinsic_mat - skew_mat @ intrinsic_mat_wo_skew logger.vinfo("err_mat\n", err_mat) assert False return skew_mat, intrinsic_mat_wo_skew