Introdução ao PCL (4): Breve introdução e uso do kdtree

Consulte o blog "Explicação detalhada do cluster europeu (KD-Tree), tutorial de nível Nanny" e "(três minutos) Aprenda pesquisa de nuvem de pontos SLAM de laser comum Kd-tree"

1. O significado da árvore kd

• O que é uma árvore kd?

A árvore KD é uma estrutura de dados de divisão espacial. Para dados de múltiplas dimensões, uma determinada dimensão é selecionada de acordo com uma determinada regra, classificada sob esta dimensão, os dados do meio são selecionados como o nó de divisão e, em seguida, os lados esquerdo e direito de os nós divisores são respectivamente. Os dados à direita passam pelas etapas de particionamento acima.

• Por que precisamos da árvore kd?

A quantidade de dados na nuvem de pontos tridimensional é grande. Usar a árvore kd para pesquisar pode reduzir o tempo e garantir que a pesquisa e a correspondência de pontos relacionados na nuvem de pontos estejam em tempo real. Resumindo, as árvores kd podem ser usadas para pesquisar rapidamente dados de nuvens de pontos.

2. Uso da árvore kd

Geralmente, há duas tarefas usando árvores kd, ou seja, pesquisa por k-vizinho mais próximo e pesquisa dentro do raio. A implementação do código refere-se principalmente ao 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 da operação