import open3d as o3d
import numpy as np

def read_kitti_bin(bin_file):
    # 读取Kitti激光雷达点云数据的二进制文件
    scan = np.fromfile(bin_file, dtype=np.float32)
    scan = scan.reshape((-1, 4))  # 每行包含[x, y, z, intensity]
    return scan[:, :3]  # 返回点云的前三列，即[x, y, z]

def write_kitti_bin(bin_file, point_cloud):
    # 将点云数据保存为Kitti激光雷达点云的二进制文件
    point_cloud = np.array(point_cloud, dtype=np.float32)
    point_cloud.tofile(bin_file)

if __name__ == "__main__":
    input_file = r"E:\rosbag\2011_09_26\2011_09_26_drive_0005_sync\velodyne_points\data\0000000000.bin"  # 替换为实际的输入文件路径
    output_file = r"E:\rosbag\pointcloud_downsampled.pcd"  # 替换为实际的输出文件路径
    voxel_size = 0.1  # 替换为您想要的体素大小（降采样程度）

    # 读取点云数据
    point_cloud = read_kitti_bin(input_file)

    # 创建Open3D点云对象
    o3d_cloud = o3d.geometry.PointCloud()
    o3d_cloud.points = o3d.utility.Vector3dVector(point_cloud)

    # 进行降采样
    downsampled_cloud = o3d_cloud.voxel_down_sample(voxel_size)

    # 获取降采样后的点云数据
    downsampled_points = np.asarray(downsampled_cloud.points)

    # 可视化降采样前后的点云
    o3d.visualization.draw_geometries([o3d_cloud], "Original Point Cloud")
    o3d.visualization.draw_geometries([downsampled_cloud], "Downsampled Point Cloud")

    # 注释掉保存降采样结果的部分
    # write_kitti_bin(output_file, downsampled_points)

import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
def read_kitti_bin(bin_file):
    # 读取Kitti激光雷达点云数据的二进制文件
    scan = np.fromfile(bin_file, dtype=np.float32)
    scan = scan.reshape((-1, 4))  # 每行包含[x, y, z, intensity]
    return scan[:, :3]  # 返回点云的前三列，即[x, y, z]

if __name__ == "__main__":
    input_file = r"E:\rosbag\2011_09_26\2011_09_26_drive_0005_sync\velodyne_points\data\0000000000.bin"  # 替换为实际的输入文件路径
    voxel_size = 0.1  # 替换为您想要的体素大小（降采样程度）

    # 读取点云数据
    point_cloud = read_kitti_bin(input_file)

    # 创建Open3D点云对象
    o3d_cloud = o3d.geometry.PointCloud()
    o3d_cloud.points = o3d.utility.Vector3dVector(point_cloud)

    # 进行降采样
    downsampled_cloud = o3d_cloud.voxel_down_sample(voxel_size)

    # 使用RANSAC算法进行地面点云滤除
    plane_model, inliers = downsampled_cloud.segment_plane(distance_threshold=0.2, ransac_n=3, num_iterations=1000)
    inlier_cloud = downsampled_cloud.select_by_index(inliers, invert=True)

    # 使用DBSCAN算法进行聚类
    with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
        labels = np.array(inlier_cloud.cluster_dbscan(eps=0.5, min_points=10, print_progress=True))

    # 获取聚类结果并根据标签着色
    max_label = labels.max()
    colors = plt.get_cmap("tab20")(labels / (max_label if max_label > 0 else 1))
    colors[labels < 0] = 0  # 将噪声点设为黑色
    inlier_cloud.colors = o3d.utility.Vector3dVector(colors[:, :3])

    # 显示聚类结果
    o3d.visualization.draw_geometries([inlier_cloud], "Clustered Point Cloud")