PCL利用kdtree搜寻邻域点代码
- 代码功能:用两种方式搜索并且显示某点的邻域点的坐标和距离信息(半径限制和个数限制)
- 代码缺陷:没有添加可视化过程
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
int main(int argc, char** argv)
{
std::string filename("C:\\Users\\426-4\\Desktop\\2_gt.pcd");
//visualizer
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile(filename, *cloud);
//搜索source关键点周围的点云
//创建kdtree 结构
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//传入点云
//source_Ransac_keypoint_trans
kdtree.setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr source_key_Neigh(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr source_key_point(new pcl::PointCloud<pcl::PointXYZ>());
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
std::vector<int> pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;
//设置输入点
pcl::PointXYZ searchPoint; //source_22
int j;
for (int i = 0; i < cloud->size(); i++)
{
//设置输入点条件
if (i == 10)
{
searchPoint = cloud->at(i);
source_key_point->push_back(cloud->at(i));
//设置邻域半径
float radius = 0.005;
int K = 10;
//利用最小半径搜索邻近点
//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;
//}
//利用最少点数搜索邻近点
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;
}
break;
}
}
}
结果实例:
0.07308 -0.00916 0.00951 (squared distance: 0)
0.07357 -0.0144 0.0197 (squared distance: 0.000131534)
0.07563 -0.00802 -0.00166 (squared distance: 0.000132571)
0.08422 -0.00824 0.00407 (squared distance: 0.00015454)
0.06549 -0.00812 -0.00243 (squared distance: 0.000201253)
0.084 -0.01291 0.0178 (squared distance: 0.000202033)
0.05903 -0.01183 0.01584 (squared distance: 0.0002446)
0.08882 -0.01174 0.01563 (squared distance: 0.000291858)
0.05964 -0.00865 -0.00642 (squared distance: 0.000434659)
0.0584 -0.01665 0.02257 (squared distance: 0.000442166)