From fbf9e947006a22affd329cd0ac08bc67dcc13fd3 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Sat, 12 Apr 2025 18:10:00 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0KdTreeWrapper?= =?UTF-8?q?=E5=8F=8A=E7=9B=B8=E5=85=B3=E6=8E=A5=E5=8F=A3=E4=BB=A5=E6=94=AF?= =?UTF-8?q?=E6=8C=81KNN=E5=92=8C=E5=8D=8A=E5=BE=84=E6=90=9C=E7=B4=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + include/pcl_wrapper/kdtree.hpp | 50 +++++++++++++++++ include/pcl_wrapper/pcl_wrapper_api.hpp | 25 ++++++--- wrapper.cpp | 72 +++++++++++++++++++++++-- 4 files changed, 136 insertions(+), 12 deletions(-) create mode 100644 include/pcl_wrapper/kdtree.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a5c393..b3a7122 100644 --- a/CMakeLists.txt +++ b/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 库 diff --git a/include/pcl_wrapper/kdtree.hpp b/include/pcl_wrapper/kdtree.hpp new file mode 100644 index 0000000..260e835 --- /dev/null +++ b/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 +#include "cloud_handle.hpp" + +namespace pcl_wrapper { + + template + class KdTreeWrapper { + public: + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + KdTreeWrapper() : kdtree(new pcl::KdTreeFLANN()) {} + + void setInputCloud(PointCloudPtr cloud) { + kdtree->setInputCloud(cloud); + } + + // KNN search + bool nearestKSearch(const PointT& pt, int k, + std::vector& indices, std::vector& 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& indices, std::vector& distances) { + return kdtree->radiusSearch(pt,radius, indices, distances ) > 0; + } + + private: + typename pcl::KdTreeFLANN::Ptr kdtree; + }; + + using KdTreeXYZ = KdTreeWrapper; + using KdTreeXYZI = KdTreeWrapper; + + // 显式实例化 + template class KdTreeWrapper; + template class KdTreeWrapper; + +} +#endif //PCL_WRAPPER_KDTREE_HPP diff --git a/include/pcl_wrapper/pcl_wrapper_api.hpp b/include/pcl_wrapper/pcl_wrapper_api.hpp index 5593658..6cd61b4 100644 --- a/include/pcl_wrapper/pcl_wrapper_api.hpp +++ b/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); diff --git a/wrapper.cpp b/wrapper.cpp index 4f03a5b..da169e8 100644 --- a/wrapper.cpp +++ b/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(cloud); pc->cloud->resize(count); } +void resize_pointcloud_xyzi(void* cloud, int count) { + auto* pc = static_cast(cloud); + pc->cloud->resize(count); +} + void set_point_xyz(void* cloud, int index, float x, float y, float z) { auto* pc = static_cast(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(cloud); // 可根据类型再区分 +void delete_pointcloud_xyz(void* cloud) { + delete static_cast(cloud); } -int get_pointcloud_size(void* cloud) { +void delete_pointcloud_xyzi(void* cloud) { + delete static_cast(cloud); +} + +int get_pointcloud_size_xyz(void* cloud) { auto* pc = static_cast(cloud); return static_cast(pc->cloud->size()); } +int get_pointcloud_size_xyzi(void* cloud) { + auto* pc = static_cast(cloud); + return static_cast(pc->cloud->size()); +} + bool get_point_xyz(void* cloud, int index, float* x, float* y, float* z) { auto* pc = static_cast(cloud); if (index < 0 || index >= pc->cloud->size()) return false; @@ -105,6 +120,55 @@ void delete_voxel_filter(void* ptr) { delete static_cast(ptr); // 可再细分 } +// === KdTree相关 === + +void* create_kdtree_xyz() { + return new KdTreeXYZ(); +} + +void set_kdtree_input_cloud_xyz(void* kd, void* cloud) { + auto* kdtree = static_cast(kd); + auto* pc = static_cast(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(kd); + pcl::PointXYZ pt(x, y, z); + std::vector indices; + std::vector 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(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(kd); + pcl::PointXYZ pt(x, y, z); + std::vector indices; + std::vector distances; + indices.reserve(max_count); + distances.reserve(max_count); + bool success = kdtree->radiusSearch(pt, radius, indices, distances); + int count = std::min(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(kd); +} + + // === IO相关 === bool load_pcd_xyz(const char* filename, void* out) {