class Program
{
    static void Main()
    {
        using var cloud = new PclPointCloudXYZ();
        cloud.Load("./dataset/0.pcd");
        Console.WriteLine("加载点云大小为：" + cloud.Size);

        using var voxel = new PclVoxelGridXYZ();
        voxel.SetLeafSize(0.1f, 0.1f, 0.1f);
        voxel.SetInputCloud(cloud);

        using var filtered = voxel.Filter();
        Console.WriteLine("体素滤波后点云大小为：" + filtered.Size);
        filtered.Save("output.pcd");
        
        using var cloud1 = new PclPointCloudXYZ();
        cloud1.Resize(3);
        cloud1.SetPoint(0, 0, 0, 0);
        cloud1.SetPoint(1, 1, 1, 1);
        cloud1.SetPoint(2, 2, 2, 2);

        using var kdtree = new PclKdTreeXYZ();
        kdtree.SetInputCloud(cloud1);

        var (idx, dist) = kdtree.NearestKSearch(0.5f, 0.5f, 0.5f, 2);
        Console.WriteLine($"KNN indices: {string.Join(",", idx)}");
    }
}