Introduction to PCL (4): Brief introduction and use of kdtree

Refer to the blog "European Clustering (KD-Tree) Detailed Explanation, Nanny-Level Tutorial" and "(Three Minutes) Learn Kd-tree Common Laser SLAM Point Cloud Search"

1. The meaning of kd tree

• What is a kd tree?

KD tree is a data structure of spatial division. For data of multiple dimensions, a certain dimension is selected according to a certain rule, sorted under this dimension, the middle data is selected as the dividing node, and then the left and right sides of the dividing node are respectively The data on the right undergoes the above partitioning steps.

• Why do you need a kd tree?

The amount of data in the three-dimensional point cloud is large. Using kd tree to search can reduce the time and ensure that the search and matching of related points in the point cloud are in a real-time state. All in all, kd trees can be used to quickly search point cloud data.

2. The use of kd tree

There are generally two tasks using kd trees, namely k-nearest neighbor search and within-radius search. The code implementation mainly refers to Shuangyu’s code

#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})

• operation result