PCL入门（四）：octree简单使用

参考博客《三维点云数据的两种结构Kdtree和Octree》和《八叉树》

1. 八叉树(Octree)

只需要考虑三维情况下的八叉树的情况，如下

1. 设置最大的递归深度；
2. 找出场景的最大尺寸，并据此创建第一个立方体
3. 若未到达最大递归深度，判断当前立方体是否完全空白，或者完全为目标包含，若满足，则该立方体停止分裂；若不满足，则将立方体进一步分割为8个子立方体；
4. 对于每一个子立方体，返回步骤3。

2. 简单使用

代码主要参考双愚的代码

• octree_search.cpp

#include 
#include 
#include 
#include 
#include 

int main (int argc, char** argv)
{
	srand((unsigned int)time(NULL));
	
	// 创建点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i=0; i<cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
	}
	
	// 创建octree对象
	float resolution = 128.0f; // 八叉树中最小尺寸(分辨率)
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	
	// 创建搜索点searchPoint
	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);
	std::cout << "Neighbors within voxel search at (" << searchPoint.x
                  << " " << searchPoint.y
                  << " " << searchPoint.z << ")"
                  << std::endl;

	// 任务一：给定搜索点searchPoint，输出该点所在体素内的其他点
	std::vector<int> pointIdxVec;
	if (octree.voxelSearch(searchPoint, pointIdxVec))
	{
		for (size_t i = 0; i < pointIdxVec.size(); ++i) 
		    std::cout << "    " << cloud->points[pointIdxVec[i]].x
		              << " " << cloud->points[pointIdxVec[i]].y
		              << " " << cloud->points[pointIdxVec[i]].z << std::endl;
	}
	
	// 任务二：给定搜索点searchPoint，输出离该点最近的10个点
	int K = 10;
	std::vector<int> pointIdxNKNSearch; // 10个点在点云中的index
	std::vector<float> pointNKNSquaredDistance; // 10个点
	std::cout << "K nearest neighbor search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with K=" << K << std::endl;
        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;
        }
	
	// 任务三：给定搜索点searchPoint，输出该点一定半径内的所有其他点
	std::vector<int> pointIdxRadiusSearch;
	std::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;
        }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

• CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(cloud_viewer)

find_package(PCL 1.2 REQUIRED)

add_executable(octree octree.cpp)
target_link_libraries(octree ${PCL_LIBRARIES})
1
2
3
4
5
6
7
8