原理
k-d tree算法原理及实现
八叉树及K-D树的应用和实现
详解KDTree
Kd-Tree算法原理和开源实现代码
kdtree_search(PCL)
kdtree_search.cpp
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#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 =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);
cloud->points[i].y =1024.0f* rand () / (RAND_MAX +1.0f);
cloud->points[i].z =1024.0f* rand () / (RAND_MAX +1.0f);
}
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
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近邻搜索
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 )
{
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;
}
}
// 在半径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;
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 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})
