kdtree测试
#include <pcl/point_cloud.h> //点类型定义头文件 #include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件 #include <iostream> #include <vector> #include <ctime> int main (int argc, char** argv) { srand (time (NULL)); //用系统时间初始化随机种子 //创建一个PointCloud<pcl::PointXYZ> pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // 随机点云生成 cloud->width = 1000; //此处点云数量 cloud->height = 1; //表示点云为无序点云 cloud->points.resize (cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size (); ++i) //循环填充点云数据 { cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); // // 产生数值为0-1024的浮点数 cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f); cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f); } //创建KdTreeFLANN对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) //设置搜索空间 kdtree.setInputCloud (cloud); //设置查询点并赋随机值 pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f); // K 临近搜索 //创建一个整数（设置为10）和两个向量来存储搜索到的K近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方 int K = 10; std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引 std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方 //打印相关信息 std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索 { //打印所有近邻坐标 for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i) std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x << " " << cloud->points[ pointIdxNKNSearch[i] ].y << " " << cloud->points[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl; } /********************************************************************************** 下面的代码展示查找到给定的searchPoint的某一半径（随机产生）内所有近邻，重新定义两个向量 pointIdxRadiusSearch pointRadiusSquaredDistance来存储关于近邻的信息 ********************************************************************************/ // 半径 R内近邻搜索方法 std::vector<int> pointIdxRadiusSearch; //存储近邻索引 std::vector<float> pointRadiusSquaredDistance; //存储近邻对应距离的平方 float radius = 256.0f * rand () / (RAND_MAX + 1.0f); //随机的生成某一半径 //打印输出 std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; // 假设我们的kdtree返回了大于0个近邻。那么它将打印出在我们"searchPoint"附近的10个最近的邻居并把它们存到先前创立的向量中。 if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法 { for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i) std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x << " " << cloud->points[ pointIdxRadiusSearch[i] ].y << " " << cloud->points[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } return 0; }输出
K nearest neighbor search at (384.369 386.372 300.143) with K=10 451.114 363.553 229.213 (squared distance: 10006.7) 452.721 398.977 207.076 (squared distance: 13492.4) 276.971 343.368 327.111 (squared distance: 14111) 447.772 491.576 334 (squared distance: 16234.2) 410.276 386.502 175.288 (squared distance: 16259.9) 445.201 488.637 253.626 (squared distance: 16322.6) 257.128 435.577 321.633 (squared distance: 19073.3) 312.382 389.858 178.369 (squared distance: 20023) 439.166 412.965 167.177 (squared distance: 21390) 482.03 493.711 266.617 (squared distance: 22183.4) Neighbors within radius search at (384.369 386.372 300.143) with radius=193.159 451.114 363.553 229.213 (squared distance: 10006.7) 452.721 398.977 207.076 (squared distance: 13492.4) 276.971 343.368 327.111 (squared distance: 14111) 447.772 491.576 334 (squared distance: 16234.2) 410.276 386.502 175.288 (squared distance: 16259.9) 445.201 488.637 253.626 (squared distance: 16322.6) 257.128 435.577 321.633 (squared distance: 19073.3) 312.382 389.858 178.369 (squared distance: 20023) 439.166 412.965 167.177 (squared distance: 21390) 482.03 493.711 266.617 (squared distance: 22183.4) 521.246 429.693 256.285 (squared distance: 22535.5) 512.669 307.615 320.109 (squared distance: 23062.3) 529.352 399.11 354.953 (squared distance: 24186.5) 292.235 430.095 178.472 (squared distance: 25204.2) 468.447 428.21 169.938 (squared distance: 25772.9) 486.896 429.648 184.04 (squared distance: 25864.7) 484.24 305.494 397.702 (squared distance: 26033.2) 285.843 501.384 233.359 (squared distance: 27395.3) 314.749 252.534 227.244 (squared distance: 28073.7) 370.492 316.152 146.118 (squared distance: 28846.8) 272.179 257.047 296.281 (squared distance: 29326.1) 424.155 426.806 135.94 (squared distance: 30180.5) 535.554 370.488 215.772 (squared distance: 30227.8) 237.352 309.898 353.828 (squared distance: 30344.3) 222.959 424.764 356.473 (squared distance: 30700.3) 505.552 501.396 241.199 (squared distance: 31390.5) 519.049 502.714 303.867 (squared distance: 31688.1) 374.487 562.587 337.03 (squared distance: 32510.2) 471.023 434.838 140.425 (squared distance: 35367.6) 204.779 353.992 361.033 (squared distance: 37008.4) 219.731 482.102 272.668 (squared distance: 37024.6)kdtree扩展
支持百万级别的图层快速搜索和光线追踪
- 内存管理：确保高效地利用内存。
- 数据结构优化：使用合适的数据结构来存储节点和图层信息。
- 算法优化：确保搜索算法能够快速找到最近的图层。
- 并行处理：利用多线程或多进程来加速搜索过程。
扩展到图层搜索
struct Layer { Vec3 position; // 图层的位置 float radius; // 图层的半径 // ... 其他图层属性 }; struct KDTreeNode { int axis; // 当前节点分割轴 float split; // 分割值 std::vector<Layer> layers; // 存储在此节点内的图层 KDTreeNode* left; // 左子树 KDTreeNode* right; // 右子树 }; KDTreeNode* buildKDTree(std::vector<Layer>& layers, int depth = 0) { if (layers.empty()) return nullptr; int axis = depth % 3; // 选择 x, y, z 轴 std::sort(layers.begin(), layers.end(), [axis](const Layer& a, const Layer& b) { return a.position[axis] < b.position[axis]; }); int mid = layers.size() / 2; KDTreeNode* node = new KDTreeNode(); node->axis = axis; node->split = layers[mid].position[axis]; node->left = buildKDTree(std::vector<Layer>(layers.begin(), layers.begin() + mid), depth + 1); node->right = buildKDTree(std::vector<Layer>(layers.begin() + mid + 1, layers.end()), depth + 1); node->layers = std::vector<Layer>(layers.begin() + mid, layers.begin() + mid + 1); return node; }加入光线追踪
struct Ray { Vec3 origin; // 光线起点 Vec3 direction; // 光线方向 }; bool intersectRayLayer(const Ray& ray, const Layer& layer, float& t) { // 实现光线与图层相交的计算 // 返回是否相交，t 为交点距离 // ... } std::vector<Layer> searchKDTree(KDTreeNode* node, const Ray& ray, float t_min, float t_max) { std::vector<Layer> result; if (!node) return result; // 检查光线是否与当前节点的图层相交 for (const auto& layer : node->layers) { float t; if (intersectRayLayer(ray, layer, t) && t >= t_min && t <= t_max) { result.push_back(layer); } } // 根据光线与分割平面的关系决定搜索哪一侧 if (ray.origin[node->axis] < node->split) { result.insert(result.end(), searchKDTree(node->left, ray, t_min, t_max).begin(), searchKDTree(node->left, ray, t_min, t_max).end()); result.insert(result.end(), searchKDTree(node->right, ray, t_min, t_max).begin(), searchKDTree(node->right, ray, t_min, t_max).end()); } else { result.insert(result.end(), searchKDTree(node->right, ray, t_min, t_max).begin(), searchKDTree(node->right, ray, t_min, t_max).end()); result.insert(result.end(), searchKDTree(node->left, ray, t_min, t_max).begin(), searchKDTree(node->left, ray, t_min, t_max).end()); } return result; }扩展测试
int main() { // 准备图层数据 std::vector<Layer> layers; // 填充 layers... // 构建 KDTree KDTreeNode* root = buildKDTree(layers); // 定义光线 Ray ray; ray.origin = {0, 0, 0}; ray.direction = {1, 0, 0}; // 搜索 KDTree std::vector<Layer> intersections = searchKDTree(root, ray, 0.01, 1000.0); // 打印结果 for (const auto& layer : intersections) { // 打印相交的图层信息 // ... } // 清理内存 // ... return 0; }参考
C++ Rect And Point Search Algorithm-CSDN博客
https://zhuanlan.zhihu.com/p/268524083
PCL(Point Cloud Library)学习记录（2024） · 语雀
GitHub - HuangCongQing/3D-LIDAR-Multi-Object-Tracking: 🔥3D-MOT(点云多目标检测和追踪C++) （2020 · 秋） 代码有详细注解
GitHub - HuangCongQing/pcl-learning: 🔥PCL（Point Cloud Library）点云库学习记录
https://www.cnblogs.com/li-yao7758258/p/6437440.html
