7 changed files with 646 additions and 627 deletions
@ -0,0 +1,594 @@ |
|||||||
|
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}") |
||||||
|
|
||||||
|
|
||||||
File diff suppressed because one or more lines are too long
Loading…
Reference in new issue