octree

#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/octree/octree_search.h>  //octree相关定义
#include <pcl/visualization/cloud_viewer.h>  //vtk可视化相关定义
#include <pcl/point_types.h>  //点类型相关定义

#include <iostream>
#include <vector>
//
//int main()
//{
//	//1.读取点云
//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("car.pcd", *cloud) == -1)
//	{
//		PCL_ERROR("Cloudn't read file!");
//		return -1;
//	}
//
//	//2.原始点云着色
//	for (size_t i = 0; i < cloud->points.size(); ++i){
//		cloud->points[i].r = 255;
//		cloud->points[i].g = 255;
//		cloud->points[i].b = 255;
//	}
//
//	//3.创建Octree实例对象
//	float resolution = 0.03f;  //设置octree体素分辨率
//	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution); //建立octree对象
//	octree.setInputCloud(cloud); //传入需要建立kdtree的点云指针
//	octree.addPointsFromInputCloud();  //构建Octree
//
//	//3.1.体素内近邻搜索
//	pcl::PointXYZRGB searchPoint1 = cloud->points[1250]; //设置查找点
//	std::vector<int> pointIdxVec;  //保存体素近邻搜索的结果向量
//	if (octree.voxelSearch(searchPoint1, pointIdxVec))
//	{
//		std::cout << "Neighbors within voxel search at (" << searchPoint1.x
//			<< " " << searchPoint1.y
//			<< " " << searchPoint1.z << ")"
//			<< std::endl;
//
//		//给查找到的近邻点设置颜色
//		for (size_t i = 0; i < pointIdxVec.size(); ++i){
//			cloud->points[pointIdxVec[i]].r = 255;
//			cloud->points[pointIdxVec[i]].g = 0;
//			cloud->points[pointIdxVec[i]].b = 0;
//		}
//	}
//
//	//3.2.K近邻搜索
//	pcl::PointXYZRGB searchPoint2 = cloud->points[3000]; //设置查找点
//	int K = 200;
//	std::vector<int> pointIdxNKNSearch; //保存K近邻点的索引结果
//	std::vector<float> pointNKNSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
//
//	std::cout << "K nearest neighbor search at (" << searchPoint2.x
//		<< " " << searchPoint2.y
//		<< " " << searchPoint2.z
//		<< ") with K=" << K << std::endl;
//
//	if (octree.nearestKSearch(searchPoint2, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
//	{   //给查找到的近邻点设置颜色
//		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){
//			cloud->points[pointIdxNKNSearch[i]].r = 0;
//			cloud->points[pointIdxNKNSearch[i]].g = 255;
//			cloud->points[pointIdxNKNSearch[i]].b = 0;
//		}
//	}
//	std::cout << "K = 200近邻点个数：" << pointIdxNKNSearch.size() << endl;
//
//	//3.3.半径内近邻搜索
//	pcl::PointXYZRGB searchPoint3 = cloud->points[6500]; //设置查找点
//	std::vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
//	std::vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
//	float radius = 0.02; //设置查找半径范围
//
//	std::cout << "Neighbors within radius search at (" << searchPoint3.x
//		<< " " << searchPoint3.y
//		<< " " << searchPoint3.z
//		<< ") with radius=" << radius << std::endl;
//
//	if (octree.radiusSearch(searchPoint3, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
//	{    //给查找到的近邻点设置颜色
//		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i){
//			cloud->points[pointIdxRadiusSearch[i]].r = 0;
//			cloud->points[pointIdxRadiusSearch[i]].g = 0;
//			cloud->points[pointIdxRadiusSearch[i]].b = 255;
//		}
//	}
//	std::cout << "半径0.02近邻点个数： " << pointIdxRadiusSearch.size() << endl;
//
//	//4.显示点云
//	pcl::visualization::CloudViewer viewer("cloud viewer");
//	viewer.showCloud(cloud);
//
//	system("pause");
//	return 0;
//}
int
main(int argc, char** argv)
{
	if (argc<5)
	{
		std::cout << "Usage: " << "<Resolution> " << "<SearchPoint.x>" << "<SearchPoint.y>" << "<SearchPoint.z>" << std::endl;
		return -2;
	}
	else
		cout << "Resolution : " << argv[1] << endl;
	 构造一个1m X 1m X 1m立方体点云，每个点之间间隔1cm
	//pcl::PointCloud<pcl::PointXYZ>::Ptr pCubeCloud(new pcl::PointCloud<pcl::PointXYZ>);
	//for (int x = 0; x < 100; x++)
	//{
	//  for (int y = 0; y < 100; y++)
	//  {
	//      for (int z = 0; z < 100; z++)
	//      {
	//          // 注意这里是pCubeCloud->push_back，而不是pCubeCloud->points->push_back
	//          pCubeCloud->push_back(pcl::PointXYZ((float)x/100.0f, (float)y/100.0f, (float)z/100.0f));
	//      }
	//  }
	//}
	 save
	//pcl::io::savePCDFile("cube.pcd", *pCubeCloud, true);

	// load
	pcl::PointCloud<pcl::PointXYZ>::Ptr pCube(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile("cube.pcd", *pCube) == -1)
		return -1;

	// 初始化八叉树
	float resolution = atof(argv[1]);                                               // 设置体素大小????
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	//octree.defineBoundingBox(0, 0, 0, 1, 1, 1);
	// 将点云载入八叉树
	octree.setInputCloud(pCube);
	octree.addPointsFromInputCloud();

	// 初始化查询点
	pcl::PointXYZ searchPoint(atof(argv[2]), atof(argv[3]), atof(argv[4]));                         // 查询点

	// 体素内搜索
	std::vector<int> pointIdxVec;
	pcl::PointCloud<pcl::RGB>::Ptr pPointsRGB(new pcl::PointCloud<pcl::RGB>);
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pCloudShow(new pcl::PointCloud<pcl::PointXYZRGBA>);


	if (octree.voxelSearch(searchPoint, pointIdxVec))
	{
		std::cout << "Neighbors within voxel search at (" << searchPoint.x
			<< " " << searchPoint.y
			<< " " << searchPoint.z << ")"
			<< std::endl;

		std::cout << "Searched Points Number : " << pointIdxVec.size() << endl;

		std::cout << "Leaf Count : " << octree.getLeafCount() << std::endl;             // 叶子数
		std::cout << "Tree Depth : " << octree.getTreeDepth() << std::endl;             // 八叉树深度
		std::cout << "Branch Count : " << octree.getBranchCount() << std::endl;         // 非叶子结点数
		std::cout << "Voxel Diameter : " << octree.getVoxelSquaredDiameter() << std::endl;  // ???Voxel Side Length*3
		std::cout << "Voxel Side Length : " << octree.getVoxelSquaredSideLen() << std::endl;// 分辨率的平方
		double minx, miny, minz, maxx, maxy, maxz;
		octree.getBoundingBox(minx, miny, minz, maxx, maxy, maxz);
		std::cout << "BoundingBox: " << "(" << minx << " - " << maxx << ")" << " , " << "(" << miny << " - " << maxy << ")" << " , " << "(" << minz << " - " << maxz << ")" << std::endl;                                // 整个八叉树的范围


	}

	// 给搜索到的点上色，原始点云中的点全为蓝色，搜索到的上为红色
	pPointsRGB->width = pCube->size();
	pPointsRGB->height = 1;
	pPointsRGB->resize(pPointsRGB->width*pPointsRGB->height);

	pCloudShow->width = pCube->size();
	pCloudShow->height = 1;
	pCloudShow->resize(pPointsRGB->width*pPointsRGB->height);

	for (size_t i = 0; i < pPointsRGB->size(); i++)
	{
		pPointsRGB->points[i].b = 255;
	}

	for (size_t i = 0; i < pointIdxVec.size(); ++i)
	{
		pPointsRGB->points[pointIdxVec[i]].b = 0;
		pPointsRGB->points[pointIdxVec[i]].r = 255;
	}

	// 合并不同字段
	pcl::concatenateFields(*pCube, *pPointsRGB, *pCloudShow);

	// 可视窗口初始化
	pcl::visualization::PCLVisualizer viewer;
	viewer.setCameraFieldOfView(0.785398);      // fov 大概45度
	viewer.setBackgroundColor(0.5, 0.5, 0.5);   // 背景设为灰色
	viewer.setCameraPosition(
		0, 0, 5,                                // camera位置
		0, 0, -1,                               // view向量--相机朝向
		0, 1, 0                                 // up向量
		);
	viewer.addPointCloud(pCloudShow, "Out");
	viewer.addCoordinateSystem(1.0, "cloud", 0);
	while (!viewer.wasStopped()) { // 显示，直到‘q’键被按下
		viewer.spinOnce();
	}

	//pcl::io::savePLYFileBinary("voxelSearchResult.ply", *pCloudShow);     // 用cloudcompare读入时rgba变为0
	system("pause");
	return 0;
}