#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <vector>
#include <ctime>
//pcl::PointCloud<pcl::PointXYZ>::points是vector类型,可以使用push_back添加随机点
const float R = 5; //半径内近邻搜索的半径
int main() {
std::cout << "Hello, World!" << std::endl;
srand((unsigned int) time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud_in (new pcl::PointCloud<pcl::PointXYZ>);
//读入点云
if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd",*pCloud_in) == -1)
{
PCL_ERROR("ERROR Load...");
return -1;
}
std::cout<<"load succeed"<<std::endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
octree.setInputCloud(pCloud_in);
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);
searchPoint.x = 0.0;
searchPoint.y = 0.0;
searchPoint.z = 0.0;
//体素内近邻搜索
std::vector<int> pointIdxVec;
if(octree.voxelSearch(searchPoint,pointIdxVec))
{
std::cout<<"Neighbors within voxel search ar ("<<searchPoint.x<<", "<<searchPoint.y<<", "<<searchPoint.z<<")"<<std::endl;
std::cout<<pointIdxVec.size()<<" points found"<<std::endl;
for(size_t i=0;i<pointIdxVec.size();i++)
pcl中octree三种检测方法比较并实现可视化
最新推荐文章于 2022-10-29 09:48:33 发布