import open3d as o3d
import numpy as np
from scipy.spatial import KDTree
def chamfer_distance(pcd1, pcd2):
    """
    计算两个点云之间的 CD 距离（Chamfer Distance）

    Args:
        pcd1: Open3D 点云对象1
        pcd2: Open3D 点云对象2

    Returns:
        cd: Chamfer Distance 值
        cd1: 点云1 到 点云2 的平均距离
        cd2: 点云2 到 点云1 的平均距离
    """
    # 获取点坐标
    points1 = np.asarray(pcd1.points)
    points2 = np.asarray(pcd2.points)

    # 构建 KD-Tree 加速最近邻搜索
    tree1 = KDTree(points1)
    tree2 = KDTree(points2)

    # 计算 points1 中每个点到 points2 的最近距离
    dist1, _ = tree2.query(points1, k=1)
    cd1 = np.mean(dist1 ** 2)  # 平方距离的平均值

    # 计算 points2 中每个点到 points1 的最近距离
    dist2, _ = tree1.query(points2, k=1)
    cd2 = np.mean(dist2 ** 2)  # 平方距离的平均值

    # 总 CD 距离（双向平均）
    cd = cd1 + cd2

    return np.sqrt(cd), cd1, cd2
pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_left_side.ply")
points = np.asarray(pcd.points)
colors = np.tile([0.8, 0.7, 1.0], (len(pcd.points), 1))
pcd.colors = o3d.utility.Vector3dVector(colors)
o3d.visualization.draw_geometries([pcd],window_name="1")
# 半径离群点移除
cl, ind = pcd.remove_radius_outlier(nb_points=5, radius=20)

# 提取内点
inlier_cloud = pcd.select_by_index(ind)
outlier_cloud = pcd.select_by_index(ind, invert=True)

print(f"原始点云: {len(pcd.points)} 点")
print(f"滤波后: {len(inlier_cloud.points)} 点")
cd,cd1,cd2=chamfer_distance(pcd1=pcd,pcd2=inlier_cloud)
print(cd)