#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++)
{
std::cout<<" "<<pCloud_in->points[pointIdxVec[i]].x<<", "<<pCloud_in->points[pointIdxVec[i]].y<<", "<<pCloud_in->points[pointIdxVec[i]].z<<std::endl;
}
std::cout<<pointIdxVec.size()<<" points found"<<std::endl;
}
//将体素内近邻搜索结果放到result1
pcl::PointCloud<pcl::PointXYZ>::Ptr result1(new pcl::PointCloud<pcl::PointXYZ>);
result1->width = 50000;
result1->height = 1;
result1->is_dense = false;
result1->points.resize(result1->height*result1->width);
for(size_t i=0;i<pointIdxVec.size();i++)
{
result1->points[i].x = pCloud_in->points[pointIdxVec[i]].x;
result1->points[i].y = pCloud_in->points[pointIdxVec[i]].y;
result1->points[i].z = pCloud_in->points[pointIdxVec[i]].z;
}
//体素内近邻搜索结果保存
pcl::io::savePCDFileASCII("result1.pcd",*result1);
std::cout<<pointIdxVec.size()<<" result1.pcd saved "<<std::endl;
//K近邻搜索
int K=100000;
std::vector<int > pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;
if(octree.nearestKSearch(searchPoint,K,pointIdxNKNSearch, pointNKNSquaredDistance ) > 0)
{
std::cout<<"K nearest neighbor search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<") with K = "<<K<<std::endl;
for(size_t i= 0;i < pointIdxNKNSearch.size();++i)
std::cout<<" "<< pCloud_in->points[pointIdxNKNSearch[i]].x<<" "<<pCloud_in->points[pointIdxNKNSearch[i]].y<<" "<<pCloud_in->points[pointIdxNKNSearch[i]].z<<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
}
//将k近邻搜索结果保存至result2
pcl::PointCloud<pcl::PointXYZ>::Ptr result2(new pcl::PointCloud<pcl::PointXYZ>);
result2->width = 100000;
result2->height = 1;
result2->is_dense = false;
result2->points.resize(result2->height*result2->width);
for(size_t i =0;i<pointIdxNKNSearch.size();i++)
{
result2->points[i].x = pCloud_in->points[pointIdxNKNSearch[i]].x;
result2->points[i].y = pCloud_in->points[pointIdxNKNSearch[i]].y;
result2->points[i].z = pCloud_in->points[pointIdxNKNSearch[i]].z;
}
//K近邻搜索结果保存保存
pcl::io::savePCDFileASCII("result2.pcd",*result2);
std::cout<<pointIdxNKNSearch.size()<<" result2.pcd saved "<<std::endl;
//半径内近邻搜索
std::vector<int > pointIdxRadiusSearch;
std::vector<float > pointRadiusSquaredDistance;
//float radius = 256.f*rand() / (RAND_MAX + 1.0f);
float radius = R;
std::cout<<"Neighbor within radius search at ( "<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<" ) with radius = "<<radius<<endl;
if(octree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointRadiusSquaredDistance) > 0 )
{
std::cout<<pointIdxRadiusSearch.size()<<"points found"<<std::endl;
for(size_t i=0;i<pointIdxRadiusSearch.size();i++)
{
std::cout<<" "<<"["<<i<<"] "<<pCloud_in->points[pointIdxRadiusSearch[i]].x<<", "
<<pCloud_in->points[pointIdxRadiusSearch[i]].y<<", "
<<pCloud_in->points[pointIdxRadiusSearch[i]].z<<std::endl;
std::cout<<"(squared distance: "<<pointRadiusSquaredDistance[i]<<" )"<<std::endl;
}
std::cout<<pointIdxRadiusSearch.size()<<std::endl;
}
//将半径内近邻搜索结果放到result3
pcl::PointCloud<pcl::PointXYZ>::Ptr result3(new pcl::PointCloud<pcl::PointXYZ>);
result3->width = 500000;
result3->height = 1;
result3->is_dense = false;
result3->points.resize(result3->height*result3->width);
for(size_t i = 0;i < pointIdxRadiusSearch.size();i++)
{
result3->points[i].x = pCloud_in->points[pointIdxRadiusSearch[i]].x;
result3->points[i].y = pCloud_in->points[pointIdxRadiusSearch[i]].y;
result3->points[i].z = pCloud_in->points[pointIdxRadiusSearch[i]].z;
}
//将半径内近邻搜索结果保存到result3.pcd文件
pcl::io::savePCDFileASCII("result3.pcd",*result3);
//输出内容
std::cout<<"搜索中心: ("<<searchPoint.x<<", "<<searchPoint.y<<", "<<searchPoint.z<<" )"<<std::endl;
std::cout<<pCloud_in->points.size()<<" scan_room1.pcd(原输入点云)"<<std::endl;
std::cout<<pointIdxVec.size()<<" result1.pcd saved(体素内近邻搜索) "<<std::endl;
std::cout<<pointIdxNKNSearch.size()<<" result2.pcd saved(K=100000近邻搜索) "<<std::endl;
std::cout<<pointIdxRadiusSearch.size()<<" result3.pcd saved(radius="<< R << "半径内近邻搜索)"<<std::endl;
//声明可视化对象
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->initCameraParameters();
//输入点云的窗口
int v(0);
viewer->createViewPort(0.0,0.0,0.25,1,v);
viewer->setBackgroundColor(255,0,255,v);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(pCloud_in,0,255,0);
viewer->addPointCloud<pcl::PointXYZ>(pCloud_in,single_color,"pCloud_in",v);
viewer->addText("room_scan1",8,8,"room_scan1",v);
//体素内近邻搜索结果的窗口
int v1(0);
viewer->createViewPort(0.25,0.0,0.5,1,v1);
viewer->setBackgroundColor(0,0,0,v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(result1,255,255,255);
viewer->addPointCloud<pcl::PointXYZ>(result1,single_color1,"result1",v1);
viewer->addText("Voxel search",8,8,"Voxel search",v1);
//K近邻搜索结果的窗口
int v2(0);
viewer->createViewPort(0.5,0.0,0.75,1,v2);
viewer->setBackgroundColor(255,0,255,v2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(result2,0,255,0);
viewer->addPointCloud<pcl::PointXYZ>(result2,single_color2,"result2",v2);
viewer->addText("K=100000 search",8,8,"K=100000 search",v2);//标签以第一个字符串为准
int v3(0);
viewer->createViewPort(0.75,0.0,1,1,v3);
viewer->setBackgroundColor(0,0,0,v3);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(result3,255,255,255);
viewer->addPointCloud<pcl::PointXYZ>(result3,single_color3,"result3",v3);
viewer->addText("radius=5 search",8,8,"radius=5 search",v3);
viewer->addCoordinateSystem(0.5);//添加坐标系(大小)
viewer->spin();
return 0;
}
可视化: 依次为:体素近邻搜索、K近邻搜索(按照八叉树的分类方式最近的K个点)、半径内近邻(指定一个中心点和一个半径)搜索