print_setting/get_lowest_position_of_z_out.py

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)