import open3d as o3d import numpy as np import copy import time import argparse import os # ------------------------ 开始：对外接口，获取bbox数据 -------------------- """ 对外部提供的获取bbox数据的接口 compute_bbox_out 参数: obj_path: 模型数据路径 返回: total_matrix: 旋转矩阵, 16位浮点型, 例如, [[ 0.13644984 0.99064698 0. -49.71074343] [ -0.99064698 0.13644984 0. -28.80249299] [ 0. 0. 1. 3.26326203] [ 0. 0. 0. 1. ]] z_max : z最高点, 1位浮点型, 例如, 1.0 min_bound : bbox最低点, 3位浮点型, 例如, [-1.0, -1.0, 0.0] max_bound : bbox最低点, 3位浮点型, 例如, [0.0, 0.0, 1.0] ply_name : ply的名字, 字符串, 例如, 857420_268473_P85240_5cm_x1=9.41+49.997+49.997.ply """ def compute_bbox_out(obj_path): obj_name = os.path.basename(obj_path) mesh_obj = read_mesh(obj_path) return compute_bbox_ext(mesh_obj, obj_name) # -------------------------- 结束：对外接口，获取bbox数据 ---------------- # -------------------------- 开始：获取z值最低 -------------------------- def get_lowest_position_of_z_ext(mesh_obj): total_matrix = np.eye(4) voxel_size = 3 # print(f"obj_path={obj_path}, get_lowest_position_of_center voxel_size={voxel_size}") start_time1 = time.time() vertices = np.asarray(mesh_obj.vertices) # 确保网格有顶点 if len(vertices) == 0: # raise ValueError(f"Mesh has no vertices: {obj_path}") print(f"Warning: Mesh has no vertices: {mesh_obj}") return None pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(vertices) # print("voxel_size",voxel_size,obj_path, len(pcd.points), len(mesh_obj.vertices)) # 对点云进行下采样（体素网格法） #""" pcd_downsampled = down_sample(pcd, voxel_size) pcd_downsampled.paint_uniform_color([0, 0, 1]) if len(np.asarray(pcd_downsampled.points)) <= 0: bbox = pcd.get_axis_aligned_bounding_box() volume = bbox.volume() # print(f"len(pcd.points)={len(pcd.points)}, volume={volume}") # 处理体积为零的情况 if volume <= 0: # 计算点云的实际范围 points = np.asarray(pcd.points) if len(points) > 0: min_bound = np.min(points, axis=0) max_bound = np.max(points, axis=0) extent = max_bound - min_bound # 确保最小维度至少为0.01 min_dimension = max(0.01, np.min(extent)) volume = min_dimension ** 3 else: volume = 1.0 # 最后的安全回退 print(f"Warning: Zero volume detected, using approximated volume {volume:.6f} for {obj_path}") # 安全计算密度 - 防止除零错误 if len(pcd.points) > 0 and volume > 0: original_density = len(pcd.points) / volume voxel_size = max(0.01, min(10.0, 0.5 / (max(1e-6, original_density) ** 0.33))) else: # 当点数为0或体积为0时使用默认体素大小 voxel_size = 1.0 # 默认值 print(f"Recalculated voxel_size: {voxel_size} for {obj_path}") pcd_downsampled = down_sample(pcd, voxel_size) pcd_downsampled.paint_uniform_color([0, 0, 1]) original_num = len(pcd.points) target_samples = 1000 num_samples = min(target_samples, original_num) # print("get_lowest_position_of_center1 time", time.time()-start_time1) start_time2 = time.time() # 确保下采样后有点云 if len(np.asarray(pcd_downsampled.points)) == 0: # 使用原始点云作为后备 pcd_downsampled = pcd print(f"Warning: Using original point cloud for {obj_path} as downsampling produced no points") points = np.asarray(pcd_downsampled.points) # 初始化最小重心Y的值 max_z_of_mass_y = float('inf') best_angle_x, best_angle_y, best_angle_z = 0, 0, 0 best_angle_x, best_angle_y, best_angle_z, max_z_of_mass_y = parallel_rotation(points, angle_step=3) # 使用最佳角度进行旋转并平移obj pcd_transformed = copy.deepcopy(mesh_obj) # 最佳角度旋转 R_x = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([1, 0, 0]) * np.radians(best_angle_x)) pcd_transformed.rotate(R_x) R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * np.radians(best_angle_y)) pcd_transformed.rotate(R_y) R_z = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 0, 1]) * np.radians(best_angle_z)) pcd_transformed.rotate(R_z) T_x = np.eye(4) T_x[:3, :3] = R_x center_point = compute_mesh_center(mesh_obj.vertices) T_center_to_origin = np.eye(4) T_center_to_origin[:3, 3] = -center_point T_origin_to_center = np.eye(4) T_origin_to_center[:3, 3] = center_point T_rot_center = T_origin_to_center @ T_x @ T_center_to_origin total_matrix = T_rot_center @ total_matrix T_y = np.eye(4) T_y[:3, :3] = R_y center_point = compute_mesh_center(mesh_obj.vertices) T_center_to_origin = np.eye(4) T_center_to_origin[:3, 3] = -center_point T_origin_to_center = np.eye(4) T_origin_to_center[:3, 3] = center_point T_rot_center = T_origin_to_center @ T_y @ T_center_to_origin total_matrix = T_rot_center @ total_matrix T_z = np.eye(4) T_z[:3, :3] = R_z center_point = compute_mesh_center(mesh_obj.vertices) T_center_to_origin = np.eye(4) T_center_to_origin[:3, 3] = -center_point T_origin_to_center = np.eye(4) T_origin_to_center[:3, 3] = center_point T_rot_center = T_origin_to_center @ T_z @ T_center_to_origin total_matrix = T_rot_center @ total_matrix #试着旋转180，让脸朝上 vertices = np.asarray(pcd_transformed.vertices) # 计算平移向量，将最小Y值平移到0 min_z = np.min(vertices[:, 2]) translation_vector = np.array([0,0,-min_z,]) pcd_transformed.translate(translation_vector) T_trans1 = np.eye(4) T_trans1[:3, 3] = translation_vector total_matrix = T_trans1 @ total_matrix # 计算 z 坐标均值 vertices = np.asarray(pcd_transformed.vertices) z_mean1 = np.mean(vertices[:, 2]) z_max1 = np.max(vertices[:, 2]) volume_centroid = get_volume_centroid(pcd_transformed) z_volume_center1 = volume_centroid[2] angle_rad = np.pi #print("旋转前质心:", pcd_transformed.get_center()) #print("旋转前点示例:", np.asarray(pcd_transformed.vertices)[:3]) R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * angle_rad) pcd_transformed.translate(-center_point) pcd_transformed.rotate(R_y, center=(0, 0, 0)) pcd_transformed.translate(center_point) aabb = pcd_transformed.get_axis_aligned_bounding_box() # center_point = aabb.get_center() center_point = compute_mesh_center(mesh_obj.vertices) # 构建绕中心点旋转的变换矩阵[3](@ref) T_center_to_origin = np.eye(4) T_center_to_origin[:3, 3] = -center_point R_y180 = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * angle_rad) T_rotate = np.eye(4) T_rotate[:3, :3] = R_y180 T_origin_to_center = np.eye(4) T_origin_to_center[:3, 3] = center_point T_rot_center = T_origin_to_center @ T_rotate @ T_center_to_origin total_matrix = T_rot_center @ total_matrix #print("旋转后质心:", pcd_transformed.get_center()) #print("旋转后点示例:", np.asarray(pcd_transformed.vertices)[:3]) # vertices = np.asarray(pcd_transformed.vertices) # 计算平移向量，将最小Y值平移到0 min_z = np.min(vertices[:, 2]) max_z = np.max(vertices[:, 2]) # print("min_z1", min_z, obj_path) translation_vector = np.array([0,0,-min_z,]) # translation_vector = np.array([0,0,-min_z + (min_z-max_z),]) # print("translation_vector1",translation_vector) pcd_transformed.translate(translation_vector) T_trans2 = np.eye(4) T_trans2[:3, 3] = translation_vector translation = total_matrix[:3, 3] # print("translation_vector2",translation_vector) # print(1,translation) total_matrix = T_trans2 @ total_matrix translation = total_matrix[:3, 3] # print(2,translation) # 计算 z 坐标均值 vertices = np.asarray(pcd_transformed.vertices) z_mean2 = np.mean(vertices[:, 2]) z_max2 = np.max(vertices[:, 2]) volume_centroid = get_volume_centroid(pcd_transformed) z_volume_center2 = volume_centroid[2] # print(f"get_lowest_position_of_center z_max1={z_max1}, z_max2={z_max2}, len={len(pcd_transformed.vertices)}, obj_path={obj_path}") # print(f"z_mean1={z_mean1}, z_mean2={z_mean2}") # if (z_mean2 > z_mean1): # print(f"z_volume_center1={z_volume_center1}, z_volume_center2={z_volume_center2}") if (z_volume_center2 > z_volume_center1): R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * -angle_rad) centroid = pcd_transformed.get_center() aabb = pcd_transformed.get_axis_aligned_bounding_box() # center_point = aabb.get_center() center_point = compute_mesh_center(mesh_obj.vertices) pcd_transformed.translate(-center_point) pcd_transformed.rotate(R_y, center=(0, 0, 0)) pcd_transformed.translate(center_point) T_center_to_origin = np.eye(4) T_center_to_origin[:3, 3] = -center_point T_origin_to_center = np.eye(4) T_origin_to_center[:3, 3] = center_point # 构建反向旋转矩阵 R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * -angle_rad) T_rotate_inv = np.eye(4) T_rotate_inv[:3, :3] = R_y # 完整的反向绕中心旋转矩阵 T_rot_center_inv = T_origin_to_center @ T_rotate_inv @ T_center_to_origin total_matrix = T_rot_center_inv @ total_matrix vertices = np.asarray(pcd_transformed.vertices) # 计算平移向量，将最小Y值平移到0 min_z = np.min(vertices[:, 2]) # print("min_z2", min_z, obj_path) translation_vector = np.array([0,0,-min_z,]) pcd_transformed.translate(translation_vector) T_trans3 = np.eye(4) T_trans3[:3, 3] = translation_vector total_matrix = T_trans3 @ total_matrix # z_mean_min = min(z_mean1, z_mean2) z_max = min(z_max1, z_max2) # print("get_lowest_position_of_center2 time", time.time()-start_time2) return total_matrix, z_max def get_lowest_position_of_center_ext(mesh_obj, total_matrix): # print(f"get_lowest_position_of_center_ext mesh_obj={mesh_obj}") temp_matrix, z_max = get_lowest_position_of_z_ext(mesh_obj) total_matrix = temp_matrix @ total_matrix return total_matrix, z_max def calculate_rotation_and_top_of_mass(angle_x, angle_y, angle_z, points): """计算某一组旋转角度后的重心""" # 计算绕X轴、Y轴和Z轴的旋转矩阵 R_x = np.array([ [1, 0, 0], [0, np.cos(np.radians(angle_x)), -np.sin(np.radians(angle_x))], [0, np.sin(np.radians(angle_x)), np.cos(np.radians(angle_x))] ]) R_y = np.array([ [np.cos(np.radians(angle_y)), 0, np.sin(np.radians(angle_y))], [0, 1, 0], [-np.sin(np.radians(angle_y)), 0, np.cos(np.radians(angle_y))] ]) R_z = np.array([ [np.cos(np.radians(angle_z)), -np.sin(np.radians(angle_z)), 0], [np.sin(np.radians(angle_z)), np.cos(np.radians(angle_z)), 0], [0, 0, 1] ]) # 综合旋转矩阵 R = R_z @ R_y @ R_x # 执行旋转 rotated_points = points @ R.T # 计算最小z值 min_z = np.min(rotated_points[:, 2]) # 计算平移向量，将最小Z值平移到0 translation_vector = np.array([0, 0, -min_z]) rotated_points += translation_vector top_of_mass = np.max(rotated_points, axis=0) return top_of_mass[2], angle_x, angle_y, angle_z def parallel_rotation(points, angle_step=4): """仅绕 Y 轴旋转（假设 X/Z 轴不影响目标函数）""" max_top = float('inf') for angle_x in range(-90, 90, angle_step): for angle_y in range(0, 360, angle_step): max_z, ax, ay, _ = calculate_rotation_and_top_of_mass(angle_x, angle_y, 0, points) if max_z < max_top: max_top = max_z best_angle_x = ax best_angle_y = ay return (best_angle_x, best_angle_y, 0, max_top) def compute_mesh_center(vertices): if len(vertices) == 0: raise ValueError("顶点数组不能为空") # 确保vertices是NumPy数组 vertices_np = np.asarray(vertices) # 使用NumPy的mean函数直接计算均值（向量化操作） centroid = np.mean(vertices_np, axis=0) return centroid # -------------------------- 结束：获取z值最低 -------------------------- # -------------------------- 开始：bbox -------------------------- def compute_bbox_ext(mesh_obj, obj_name="", is_downsample=True): total_matrix = np.eye(4) total_matrix, z_max= get_lowest_position_of_center_ext(mesh_obj, total_matrix) transformed_vertices = mesh_transform_by_matrix(np.asarray(mesh_obj.vertices), total_matrix) obj_transformed = copy.deepcopy(mesh_obj) obj_transformed.vertices = o3d.utility.Vector3dVector(transformed_vertices) voxel_size = 3 # 设置体素的大小，决定下采样的密度 # 将点云摆正和X轴平衡 obj_transformed_second,total_matrix = arrange_box_correctly(obj_transformed,voxel_size,total_matrix) total_matrix, min_bound, max_bound, ply_name, pcd_fix = get_new_bbox(obj_transformed_second,obj_name,voxel_size,is_downsample,total_matrix) del obj_transformed del obj_transformed_second # return total_matrix, z_max, min_bound, max_bound, ply_name, pcd_fix return total_matrix, z_max, min_bound, max_bound, ply_name def arrange_box_correctly(obj_transformed, voxel_size,total_matrix): vertices = np.asarray(obj_transformed.vertices) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(vertices) # 降采样与特征计算 # pcd_downsampled = down_sample(pcd, voxel_size) # points = np.asarray(pcd_downsampled.points) points = np.asarray(pcd.points) cov = np.cov(points.T) center = obj_transformed.get_center() eigen_vals, eigen_vecs = np.linalg.eigh(cov) max_axis = eigen_vecs[:, np.argmax(eigen_vals)] # print("max_axis", max_axis) # 强制主方向向量X分量为正（指向右侧） if max_axis[0] < 0 or (max_axis[0] == 0 and max_axis[1] < 0): max_axis = -max_axis target_dir = np.array([1, 0]) # 目标方向为X正轴 current_dir = max_axis[:2] / np.linalg.norm(max_axis[:2]) dot_product = np.dot(current_dir, target_dir) # print("dot_product", dot_product) if dot_product < 0.8: # 阈值控制方向敏感性（建议0.6~0.9） max_axis = -max_axis # 强制翻转方向 # 计算旋转角度 angle_z = np.arctan2(max_axis[1], max_axis[0]) % (2 * np.pi) if max_axis[0] <= 0 and max_axis[1] <= 0: angle_z += np.pi R = o3d.geometry.get_rotation_matrix_from_axis_angle([0, 0, -angle_z]) T = np.eye(4) T[:3, :3] = R T[:3, 3] = center - R.dot(center) # 保持中心不变 obj_transformed.transform(T) total_matrix = T @ total_matrix return obj_transformed, total_matrix def get_new_bbox(obj_transformed_second,obj_name,voxel_size,is_downsample,total_matrix): # 计算点云的边界 points = np.asarray(obj_transformed_second.vertices) min_bound = np.min(points, axis=0) # 获取点云的最小边界 max_bound = np.max(points, axis=0) # 获取点云的最大边界 # print(f"get_new_bbox1: obj_name={obj_name}, min_bound={min_bound}, max_bound={max_bound}") # 确保包围盒的Y坐标不低于0 min_bound[2] = max(min_bound[2], 0) # 确保Y坐标的最小值不低于0 # 重新计算包围盒的中心和半长轴 bbox_center = (min_bound + max_bound) / 2 # 计算包围盒的中心点 bbox_extent = (max_bound - min_bound) # 计算包围盒的半长轴（尺寸） # 创建包围盒，确保尺寸正确 new_bbox = o3d.geometry.OrientedBoundingBox(center=bbox_center, R=np.eye(3), # 旋转矩阵，默认没有旋转 extent=bbox_extent) # 获取包围盒的长、宽和高 x_length = round(bbox_extent[0],3) # X 方向的长 y_length = round(bbox_extent[1],3) # Y 方向的宽 z_length = round(bbox_extent[2],3) # Z 方向的高 bbox_points = np.array([ [min_bound[0], min_bound[1], min_bound[2]], [max_bound[0], min_bound[1], min_bound[2]], [max_bound[0], max_bound[1], min_bound[2]], [min_bound[0], max_bound[1], min_bound[2]], [min_bound[0], min_bound[1], max_bound[2]], [max_bound[0], min_bound[1], max_bound[2]], [max_bound[0], max_bound[1], max_bound[2]], [min_bound[0], max_bound[1], max_bound[2]] ]) first_corner = bbox_points[2] translation_vector = -first_corner obj_transformed_second.translate(translation_vector) T_trans = np.eye(4) T_trans[:3, 3] = translation_vector # 设置平移分量 [2,3](@ref) total_matrix = T_trans @ total_matrix # 矩阵乘法顺序：最新变换左乘[4,5](@ref) new_bbox.translate(translation_vector) vertices = np.asarray(obj_transformed_second.vertices) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(vertices) if is_downsample: pcd_downsampled = down_sample(pcd, voxel_size, False) pcd_fix = pcd_downsampled else: pcd_fix = pcd min_bound = np.min(pcd.points, axis=0) # 获取点云的最小边界 max_bound = np.max(pcd.points, axis=0) # 获取点云的最大边界 # print(f"get_new_bbox2: min_bound={min_bound}, max_bound={max_bound}") if (not obj_name == ""): ply_print_pid = obj_name.replace(".obj","") ply_name = f"{ply_print_pid}={z_length}+{y_length}+{x_length}.ply" else: ply_name = "" return total_matrix, min_bound, max_bound, ply_name, pcd_fix # -------------------------- 结束：bbox -------------------------- # -------------------------- 开始：general -------------------------- def down_sample(pcd, voxel_size, farthest_sample = False): original_num = len(pcd.points) target_samples = 1500 # 1000 num_samples = min(target_samples, original_num) # 第一步：使用体素下采样快速减少点数量 # voxel_size = 3 if farthest_sample: pcd_voxel = pcd.farthest_point_down_sample(num_samples=num_samples) else: pcd_voxel = pcd.voxel_down_sample(voxel_size) down_num = len(pcd_voxel.points) # print(f"original_num={original_num}, down_num={down_num}") # 第二步：仅在必要时进行最远点下采样 if len(pcd_voxel.points) > target_samples and False: pcd_downsampled = pcd_voxel.farthest_point_down_sample(num_samples=num_samples) else: pcd_downsampled = pcd_voxel return pcd_downsampled def read_mesh(obj_path, enable_post_processing=False): mesh_obj = o3d.io.read_triangle_mesh(obj_path, enable_post_processing) return mesh_obj def mesh_transform_by_matrix(vertices, transform_matrix): """ 手动实现网格变换：对每个顶点应用齐次变换矩阵 参数: vertices: 网格顶点数组 (N, 3) transform_matrix: 4x4 齐次变换矩阵 返回: 变换后的顶点数组 (N, 3) """ # 1. 顶点转齐次坐标 (N, 3) → (N, 4) homogeneous_vertices = np.hstack((vertices, np.ones((vertices.shape[0], 1)))) # 2. 应用变换矩阵：矩阵乘法 (4x4) * (4xN) → (4xN) transformed_homogeneous = transform_matrix @ homogeneous_vertices.T # 3. 转回非齐次坐标 (3xN) → (N, 3) transformed_vertices = transformed_homogeneous[:3, :].T del homogeneous_vertices del transformed_homogeneous return transformed_vertices def get_volume_centroid(mesh): """向量化版本的体积重心计算""" vertices = np.asarray(mesh.vertices) triangles = np.asarray(mesh.triangles) # 一次性获取所有三角形的顶点 [n_triangles, 3, 3] tri_vertices = vertices[triangles] # 分别提取三个顶点 v0 = tri_vertices[:, 0, :] # [n, 3] v1 = tri_vertices[:, 1, :] # [n, 3] v2 = tri_vertices[:, 2, :] # [n, 3] # 向量化计算叉积和点积 cross_vec = np.cross(v1, v2) # [n, 3] dot_results = np.einsum('ij,ij->i', v0, cross_vec) # 批量点积 [n] # 计算每个四面体体积 [n] tetra_volumes = np.abs(dot_results) / 6.0 # 计算每个四面体重心 [n, 3] tetra_centers = (v0 + v1 + v2) / 4.0 # 总体积和加权重心 total_volume = np.sum(tetra_volumes) if total_volume > 0: weighted_center = np.sum(tetra_volumes[:, np.newaxis] * tetra_centers, axis=0) / total_volume return weighted_center else: return np.mean(vertices, axis=0) voxel_size = 3 # -------------------------- 结束：general -------------------------- if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument("--obj_path", type=str, required=True, help="obj 路径") args = parser.parse_args() obj_path = args.obj_path total_matrix, z_max, min_bound, max_bound, ply_name = compute_bbox_out(obj_path) print(f"total_matrix={total_matrix}, z_max={z_max}, min_bound={min_bound}, max_bound={max_bound}, ply_name={ply_name}")