feat:测试kdtree通过

PclPointCloud.cs

@@ -8,21 +8,25 @@ public class PclPointCloudXYZ : IDisposable {
     public PclPointCloudXYZ() => handle = create_pointcloud_xyz();
     // 构造函数用于内部封装 handle 对象
     public PclPointCloudXYZ(IntPtr handle) => this.handle = handle;
-    public void Dispose() { delete_pointcloud(handle); handle = IntPtr.Zero; }
+    public void Dispose() { delete_pointcloud_xyz(handle); handle = IntPtr.Zero; }
+    
+    public void Resize(int size) => resize_pointcloud_xyz(handle, size);
 
     public bool Load(string path) => load_pcd_xyz( path,handle);
     public int Save(string path) => save_pcd_xyz(path,handle);
     public IntPtr Handle => handle;
     
-    public int Size => get_pointcloud_size(handle);
+    public int Size => get_pointcloud_size_xyz(handle);
     public bool GetPoint(int index, out float x, out float y, out float z) => get_point_xyz(handle, index, out x, out y, out z);
-
+    public void SetPoint(int index, float x, float y, float z) => set_point_xyz(handle, index, x, y, z);
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_pointcloud_xyz();
-    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud(IntPtr ptr);
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void resize_pointcloud_xyz(IntPtr ptr, int size);
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud_xyz(IntPtr ptr);
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern bool load_pcd_xyz(string path,IntPtr ptr);
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern int save_pcd_xyz(string path,IntPtr ptr);
-    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size(IntPtr cloud);
+    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size_xyz(IntPtr cloud);
     [DllImport("./pcl/libpclwrapper.dylib")] static extern bool get_point_xyz(IntPtr cloud, int index, out float x, out float y, out float z);
+    [DllImport("./pcl/libpclwrapper.dylib")] static extern void set_point_xyz(IntPtr cloud, int index, float x, float y, float z);
 }
 
 
@@ -32,20 +36,26 @@ public class PclPointCloudXYZI : IDisposable {
     public PclPointCloudXYZI() => handle = create_pointcloud_xyzi();
     // 构造函数用于内部封装 handle 对象
     public PclPointCloudXYZI(IntPtr handle) => this.handle = handle;
-    public void Dispose() { delete_pointcloud(handle); handle = IntPtr.Zero; }
+    public void Dispose() { delete_pointcloud_xyzi(handle); handle = IntPtr.Zero; }
+    
+    public void Resize(int size) => resize_pointcloud_xyzi(handle, size);
 
     public bool Load(string path) => load_pcd_xyzi( path,handle);
     public int Save(string path) => save_pcd_xyzi(path,handle);
     public IntPtr Handle => handle;
-    public int Size => get_pointcloud_size(handle);
+    public int Size => get_pointcloud_size_xyzi(handle);
     public bool GetPoint(int index, out float x, out float y, out float z, out float intensity) => get_point_xyzi(handle, index, out x, out y, out z, out intensity);
+    public void SetPoint(int index, float x, float y, float z, float intensity) => set_point_xyzi(handle, index, x, y, z, intensity);
 
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_pointcloud_xyzi();
-    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud(IntPtr ptr);
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud_xyzi(IntPtr ptr);
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void resize_pointcloud_xyzi(IntPtr ptr, int size);
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern bool load_pcd_xyzi(string path,IntPtr ptr);
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern int save_pcd_xyzi(string path,IntPtr ptr);
-    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size(IntPtr cloud);
+    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size_xyzi(IntPtr cloud);
     [DllImport("./pcl/libpclwrapper.dylib")] static extern bool get_point_xyzi(IntPtr cloud, int index, out float x, out float y, out float z, out float intensity);
+    [DllImport("./pcl/libpclwrapper.dylib")] static extern void set_point_xyzi(IntPtr cloud, int index, float x, float y, float z, float intensity);
+
 }
 
 public class PclVoxelGridXYZ : IDisposable {
@@ -85,4 +95,58 @@ public class PclVoxelGridXYZI : IDisposable {
     [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr apply_voxel_filter_xyzi(IntPtr filterPtr);
 
     
+}
+
+public class PclKdTreeXYZ : IDisposable {
+    private IntPtr handle;
+
+    public PclKdTreeXYZ() => handle = create_kdtree_xyz();
+    public void Dispose() {
+        if (handle != IntPtr.Zero) {
+            delete_kdtree_xyz(handle);
+            handle = IntPtr.Zero;
+        }
+    }
+
+    public void SetInputCloud(PclPointCloudXYZ cloud) => set_kdtree_input_cloud_xyz(handle, cloud.Handle);
+
+    /// 最近邻搜索（KNN）
+    public (int[] indices, float[] distances) NearestKSearch(float x, float y, float z, int k) {
+        int[] indices = new int[k];
+        float[] distances = new float[k];
+        int found = knn_search_xyz(handle, x, y, z, k, indices, distances);
+        Array.Resize(ref indices, found);
+        Array.Resize(ref distances, found);
+        return (indices, distances);
+    }
+
+    /// 半径搜索
+    public (int[] indices, float[] distances) RadiusSearch(float x, float y, float z, float radius, int maxCount = 100) {
+        int[] indices = new int[maxCount];
+        float[] distances = new float[maxCount];
+        int found = radius_search_xyz(handle, x, y, z, radius, indices, distances, maxCount);
+        Array.Resize(ref indices, found);
+        Array.Resize(ref distances, found);
+        return (indices, distances);
+    }
+
+    public IntPtr Handle => handle;
+
+    #region DllImport
+
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_kdtree_xyz();
+
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_kdtree_xyz(IntPtr kd);
+
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_kdtree_input_cloud_xyz(IntPtr kd, IntPtr cloud);
+
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int knn_search_xyz(
+        IntPtr kd, float x, float y, float z, int k,
+        [Out] int[] indices, [Out] float[] distances);
+
+    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int radius_search_xyz(
+        IntPtr kd, float x, float y, float z, float radius,
+        [Out] int[] indices, [Out] float[] distances, int maxCount);
+
+    #endregion
 }

Program.cs

@@ -1,7 +1,4 @@
-using System;
+class Program
-using System.Runtime.InteropServices;
-
-class Program
 {
     static void Main()
     {
@@ -16,5 +13,17 @@ class Program
         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)}");
     }
 }