PCL 入門 (4): kdtree の簡単な紹介と使用法

ブログ「ヨーロッパ クラスタリング (KD-Tree) の詳細な説明、ナニー レベルのチュートリアル」および「(3 分間) Kd-tree 共通レーザー SLAM 点群検索を学ぶ」を参照してください。

1. kdツリーの意味

• kdツリーとは何ですか?

KDツリーは空間分割のデータ構造であり、多次元のデータに対して、ある規則に従ってある次元を選択し、その次元でソートし、中央のデータを分割ノードとして選択し、その左辺と右辺を分割する。分割ノードはそれぞれ、右側のデータに上記の分割ステップが適用されます。

• なぜ kd ツリーが必要なのでしょうか?

3 次元点群のデータ量は大きいため、kd ツリーを使用して検索することで時間を短縮し、点群内の関連点の検索とマッチングをリアルタイムに行うことができます。全体として、kd ツリーを使用すると、点群データを迅速に検索できます。

2. kdツリーの使用

一般に、kd ツリーを使用するタスクは 2 つあります。それは、k 最近傍検索と半径内検索です。コード実装は主に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})

• 演算結果