(八)k-dTree庫教程二--k-d樹在PCL中的應用
阿新 • • 發佈:2019-01-06
k-d樹在PCL中的應用
上一講中,我們介紹了k-d樹在二維上的原理,k-d樹構建演算法和k-d樹是如何應用於最近鄰查詢演算法的。對於PCL中的k-d樹來說,只不過是將二維提升到了三維而已。下面我們就看看PCL是如何將k-d樹應用於範圍搜尋和最近鄰搜尋的。
PCL中使用的k-d樹的演算法來自FLANN(Fast Library for Approximate Nearest Neighbors),它是目前最完整的(近似)最近鄰開源庫。不但實現了一系列查詢演算法,還包含了一種自動選取最快演算法的機制。
K近鄰搜尋
K近鄰搜尋是給定查詢點及正整數K,從資料集中找到距離查詢點最近的K個數據。
標頭檔案
#include <pcl/kdtree/kdtree_flann.h>
步驟
- 初始化k-d樹
// 初始化kdTree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 設定要搜尋的點雲
kdtree.setInputCloud(cloud);
- 輸入查詢點和正整數K進行搜尋
kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)
關鍵函式:
int
nearestKSearch (const PointT &point, int k,
std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;
- 輸入輸出引數:
- point[in]:查詢點
- k[in]:正整數K,即要搜尋的最近鄰點的數目
- k_indices[out]:搜尋到的最近鄰點在點雲中的下標
- k_sqr_distances[out]:搜尋到的最近鄰點距離查詢點的距離的平方值
- 返回值:搜尋到的最近鄰點的數目
範圍搜尋
範圍搜尋就是給定查詢點和查詢距離的閾值,從資料集中找出所有與查詢點距離小於閾值的資料。
標頭檔案
#include <pcl/kdtree/kdtree_flann.h>
步驟
- 初始化k-d樹
// 初始化kdTree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 設定要搜尋的點雲
kdtree.setInputCloud(cloud);
- 輸入查詢點和搜尋半徑進行搜尋
kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)
關鍵函式:
int
radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
- 輸入輸出引數:
- point[in]:查詢點
- radius[in]:搜尋半徑
- k_indices[out]:搜尋到的最近鄰點在點雲中的下標
- k_sqr_distances[out]:搜尋到的最近鄰點距離查詢點的距離的平方值
- max_nn[in]:預設為0,搜尋到的點的上限,如果搜尋到的點數目多於它,那麼丟棄超出的部分;如果max_nn設為0或者大於搜尋點雲的大小,則返回所有找到的點。
- 返回值:搜尋到的最近鄰點的數目
程式
/*
* 功能:kdTree的應用,包括k最近鄰搜尋和範圍搜尋
*/
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
using namespace std;
int
main(int argc, char** argv)
{
// 種下隨機數種子
srand(time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 用隨機數,隨機生成1000個點的無序點雲
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); // c++中rand()函式生成的範圍:0~RAND_MAX,
cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
// 初始化kdTree
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;
}
// 以radius為半徑的範圍搜尋
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;
}
system("pause");
return 0;
}