八叉树体素遍历近邻体素搜索

最新推荐文章于 2024-09-05 11:39:37 发布

xinxiangwangzhi_

最新推荐文章于 2024-09-05 11:39:37 发布

阅读量2.4k

点赞数 8

分类专栏： pcl 点云文章标签：自动驾驶 c++ 算法

本文链接：https://blog.csdn.net/xinxiangwangzhi_/article/details/120069233

版权

pcl 八叉树体素相邻查找点云处理

关键词由CSDN通过智能技术生成

点云同时被 2 个专栏收录

65 篇文章 32 订阅

订阅专栏

pcl

25 篇文章 17 订阅

订阅专栏

网上基本上没有关于pcl 八叉树如何寻找相邻体素问题，这个问题也困扰了我很久。网上基本上都是查找某一体素内的近邻点云（这部分内容可以查看我的之前的八叉树博客），而非查找相邻的体素。一个偶然的机会get到了这个技能，特此分享一下。
说明：pcl中octreekey保存了该体素在xyz方向上的索引号，因此可以根据这一特点进行相邻体素查找。pcl是这样说明的：Octree keys contain integer indices for each coordinate axis in order to address an octree leaf node.
叶子节点也就是最终的体素。


//访问体素，以及查找相邻体素


#include <iostream>
#include<vector>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include<pcl/octree/octree.h>
int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	std::string file_name("part216offgroundpoints");
	pcl::io::loadPCDFile<pcl::PointXYZ>(file_name + ".pcd", *cloud);
	std::cerr << "点云数量：" << cloud->points.size() << std::endl;
	float radius = 1.0;

	std::cerr << "半径r=" << radius << std::endl ;

	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(radius);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	std::cerr << "叶子节点数量：" << octree.getLeafCount() << std::endl;
	std::cerr << "分支数量：" << octree.getBranchCount() << std::endl;
	//auto indices_leaf = octree.getIndices();

	//auto leaf = octree.findLeaf(167, 174, 60);
	//auto vec = leaf->getPointIndicesVector();

	//std::cout << leaf->getSize() << std::endl;
	auto re = octree.getResolution();

	int num = 0;
	std::vector<pcl::octree::OctreeKey>key_vec;
	std::vector<int> indices, indices_1, indices_2;
	//利用迭代器遍历所有八叉树
	for (auto iter = octree.leaf_depth_begin(); iter != octree.leaf_depth_end(); ++iter)
	{
		auto key = iter.getCurrentOctreeKey();
		key_vec.emplace_back(key);
		//根据key寻找相邻体素的迭代器，可以查找更多相邻的体素
		auto it_key = octree.findLeaf(key.x, key.y, key.z);
		auto it_key_1 = octree.findLeaf(key.x , key.y + 1, key.z );
		auto it_key_2 = octree.findLeaf(key.x, key.y - 1, key.z );
		if (it_key != nullptr&&it_key_1 != nullptr&&it_key_2 != nullptr)
		{
			indices = iter.getLeafContainer().getPointIndicesVector();
			indices_1 = it_key_1->getPointIndicesVector();
			indices_2 = it_key_2->getPointIndicesVector();
			num++;
			//方便可视化，输出第三次结果，退出循环
			if (num == 3)
				break;
		}
		auto vec_leaf = iter.getNodeID();
		std::cout << vec_leaf << "\n";

	}
	std::cout << num << std::endl;
	pcl::PointCloud<pcl::PointXYZ> box, box_1, box_2;
	for (size_t i = 0; i < indices.size(); ++i)
	{
		box.points.push_back(cloud->points[indices[i]]);
	}

	for (size_t i = 0; i < indices_1.size(); ++i)
	{
		box_1.points.push_back(cloud->points[indices_1[i]]);
	}

	for (size_t i = 0; i < indices_2.size(); ++i)
	{
		box_2.points.push_back(cloud->points[indices_2[i]]);
	}
	box.resize(indices.size());
	box_1.resize(indices_1.size());
	box_2.resize(indices_2.size());
	pcl::io::savePCDFile<pcl::PointXYZ>("box.pcd", box);
	pcl::io::savePCDFile<pcl::PointXYZ>("box_1.pcd", box_1);
	pcl::io::savePCDFile<pcl::PointXYZ>("box_2.pcd", box_2);
    std::cout << "Hello World!\n"; 
}