feat: 添加KdTreeWrapper及相关接口以支持KNN和半径搜索

CMakeLists.txt

@@ -27,6 +27,7 @@ add_library(pclwrapper SHARED wrapper.cpp
         include/pcl_wrapper/cloud_handle.hpp
         include/pcl_wrapper/voxel_filter.hpp
         include/pcl_wrapper/io.hpp
+        include/pcl_wrapper/kdtree.hpp
 )
 
 # 指定 OpenMP 和 PCL 库

include/pcl_wrapper/kdtree.hpp

@@ -0,0 +1,50 @@
+//
+// Created by QP on 2025/4/12.
+//
+
+#ifndef PCL_WRAPPER_KDTREE_HPP
+#define PCL_WRAPPER_KDTREE_HPP
+#pragma once
+#include <pcl/kdtree/kdtree_flann.h>
+#include "cloud_handle.hpp"
+
+namespace pcl_wrapper {
+
+    template<typename PointT>
+    class KdTreeWrapper {
+    public:
+        using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
+
+        KdTreeWrapper() : kdtree(new pcl::KdTreeFLANN<PointT>()) {}
+
+        void setInputCloud(PointCloudPtr cloud) {
+            kdtree->setInputCloud(cloud);
+        }
+
+        // KNN search
+        bool nearestKSearch(const PointT& pt, int k,
+                            std::vector<int>& indices, std::vector<float>& distances) {
+            indices.resize(k);
+            distances.resize(k);
+            return kdtree->nearestKSearch(pt, k, indices, distances) > 0;
+        }
+
+        // Radius search
+        bool radiusSearch(const PointT& pt, double radius,
+                          std::vector<int>& indices, std::vector<float>& distances) {
+            return kdtree->radiusSearch(pt,radius, indices, distances ) > 0;
+        }
+
+    private:
+        typename pcl::KdTreeFLANN<PointT>::Ptr kdtree;
+    };
+
+    using KdTreeXYZ  = KdTreeWrapper<pcl::PointXYZ>;
+    using KdTreeXYZI = KdTreeWrapper<pcl::PointXYZI>;
+
+    // 显式实例化
+    template class KdTreeWrapper<pcl::PointXYZ>;
+    template class KdTreeWrapper<pcl::PointXYZI>;
+
+}
+#endif //PCL_WRAPPER_KDTREE_HPP

include/pcl_wrapper/pcl_wrapper_api.hpp

@@ -13,16 +13,16 @@ extern "C" {
 
 void* create_pointcloud_xyz();
 void* create_pointcloud_xyzi();
-void  reserve_pointcloud_xyz(void* cloud, int count);
+void  resize_pointcloud_xyz(void* cloud, int count);
+void  resize_pointcloud_xyzi(void* cloud, int count);
 void  set_point_xyz(void* cloud, int index, float x, float y, float z);
 void  set_point_xyzi(void* cloud, int index, float x, float y, float z, float intensity);
-void  delete_pointcloud(void* cloud);
-// 统一接口：获取点数量
-int get_pointcloud_size(void* cloud);
-// 读取 PointXYZ
-bool get_point_xyz(void* cloud, int index, float* x, float* y, float* z);
-// 读取 PointXYZI
-bool get_point_xyzi(void* cloud, int index, float* x, float* y, float* z, float* intensity);
+int get_pointcloud_size_xyz(void* cloud);
+int get_pointcloud_size_xyzi(void* cloud);
+bool get_point_xyz(void* cloud, int index, float* x, float* y, float* z); // 读取 PointXYZ
+bool get_point_xyzi(void* cloud, int index, float* x, float* y, float* z, float* intensity); // 读取 PointXYZI
+void delete_pointcloud_xyz(void* cloud);
+void delete_pointcloud_xyzi(void* cloud);
 
 // ========== Voxel Filter ==========
 
@@ -34,6 +34,15 @@ void* apply_voxel_filter_xyz(void* filter);
 void* apply_voxel_filter_xyzi(void* filter);
 void  delete_voxel_filter(void* filter);
 
+// ========== KdTree ==========
+void* create_kdtree_xyz();
+void set_kdtree_input_cloud_xyz(void* kdtree, void* cloud);
+int knn_search_xyz(void* kdtree, float x, float y, float z, int k,
+                   int* out_indices, float* out_distances);
+int radius_search_xyz(void* kdtree, float x, float y, float z, float radius,
+                      int* out_indices, float* out_distances, int max_count);
+void delete_kdtree_xyz(void* kdtree);
+
 // ========= IO ==========
 bool load_pcd_xyz(const char* filename, void* out);
 bool load_pcd_xyzi(const char* filename, void* out);

wrapper.cpp

@@ -1,5 +1,6 @@
 #include "cloud_handle.hpp"
 #include "voxel_filter.hpp"
+#include "kdtree.hpp"
 #include "io.hpp"
 #include "pcl_wrapper_api.hpp"
 
@@ -16,11 +17,16 @@ void* create_pointcloud_xyzi() {
     return new PointCloudXYZI();
 }
 
-void reserve_pointcloud_xyz(void* cloud, int count) {
+void resize_pointcloud_xyz(void* cloud, int count) {
     auto* pc = static_cast<PointCloudXYZ*>(cloud);
     pc->cloud->resize(count);
 }
 
+void resize_pointcloud_xyzi(void* cloud, int count) {
+    auto* pc = static_cast<PointCloudXYZI*>(cloud);
+    pc->cloud->resize(count);
+}
+
 void set_point_xyz(void* cloud, int index, float x, float y, float z) {
     auto* pc = static_cast<PointCloudXYZ*>(cloud);
     (*pc->cloud)[index].x = x;
@@ -36,15 +42,24 @@ void set_point_xyzi(void* cloud, int index, float x, float y, float z, float int
     (*pc->cloud)[index].intensity = intensity;
 }
 
-void delete_pointcloud(void* cloud) {
-    delete static_cast<PointCloudXYZ*>(cloud); // 可根据类型再区分
+void delete_pointcloud_xyz(void* cloud) {
+    delete static_cast<PointCloudXYZ*>(cloud);
 }
 
-int get_pointcloud_size(void* cloud) {
+void delete_pointcloud_xyzi(void* cloud) {
+    delete static_cast<PointCloudXYZI*>(cloud);
+}
+
+int get_pointcloud_size_xyz(void* cloud) {
     auto* pc = static_cast<PointCloudXYZ*>(cloud);
     return static_cast<int>(pc->cloud->size());
 }
 
+int get_pointcloud_size_xyzi(void* cloud) {
+    auto* pc = static_cast<PointCloudXYZI*>(cloud);
+    return static_cast<int>(pc->cloud->size());
+}
+
 bool get_point_xyz(void* cloud, int index, float* x, float* y, float* z) {
     auto* pc = static_cast<PointCloudXYZ*>(cloud);
     if (index < 0 || index >= pc->cloud->size()) return false;
@@ -105,6 +120,55 @@ void delete_voxel_filter(void* ptr) {
     delete static_cast<VoxelGridXYZ*>(ptr); // 可再细分
 }
 
+// === KdTree相关 ===
+
+void* create_kdtree_xyz() {
+    return new KdTreeXYZ();
+}
+
+void set_kdtree_input_cloud_xyz(void* kd, void* cloud) {
+    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
+    auto* pc = static_cast<PointCloudXYZ*>(cloud);
+    kdtree->setInputCloud(pc->cloud);
+}
+
+int knn_search_xyz(void* kd, float x, float y, float z, int k,
+                   int* out_indices, float* out_distances) {
+    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
+    pcl::PointXYZ pt(x, y, z);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    bool success = kdtree->nearestKSearch(pt, k, indices, distances);
+    if (!success) return 0;
+    for (int i = 0; i < k; ++i) {
+        out_indices[i] = indices[i];
+        out_distances[i] = distances[i];
+    }
+    return static_cast<int>(indices.size());
+}
+
+int radius_search_xyz(void* kd, float x, float y, float z, float radius,
+                      int* out_indices, float* out_distances, int max_count) {
+    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
+    pcl::PointXYZ pt(x, y, z);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    indices.reserve(max_count);
+    distances.reserve(max_count);
+    bool success = kdtree->radiusSearch(pt, radius, indices, distances);
+    int count = std::min<int>(indices.size(), max_count);
+    for (int i = 0; i < count; ++i) {
+        out_indices[i] = indices[i];
+        out_distances[i] = distances[i];
+    }
+    return count;
+}
+
+void delete_kdtree_xyz(void* kd) {
+    delete static_cast<KdTreeXYZ*>(kd);
+}
+
+
 // === IO相关 ===
 
 bool load_pcd_xyz(const char* filename, void* out) {