12345qiupeng
/
pcl_wrapper_test
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat:测试kdtree通过

main
邱棚 2025-04-12 18:09:49 +08:00
parent b321f2e9b0
commit f235b66b94
3 changed files with 86 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
PclPointCloud.cs
View File

@ -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 {
@ -86,3 +96,57 @@ public class PclVoxelGridXYZI : IDisposable {
}
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
}

17
Program.cs
View File

@ -1,7 +1,4 @@
using System;
using System.Runtime.InteropServices;
class Program
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)}");
}
}

BIN
pcl/libpclwrapper.dylib
View File

Binary file not shown.