import open3d as o3d import numpy as np import copy import time import argparse """ 对外部提供的获取最低z的接口 get_lowest_position_of_z_out 参数: obj_path, 模型数据路径 返回: total_matrix: 旋转矩阵 z_max: Z最高点 """ def get_lowest_position_of_z_out(obj_path): mesh_obj = o3d.io.read_triangle_mesh(obj_path) 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]) 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) centroid = pcd_transformed.get_center() 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]) # print(f"get_lowest_position_of_center z_max1={z_max1}, z_max2={z_max2}, len={len(pcd_transformed.vertices)}, obj_path={obj_path}") if (z_mean2 > z_mean1): # if (z_max2 > z_max1): 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 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 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 if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument("--obj_path", type=str, required=True, help="batchobj_path_id") args = parser.parse_args() obj_path = args.obj_path max, z = get_lowest_position_of_z_out(obj_path)