Tabla de contenido
Consulte el blog "Explicación detallada de la agrupación europea (KD-Tree), tutorial a nivel de niñera" y "(Tres minutos) Aprenda la búsqueda de nube de puntos SLAM láser común de Kd-tree"
1. El significado del árbol kd
- ¿Qué es un árbol kd?
El árbol KD es una estructura de datos de división espacial. Para datos de múltiples dimensiones, se selecciona una determinada dimensión de acuerdo con una determinada regla, se clasifica según esta dimensión, los datos del medio se seleccionan como nodo divisorio y luego los lados izquierdo y derecho de los nodos divisorios son respectivamente. Los datos de la derecha están sujetos a los pasos de división anteriores.
- ¿Por qué necesitamos el árbol kd?
La cantidad de datos en la nube de puntos tridimensionales es grande. El uso del árbol kd para buscar puede reducir el tiempo y garantizar que la búsqueda y coincidencia de puntos relacionados en la nube de puntos se realicen en tiempo real. Con todo, los árboles kd se pueden utilizar para buscar rápidamente datos de nubes de puntos.
2. Uso del árbol kd
Generalmente hay dos tareas que utilizan árboles kd, a saber, la búsqueda de k-vecino más cercano y la búsqueda dentro del radio. La implementación del código se refiere principalmente al código de Shuangyu.
#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <vector>
#include <ctime>
int main(int argc, char** argv)
{
srand(time(NULL));
//第一步：生成点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = 100;
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);
cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
//第二步：生成搜索点
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);
//第三步：定义kd树
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud); //将点云cloud作为输入
//第四步：采用kdtree.nearestKSearch方法，输出点searchPoint的最近10个点云
int K = 10;
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
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;
}
}
//第五步：采用kdtree.radiusSearch方法，输出点searchPoint的给定半径距离内的其他点
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;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
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;
}
- CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(kdtree_search)
find_package(PCL)
add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})
- resultado de la operación
