|
|
import open3d as o3d |
|
|
import numpy as np |
|
|
import copy |
|
|
import time |
|
|
import argparse |
|
|
|
|
|
from general import * |
|
|
|
|
|
# ------------------------ 开始：对外接口，获取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(mesh_obj): |
|
|
return compute_bbox_ext(mesh_obj) |
|
|
|
|
|
# -------------------------- 结束：对外接口，获取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]) |
|
|
start_time_v1 = time.time() |
|
|
volume_centroid = get_volume_centroid(pcd_transformed) |
|
|
z_volume_center1 = volume_centroid[2] |
|
|
delta = time.time() - start_time_v1 |
|
|
# print(f"get_volume_centroid time={delta}") |
|
|
|
|
|
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 get_models_bbox(dict_pcd_fix): |
|
|
""" |
|
|
单独提取：从dict_fix中解析所有模型的包围盒（bbox）尺寸信息 |
|
|
:param dict_fix: 包含PLY文件名和对应点云的字典 |
|
|
:return: 模型列表（包含name和dimensions） |
|
|
""" |
|
|
all_models = [] |
|
|
extend_dist = 2 # 尺寸扩展量（单位：厘米） |
|
|
for ply_file in dict_pcd_fix: |
|
|
# 解析PLY文件名中的尺寸信息（格式："模型ID=维度1+维度2+维度3.ply"） |
|
|
bbox_with_text = ply_file.split("=") |
|
|
bbox_with = bbox_with_text[-1] |
|
|
split_text = bbox_with.replace(".ply", "").split("+") |
|
|
|
|
|
# 转换单位：米 → 厘米 → 加扩展量 → 转回米（int取整避免浮点数精度问题） |
|
|
x_length = int(float(split_text[2]) * 100) + extend_dist # 第三个维度→x方向 |
|
|
y_length = int(float(split_text[0]) * 100) + extend_dist # 第一个维度→y方向 |
|
|
z_length = int(float(split_text[1]) * 100) + extend_dist # 第二个维度→z方向 |
|
|
|
|
|
all_models.append({ |
|
|
'name': ply_file, |
|
|
'dimensions': (int(x_length / 100), int(z_length / 100), int(y_length / 100)) # 单位：米 |
|
|
}) |
|
|
return all_models |
|
|
|
|
|
def arrange_models_on_platform(models, machine_size): |
|
|
""" |
|
|
单独提取：将模型在打印平台上进行排版布局 |
|
|
:param models: 由get_models_bbox返回的模型列表（包含name和dimensions） |
|
|
:param machine_size: 打印机尺寸 (width, depth, height) |
|
|
:return: (placed_models, unplaced_models) - 已放置和未放置的模型列表 |
|
|
""" |
|
|
# 初始化打印平台 |
|
|
platform = Platform( |
|
|
int(machine_size[0]), |
|
|
int(machine_size[1]), |
|
|
int(machine_size[2]) |
|
|
) |
|
|
print("开始计算排序...") |
|
|
platform.arrange_models(models) |
|
|
platform.print_results() |
|
|
return platform.get_result() |
|
|
|
|
|
import os |
|
|
|
|
|
def compute_bbox_all_ext(base_original_obj_dir,compact_min_dis=True): |
|
|
|
|
|
obj_id_list = [aa.split(".o")[0] for aa in os.listdir(base_original_obj_dir) if aa.endswith(".obj")] |
|
|
obj_id_list = obj_id_list |
|
|
|
|
|
dict_mesh_obj = {} |
|
|
for pid_t_y in obj_id_list: |
|
|
|
|
|
obj_name = pid_t_y+".obj" |
|
|
obj_path = os.path.join(base_original_obj_dir,obj_name) |
|
|
mesh_obj = read_mesh(obj_path) |
|
|
|
|
|
if mesh_obj is None: |
|
|
continue |
|
|
|
|
|
dict_mesh_obj[obj_name] = mesh_obj |
|
|
|
|
|
return compute_bbox_all(dict_mesh_obj,compact_min_dis) |
|
|
|
|
|
def compute_bbox_all(dict_mesh_obj,is_downsample): |
|
|
dict_total_matrix= {} |
|
|
dict_pcd_fix= {} |
|
|
for key, value in dict_mesh_obj.items(): |
|
|
start_time = time.time() |
|
|
|
|
|
obj_name = key |
|
|
mesh_obj = value |
|
|
|
|
|
total_matrix, pcd_fix, ply_name = compute_bbox(mesh_obj,obj_name,is_downsample) |
|
|
|
|
|
dict_total_matrix[obj_name] = total_matrix |
|
|
dict_pcd_fix[ply_name] = pcd_fix |
|
|
|
|
|
print(f"compute_bbox obj_name={obj_name} ply_name={ply_name} time={time.time()-start_time}") |
|
|
|
|
|
# dict_mesh_obj.clear() |
|
|
# del dict_mesh_obj |
|
|
|
|
|
all_models = get_models_bbox(dict_pcd_fix) |
|
|
|
|
|
return dict_total_matrix,all_models |
|
|
|
|
|
|
|
|
def compute_bbox(mesh_obj, obj_name="", is_downsample=True): |
|
|
# return compute_bbox_ext(mesh_obj, obj_name, is_downsample) |
|
|
|
|
|
mesh_obj_origin = copy.deepcopy(mesh_obj) |
|
|
total_matrix, z_max, min_bound, max_bound, ply_name = compute_bbox_ext(mesh_obj, obj_name, is_downsample) |
|
|
|
|
|
transformed_vertices = mesh_transform_by_matrix(np.asarray(mesh_obj_origin.vertices), total_matrix) |
|
|
|
|
|
pcd = o3d.geometry.PointCloud() |
|
|
pcd.points = o3d.utility.Vector3dVector(transformed_vertices) |
|
|
if is_downsample: |
|
|
pcd_downsampled = down_sample(pcd, voxel_size, False) |
|
|
pcd_fix = pcd_downsampled |
|
|
else: |
|
|
pcd_fix = pcd |
|
|
|
|
|
return total_matrix, pcd_fix, ply_name |
|
|
|
|
|
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) |
|
|
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: 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 |
|
|
|
|
|
class Platform: |
|
|
def __init__(self, width, depth, height): |
|
|
self.width = width |
|
|
self.depth = depth |
|
|
self.height = height |
|
|
self.placed_models = [] # 已放置的模型 |
|
|
self.unplaced_models = [] # 未能放置的模型 |
|
|
self.first_line = True |
|
|
self.remove_multiobj_name = "" |
|
|
|
|
|
def is_cross_border(self, x, y, z, model): |
|
|
mx, my, mz = model['dimensions'] |
|
|
|
|
|
return is_cross_border_c(x, y, z, mx, my, mz, self.width, self.depth, self.height) |
|
|
|
|
|
def check_multiobj_cross_pre(self, name, pre_model): |
|
|
if (pre_model==None): |
|
|
return |
|
|
|
|
|
if not is_same_obj(name, pre_model['name']): |
|
|
return |
|
|
|
|
|
self.unplaced_models.append(pre_model) |
|
|
if pre_model in self.placed_models: |
|
|
self.placed_models.remove(pre_model) |
|
|
|
|
|
if "pre_model" in pre_model: |
|
|
self.pre_model = pre_model["pre_model"] |
|
|
|
|
|
self.check_multiobj_cross_pre(name, self.pre_model) |
|
|
|
|
|
def check_multiobj_cross(self, model): |
|
|
if not is_multi_obj(model['name']): |
|
|
return False |
|
|
|
|
|
self.unplaced_models.append(model) |
|
|
|
|
|
if "pre_model" in model: |
|
|
self.pre_model = model["pre_model"] |
|
|
|
|
|
self.check_multiobj_cross_pre(model['name'], self.pre_model) |
|
|
|
|
|
return True |
|
|
|
|
|
def can_place(self, x, y, z, model, is_print=False): |
|
|
mx, my, mz = model['dimensions'] |
|
|
|
|
|
if self.is_cross_border(x, y, z, model): |
|
|
|
|
|
print(f"can_place False 1 cross_border {x}, {y}, {z}, {model}, {self.width}, {self.depth}, {self.height}") |
|
|
return False |
|
|
|
|
|
# 碰撞检测（正确逻辑与间距处理） |
|
|
for placed in self.placed_models: |
|
|
px, py, pz = placed['position'] |
|
|
pdx, pdy, pdz = placed['dimensions'] |
|
|
|
|
|
# 使用AABB碰撞检测算法[4](@ref) |
|
|
if (x > px - pdx - extend_dist_model_x and |
|
|
x - mx - extend_dist_model_x < px and |
|
|
y > py - pdy - extend_dist_model_y and |
|
|
y - my - extend_dist_model_y < py and |
|
|
z < pz + pdz and |
|
|
z + mz > pz): |
|
|
print("can_place False 2",False,model,x,y,z,px,pdx,extend_dist_model_x,py,pdy,extend_dist_model_y,my,pz,pdz,pz) |
|
|
return False |
|
|
|
|
|
return True |
|
|
|
|
|
def place_model(self, model, pre_model): |
|
|
mx, my, mz = model['dimensions'] |
|
|
if mz > self.height: |
|
|
self.unplaced_models.append(model) |
|
|
return False |
|
|
|
|
|
if is_same_obj(model['name'], self.remove_multiobj_name): |
|
|
self.unplaced_models.append(model) |
|
|
return False |
|
|
|
|
|
z = 0 |
|
|
|
|
|
if pre_model is None: |
|
|
if self.first_line: |
|
|
model['position'] = (mx + extend_dist_border_x_min, self.depth - extend_dist_border_y_max, 0) |
|
|
print(f"First Model {model['name']}") |
|
|
model['first_line'] = True |
|
|
else: |
|
|
model['position'] = (self.width - extend_dist_border_x_max, self.depth - extend_dist_border_y_max, 0) |
|
|
model['first_line'] = False |
|
|
|
|
|
print("model position1", model['name'], model['position']) |
|
|
self.placed_models.append(model) |
|
|
return True |
|
|
|
|
|
pre_px, pre_py, pre_pz = pre_model['position'] |
|
|
pre_mx, pre_my, pre_mz = pre_model['dimensions'] |
|
|
if self.first_line: |
|
|
px = pre_px + mx + extend_dist_model_x |
|
|
model['first_line'] = True |
|
|
else: |
|
|
px = pre_px - pre_mx - extend_dist_model_x |
|
|
model['first_line'] = False |
|
|
print(model['name'], "px", px, pre_px, pre_mx) |
|
|
|
|
|
reach_limit_x = False |
|
|
if self.first_line: |
|
|
if px > self.width: |
|
|
reach_limit_x = True |
|
|
else: |
|
|
if px - mx < 0: |
|
|
reach_limit_x = True |
|
|
|
|
|
if reach_limit_x: |
|
|
self.first_line = False |
|
|
px = self.width - extend_dist_border_x_max |
|
|
start_y = my + extend_dist_border_y_min |
|
|
final_y = self.depth |
|
|
print("reach_limit_x final_y1", model['name'], my, final_y, my, extend_dist_border_x_max, px) |
|
|
for y in range(start_y, final_y, +1): |
|
|
# print("y",y) |
|
|
if self.can_place(px, y, z, model, True)==False: |
|
|
y -= 1 |
|
|
|
|
|
if self.is_cross_border(px, y, z, model): |
|
|
print(f"cross border : {model['name']}") |
|
|
if self.check_multiobj_cross(model): |
|
|
self.remove_multiobj_name = model['name'] |
|
|
return False |
|
|
|
|
|
model['position'] = (px, y, z) |
|
|
print("model position2", model['name'], model['position']) |
|
|
self.placed_models.append(model) |
|
|
return True |
|
|
else: |
|
|
start_y = my + extend_dist_border_y_min |
|
|
final_y = self.depth |
|
|
print("final_y2", model['name'], start_y, final_y, my, extend_dist_border_y_max, px) |
|
|
for y in range(start_y, final_y, +1): |
|
|
if self.can_place(px, y, z, model)==False: |
|
|
y -= 1 |
|
|
|
|
|
if self.is_cross_border(px, y, z, model): |
|
|
print(f"cross border : {model['name']}") |
|
|
if self.check_multiobj_cross(model): |
|
|
self.remove_multiobj_name = model['name'] |
|
|
return False |
|
|
|
|
|
model['position'] = (px, y, z) |
|
|
print("model position2", model['name'], model['position']) |
|
|
self.placed_models.append(model) |
|
|
return True |
|
|
|
|
|
if 'position' in model: |
|
|
print("model position3", model['name'], model['position']) |
|
|
else: |
|
|
print("model position3 no exist position", model['name']) |
|
|
|
|
|
self.unplaced_models.append(model) |
|
|
return False |
|
|
|
|
|
def arrange_models(self, models): |
|
|
|
|
|
"""对所有模型进行排布（单层）""" |
|
|
print("⚠️ 单层放置模式：所有模型只能放在平台底面（Z=0）") |
|
|
# 按高度和面积排序，优先放大模型 |
|
|
models = sorted(models, key=lambda m: (-m['dimensions'][2], -m['dimensions'][0] * m['dimensions'][1])) |
|
|
self.pre_model = None |
|
|
for model in models: |
|
|
print(f"arrange_models {model['name']}") |
|
|
pre_model_temp = self.pre_model |
|
|
if self.place_model(model, self.pre_model): |
|
|
self.pre_model = model |
|
|
|
|
|
model["pre_model"] = pre_model_temp |
|
|
|
|
|
def print_results(self): |
|
|
"""打印排布结果""" |
|
|
print("Placed Models:") |
|
|
for model in self.placed_models: |
|
|
print(f" - {model['name']} at {model['position']} with dimensions {model['dimensions']}") |
|
|
print("Unplaced Models:") |
|
|
for model in self.unplaced_models: |
|
|
print(f" - {model['name']} with dimensions {model['dimensions']}") |
|
|
|
|
|
def get_result(self): |
|
|
return self.placed_models, self.unplaced_models |
|
|
|
|
|
# -------------------------- 结束：bbox -------------------------- |
|
|
|
|
|
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_ext(obj_path) |
|
|
|
|
|
|
