import open3d as o3d
import numpy as np


def compute_curvature_open3d(pcd, radius=0.1, max_nn=30):
    """
    使用 Open3D 计算曲率

    Args:
        pcd: Open3D 点云对象
        radius: 搜索半径
        max_nn: 最大邻居数

    Returns:
        curvature: 曲率数组
    """
    # 估计法线
    pcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)
    )

    # 计算曲率（Open3D 在估计法线时已计算）
    # 曲率值存储在 pcd.point.normals 的第三个分量？
    # 实际上 Open3D 需要单独计算

    # 方法：使用 PCA 重新计算
    points = np.asarray(pcd.points)
    n_points = len(points)
    curvature = np.zeros(n_points)

    tree = o3d.geometry.KDTreeFlann(pcd)

    for i in range(n_points):
        [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius)

        if k < 3:
            curvature[i] = 1.0
            continue

        neighbors = points[idx]
        centroid = np.mean(neighbors, axis=0)
        centered = neighbors - centroid
        cov = np.cov(centered.T)
        eigenvalues = np.linalg.eigvalsh(cov)
        eigenvalues = np.sort(eigenvalues)

        curvature[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8)

    return curvature


# 使用示例
pcd = o3d.io.read_point_cloud("pointcloud.pcd")
curvature = compute_curvature_open3d(pcd, radius=0.05)