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