import open3d as o3d import numpy as np import copy import time from get_lowest_position_of_z_out import get_lowest_position_of_z_out # 对外部提供的获取最低z的接口 def get_lowest_position_of_center_out2(obj_path): total_matrix = np.eye(4) mesh_obj = o3d.io.read_triangle_mesh(obj_path) voxel_size = 3 return get_lowest_position_of_center(mesh_obj, obj_path, total_matrix, voxel_size) def calculate_rotation_and_center_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 # 计算重心 center_of_mass = np.mean(rotated_points, axis=0) return center_of_mass[2], angle_x, angle_y, angle_z def parallel_rotation4(points, angle_step=4): """仅绕 Y 轴旋转（假设 X/Z 轴不影响目标函数）""" min_center = float('inf') for angle_x in range(-45, 45, angle_step): for angle_y in range(0, 360, angle_step): center_z, ax, ay, _ = calculate_rotation_and_center_of_mass(angle_x, angle_y, 0, points) if center_z < min_center: min_center = center_z best_angle_x = ax best_angle_y = ay return (best_angle_x, best_angle_y, 0, min_center) import numpy as np from numba import cuda import math # CUDA核函数：计算所有旋转角度下的重心高度 @cuda.jit def compute_centers_kernel(points, centers, angle_x_start, angle_x_step, angle_y_start, angle_y_step, num_x, num_y): i = cuda.blockIdx.x * cuda.blockDim.x + cuda.threadIdx.x j = cuda.blockIdx.y * cuda.blockDim.y + cuda.threadIdx.y if i >= num_x or j >= num_y: return # 获取整数角度值 angle_x = angle_x_start + i * angle_x_step angle_y = angle_y_start + j * angle_y_step rx = math.radians(float(angle_x)) # 使用 float() 进行转换 ry = math.radians(float(angle_y)) rz = 0.0 # 计算旋转矩阵 cos_x = math.cos(rx) sin_x = math.sin(rx) cos_y = math.cos(ry) sin_y = math.sin(ry) cos_z = math.cos(rz) sin_z = math.sin(rz) # 旋转矩阵: R = R_z * R_y * R_x R00 = cos_z * cos_y R01 = cos_z * sin_y * sin_x - sin_z * cos_x R02 = cos_z * sin_y * cos_x + sin_z * sin_x R10 = sin_z * cos_y R11 = sin_z * sin_y * sin_x + cos_z * cos_x R12 = sin_z * sin_y * cos_x - cos_z * sin_x R20 = -sin_y R21 = cos_y * sin_x R22 = cos_y * cos_x n = points.shape[0] min_z = 1e10 # 第一遍：计算最小Z值 for k in range(n): x = points[k, 0] y = points[k, 1] z = points[k, 2] x_rot = R00 * x + R01 * y + R02 * z y_rot = R10 * x + R11 * y + R12 * z z_rot = R20 * x + R21 * y + R22 * z if z_rot < min_z: min_z = z_rot total_z = 0.0 # 第二遍：平移并计算Z坐标和 for k in range(n): x = points[k, 0] y = points[k, 1] z = points[k, 2] x_rot = R00 * x + R01 * y + R02 * z y_rot = R10 * x + R11 * y + R12 * z z_rot = R20 * x + R21 * y + R22 * z z_trans = z_rot - min_z total_z += z_trans center_z = total_z / n centers[i, j] = center_z # CUDA版本的并行旋转计算 def parallel_rotation4_cuda(points, angle_step=4): angle_x_start = -45 angle_x_end = 45 angle_y_start = 0 angle_y_end = 360 num_x = int((angle_x_end - angle_x_start) / angle_step) num_y = int((angle_y_end - angle_y_start) / angle_step) # 将点云数据复制到GPU d_points = cuda.to_device(points.astype(np.float32)) d_centers = cuda.device_array((num_x, num_y), dtype=np.float32) # 配置线程块和网格 threadsperblock = (16, 16) blockspergrid_x = (num_x + threadsperblock[0] - 1) // threadsperblock[0] blockspergrid_y = (num_y + threadsperblock[1] - 1) // threadsperblock[1] blockspergrid = (blockspergrid_x, blockspergrid_y) # 启动核函数 compute_centers_kernel[blockspergrid, threadsperblock]( d_points, d_centers, angle_x_start, angle_step, angle_y_start, angle_step, num_x, num_y ) # 将结果复制回主机 centers = d_centers.copy_to_host() # 找到最小重心值的索引 min_index = np.argmin(centers) i = min_index // num_y j = min_index % num_y best_angle_x = angle_x_start + i * angle_step best_angle_y = angle_y_start + j * angle_step min_center = centers[i, j] return best_angle_x, best_angle_y, 0, min_center def read_mesh(obj_path, simple=True): mesh_obj = o3d.io.read_triangle_mesh(obj_path) return mesh_obj def compute_mesh_center2(vertices): """ 计算网格质心 参数: vertices: 顶点坐标数组，形状为(N, 3)的NumPy数组或列表 返回: centroid: 质心坐标的NumPy数组 [x, y, z] """ if len(vertices) == 0: raise ValueError("顶点数组不能为空") n = len(vertices) # 顶点数量 # 初始化坐标累加器 sum_x, sum_y, sum_z = 0.0, 0.0, 0.0 start_time1 = time.time() # 遍历所有顶点累加坐标值 for vertex in vertices: sum_x += vertex[0] sum_y += vertex[1] sum_z += vertex[2] print("compute_mesh_center1 time", time.time()-start_time1) # 计算各坐标轴的平均值 centroid = np.array([sum_x / n, sum_y / n, sum_z / n]) return centroid def compute_mesh_center(vertices): """ 计算网格质心（优化版） 参数: vertices: 顶点坐标数组，形状为(N, 3)的NumPy数组或列表 返回: centroid: 质心坐标的NumPy数组 [x, y, z] """ 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 get_lowest_position_of_center_ext3(mesh_obj, obj_path, voxel_size = 3): best_angle_x, best_angle_y, best_angle_z, z_mean_min, pcd_transformed= get_lowest_position_of_center2(mesh_obj, obj_path, voxel_size) return best_angle_x, best_angle_y, best_angle_z, z_mean_min def get_lowest_position_of_center_ext2(mesh_obj, obj_path, total_matrix, voxel_size): # total_matrix, z_mean_min = get_lowest_position_of_center(obj_path, total_matrix, voxel_size) temp_matrix, z_mean_min = get_lowest_position_of_center(mesh_obj, obj_path, np.eye(4), voxel_size) # print("temp_matrix=",temp_matrix,voxel_size,mesh_obj) total_matrix = temp_matrix @ total_matrix return total_matrix, z_mean_min def get_lowest_position_of_center_ext(obj_path, total_matrix): temp_matrix, z_max = get_lowest_position_of_z_out(obj_path) total_matrix = temp_matrix @ total_matrix return total_matrix, z_max 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 get_lowest_position_of_center(mesh_obj, obj_path, total_matrix, voxel_size): # 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的值 min_center_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, min_center_of_mass_y = parallel_rotation4(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 = min(z_max1, z_max2) # print("get_lowest_position_of_center2 time", time.time()-start_time2) return total_matrix, z_max_min import requests import json import re def is_valid_float_string(s): # 匹配科学计数法或普通小数，允许开头和末尾的空格 pattern = r'^[-+]?[0-9]*\.?[0-9]+([eE][-+]?[0-9]+)?$' return bool(re.match(pattern, s.strip())) def safe_convert_to_float(s): """ 尝试将字符串安全转换为float，处理一些不完整情况。 """ s = s.strip().lower() # 去除空格，统一为小写 # 处理空字符串或完全非数字的情况 if not s or s in ['na', 'nan', 'inf', 'null', 'none']: return None # 或者可以根据需要返回 float('nan') # 检查是否为有效的浮点数字符串 if is_valid_float_string(s): return float(s) # 处理类似 '0.00000000e' 的情况（缺少指数） if s.endswith('e'): # 尝试添加指数 '0' -> 'e0' try: return float(s + '0') except ValueError: pass # 如果添加'e0'后仍然失败，则继续下面的异常处理 # 更激进的清理：移除非数字、小数点、负号和指数e以外的所有字符 # 注意：这可能破坏有特定格式的字符串，慎用 cleaned_s = re.sub(r'[^\d\.eE-]', '', s) try: return float(cleaned_s) except ValueError: pass # 如果所有尝试都失败，返回None或抛出异常 return None def string_to_matrix(data_string): """ 将字符串转换为NumPy浮点数矩阵，并处理可能的转换错误。 """ # 分割字符串 lines = data_string.strip().split(';') matrix = [] for line in lines: num_list = line.split() float_row = [] for num in num_list: # 使用安全转换函数 value = safe_convert_to_float(num) if value is None: # 处理转换失败，例如记录日志、使用NaN代替 print(f"警告: 无法转换字符串 '{num}'，将其替换为NaN。") value = np.nan # 用NaN标记缺失或无效值 float_row.append(value) matrix.append(float_row) return np.array(matrix) import ast def get_lowest_position_of_center_net(printId, total_matrix): print("get_lowest_position_of_center_net", printId) url = f"https://mp.api.suwa3d.com/api/printOrder/infoByPrintId?printId={printId}" res = requests.get(url) datas = res.json()["data"]["layout"] print("datas=", datas) homo_matrix_str = datas.get("homo_matrix") print("homo_matrix_str=", homo_matrix_str) # 1. 去除字符串首尾的方括号 str_cleaned = homo_matrix_str.strip('[]') # 2. 按行分割字符串 rows = str_cleaned.split('\n') # 3. 修复：处理每行中的逗号问题 matrix_list = [] for row in rows: if row.strip() == '': continue # 去除行首尾的方括号和空格 row_cleaned = row.strip(' []') # 按逗号分割，但过滤掉空字符串 elements = [elem.strip() for elem in row_cleaned.split(',') if elem.strip() != ''] # 进一步清理每个元素：去除可能残留的逗号和方括号 cleaned_elements = [] for elem in elements: # 去除元素中可能存在的逗号、方括号和空格 elem_cleaned = elem.strip(' ,[]') if elem_cleaned != '': cleaned_elements.append(elem_cleaned) if cleaned_elements: # 只添加非空行 matrix_list.append(cleaned_elements) print("matrix_list=", matrix_list) # 4. 安全地转换为浮点数数组（带错误处理） try: reconstructed_matrix = np.array(matrix_list, dtype=float) except ValueError as e: print(f"转换矩阵时出错: {e}") print("尝试逐个元素转换...") # 逐个元素转换，便于定位问题元素 float_matrix = [] for i, row in enumerate(matrix_list): float_row = [] for j, elem in enumerate(row): try: # 再次清理元素并转换 cleaned_elem = elem.strip(' ,') float_val = float(cleaned_elem) float_row.append(float_val) except ValueError as ve: print(f"无法转换的元素: 行{i}, 列{j}, 值'{elem}', 错误: {ve}") # 可以选择设置为0或NaN，或者抛出异常 float_row.append(0.0) # 或者 np.nan float_matrix.append(float_row) reconstructed_matrix = np.array(float_matrix, dtype=float) layout_z = datas.get("layout_z", 0) print("layout_z", layout_z) reconstructed_matrix = reconstructed_matrix @ total_matrix print("reconstructed_matrix=", reconstructed_matrix) return reconstructed_matrix, layout_z def get_lowest_position_of_center2(mesh_obj, obj_path, voxel_size = 3): # mesh_obj = read_mesh(obj_path) vertices = np.asarray(mesh_obj.vertices) # 确保网格有顶点 if len(vertices) == 0: print(f"Warning: Mesh has no vertices: {obj_path}") return None pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(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]) # 确保下采样后有点云 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的值 best_angle_x, best_angle_y, best_angle_z = 0, 0, 0 # 旋转并计算最优角度：绕X、Y、Z轴进行每度的旋转 best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation4(points, angle_step=3) # print("best_angle1", best_angle_x, best_angle_y, best_angle_z) # 使用最佳角度进行旋转并平移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) #""" aabb = pcd_transformed.get_axis_aligned_bounding_box() center_point = aabb.get_center() 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) # 计算 z 坐标均值 vertices = np.asarray(pcd_transformed.vertices) z_mean1 = np.mean(vertices[:, 2]) angle_rad = np.pi 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) best_angle_y += angle_rad / np.pi * 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) # 计算 z 坐标均值 vertices = np.asarray(pcd_transformed.vertices) z_mean2 = np.mean(vertices[:, 2]) # print("z_mean",z_mean1,z_mean2,len(pcd_transformed.vertices),obj_path) if z_mean2 > z_mean1: R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * -angle_rad) aabb = pcd_transformed.get_axis_aligned_bounding_box() center_point = aabb.get_center() pcd_transformed.translate(-center_point) pcd_transformed.rotate(R_y, center=(0, 0, 0)) pcd_transformed.translate(center_point) best_angle_y += -angle_rad / np.pi * 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) z_mean_min = min(z_mean1, z_mean2) angle_z_delta = arrange_box_correctly(pcd_transformed, voxel_size) best_angle_z += angle_z_delta # print("angle_z_delta", angle_z_delta, best_angle_z) return best_angle_x, best_angle_y, best_angle_z, z_mean_min, pcd_transformed def arrange_box_correctly(obj_transformed, voxel_size): vertices = np.asarray(obj_transformed.vertices) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(vertices) # 降采样与特征计算 pcd_downsampled = down_sample(pcd, voxel_size) original_num = len(pcd.points) target_samples = 1000 num_samples = min(target_samples, original_num) points = np.asarray(pcd_downsampled.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)] # 强制主方向向量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 # print("max_axis2", max_axis, -angle_z, np.rad2deg(-angle_z)) 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) return np.rad2deg(-angle_z)