Getting started with PCL (4): Simple use of octree

Refer to the blog "Two Structures of 3D Point Cloud Data Kdtree and Octree" and "Octree"

1. Octree

We only need to consider the case of octree in three dimensions, as follows

1. Set the maximum recursion depth;
2. Find the maximum size of the scene and create the first cube accordingly
3. If the maximum recursion depth is not reached, determine whether the current cube is completely blank or completely contained by the target. If satisfied, the cube will stop splitting; if not satisfied, the cube will be further divided into 8 sub-cubes;
4. For each subcube, return to step 3.

2. Easy to use

The code mainly refers to Shuangyu’s code

• octree_search.cpp

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>

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;
        }
}

• 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})