152 lines
7.9 KiB
C#
152 lines
7.9 KiB
C#
using System;
|
||
using System.Drawing;
|
||
using System.Runtime.InteropServices;
|
||
|
||
public class PclPointCloudXYZ : IDisposable {
|
||
private IntPtr handle;
|
||
|
||
public PclPointCloudXYZ() => handle = create_pointcloud_xyz();
|
||
// 构造函数用于内部封装 handle 对象
|
||
public PclPointCloudXYZ(IntPtr handle) => this.handle = handle;
|
||
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_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 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_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);
|
||
}
|
||
|
||
|
||
public class PclPointCloudXYZI : IDisposable {
|
||
private IntPtr handle;
|
||
|
||
public PclPointCloudXYZI() => handle = create_pointcloud_xyzi();
|
||
// 构造函数用于内部封装 handle 对象
|
||
public PclPointCloudXYZI(IntPtr handle) => this.handle = handle;
|
||
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_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_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_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 {
|
||
private IntPtr handle;
|
||
|
||
public PclVoxelGridXYZ() => handle = create_voxel_filter_xyz();
|
||
public void Dispose() { delete_voxel_filter(handle); handle = IntPtr.Zero; }
|
||
|
||
public void SetLeafSize(float x, float y, float z) => set_voxel_leaf_size(handle, x, y, z);
|
||
public void SetInputCloud(PclPointCloudXYZ cloud) => set_voxel_input_cloud(handle, cloud.Handle);
|
||
public PclPointCloudXYZ Filter() => new PclPointCloudXYZ(apply_voxel_filter_xyz(handle));
|
||
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_voxel_filter_xyz();
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_voxel_filter(IntPtr ptr);
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_leaf_size(IntPtr ptr, float x, float y, float z);
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_input_cloud(IntPtr filterPtr, IntPtr cloudPtr);
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr apply_voxel_filter_xyz(IntPtr filterPtr);
|
||
|
||
|
||
}
|
||
|
||
|
||
public class PclVoxelGridXYZI : IDisposable {
|
||
private IntPtr handle;
|
||
|
||
public PclVoxelGridXYZI() => handle = create_voxel_filter_xyzi();
|
||
public void Dispose() { delete_voxel_filter(handle); handle = IntPtr.Zero; }
|
||
|
||
public void SetLeafSize(float x, float y, float z) => set_voxel_leaf_size(handle, x, y, z);
|
||
public void SetInputCloud(PclPointCloudXYZI cloud) => set_voxel_input_cloud(handle, cloud.Handle);
|
||
public PclPointCloudXYZI Filter() => new PclPointCloudXYZI(apply_voxel_filter_xyzi(handle));
|
||
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_voxel_filter_xyzi();
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_voxel_filter(IntPtr ptr);
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_leaf_size(IntPtr ptr, float x, float y, float z);
|
||
[DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_input_cloud(IntPtr filterPtr, IntPtr cloudPtr);
|
||
[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
|
||
} |