octree
基于octree的空间划分及搜索操作
代码如下:
#include<pcl\point_cloud.h>
#include<pcl\octree\octree.h>
#include<iostream>
#include<vector>
#include<ctime>
#include<pcl\io\pcd_io.h>
using namespace std;
int main()
{
//pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//用Ptr指针创建点云对象
if (pcl::io::loadPCDFile("E:\\test_PCL\\ConsoleApplication1\\ConsoleApplication1\\pipline.pcd",*cloud) == -1)//读取pcd点云图像
{
cout << "read failed" << endl;
}
float resolution = 128.0f;//定义分辨率,即octree最小体素的尺寸
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//创建octree对象
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
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);
// 体素内近邻搜索
vector<int>pointIdxVec; //存放近邻点的索引
if (octree.voxelSearch(searchpoint, pointIdxVec))//如果存在近邻点
{
for (int i = 0; i < pointIdxVec.size();++i)
{
cout << " " << cloud->points[pointIdxVec[i]].x
<< " " << cloud->points[pointIdxVec[i]].y
<< " " << cloud->points[pointIdxVec[i]].z <<endl;
}
}
//K近邻搜索
int K = 10;//定义近邻点数
vector<int>pointIdxNKNSearch;//近邻点索引
vector<float>pointNKNSquaredDistance;//对应近邻点与搜索点的距离的平方
if(octree.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;
}
//半径内近邻搜索
vector<int>pointIdxRadiusSearch;
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(octree.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;
}
}
上面使用的是:pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>)定义点云对象;也可以使用pcl::PointCloud<pcl::PointXYZ> cloud进行定义,略有不同,除了cloud->points.x改为cloud.points.x,还有其他改动。代码如下:
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointer(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//用Ptr指针创建点云对象
//loadPCDFile("",cloud)和loadPCDFile("",*cloud),注意这个差别
if (pcl::io::loadPCDFile("E:\\test_PCL\\ConsoleApplication1\\ConsoleApplication1\\pipline.pcd",cloud) == -1)//读取pcd点云图像
{
cout << "read failed" << endl;
}
float resolution = 128.0f;//定义分辨率,即octree最小体素的尺寸
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//创建octree对象
cloudPointer = cloud.makeShared();//需要对类型进行转换
octree.setInputCloud(cloudPointer);//只接受Ptr类型的点云对象
octree.addPointsFromInputCloud();
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);