Octree in PCL

  building spatial index has been widely used in point cloud data processing. The common spatial index is generally various spatial index structures that divide space from self-determined to lower levels. The more representative ones include BSP tree, KD tree, KDB tree, R tree, R + tree, CELL tree, quadtree and octree, while KD tree and octree are widely used in 3D point cloud data. PCL implements the data structure establishment and index method of octree, so as to facilitate other point cloud data processing operations on the basis of PCL.

1, Octree

  1. What is an octree

  Octree is a tree data structure used to describe three-dimensional space. Each node of the Octree represents a cube volume element (voxel lattice for short). Each node has eight nodes. The voxel lattice represented by these eight nodes together is equal to the volume of the parent node. The general center point is used as the bifurcation center of the node. As shown in the figure below:

  2. Calculation principle

a) Set the maximum recursion depth;
b) Find out the maximum size of the scene and build the first cube with this size;
c) Sequentially throw the unit element into a cube that can contain no child nodes;
d) If the maximum recursion depth is not reached, it is subdivided into eight equal parts, and then all the unit elements contained in the cube are shared to the eight sub cubes;
e) If it is found that the number of unit element elements allocated to the subcube is not zero and is the same as the parent node cube, the subcube will stop subdivision, because according to the space segmentation theory, the allocation of subdivided space must be less. If the number is the same, how to cut the number is still the same, which will lead to infinite cutting.
f) Repeat step c until the maximum recursion depth is reached.

  3. Data structure

  the nodes of octree are divided into three categories:
   gray node: its corresponding cube part is occupied by the target (non leaf node)
   white node: its corresponding cube is not occupied by the target (leaf node)
   black node: its corresponding cube is completely occupied by the target (leaf node)
   for non leaf nodes, the data structure is partitioned by the octet method and divided into smaller sub blocks. Each block has node capacity. When the node reaches the maximum capacity, the node stops splitting.

  3. Storage structure

The storage structure of octree is generally divided into three types: regular octree, linear octree, and one to eight.
Regular octree: there are two fields and eight sub leaf nodes. One represents that the node is gray / White / black. This representation is considered in terms of storage space and is generally not used.
Linear octree: the octree is transformed into a linear table and represented in a memory compact manner.

One to eight formula: using pointer representation, the advantage is that the order can be random.

2, Octree in PCL

  1. Introduction

  Octree structure is based on the subdivision of geometric entities in three-dimensional space. Each voxel has the same time and space complexity. The size is 2 n ∗ 2 n ∗ 2 n 2n * 2n *2n The geometric objects in the three-dimensional space of 2n * 2n * 2n are divided to form a pattern with root nodes. In the octree structure, if the divided voxel has the same attributes, the voxel constitutes a leaf node. Otherwise, continue to divide the body yard into 8 sub cubes and divide them recursively in turn 2 n ∗ 2 n ∗ 2 n 2n * 2n *2n A spatial object of 2n * 2n * 2n size can be divided up to N times.
  the octree module in PCL uses more than a dozen classes to realize the efficient management and retrieval of point cloud using octree data structure, as well as some corresponding spatial processing algorithms, such as compression, spatial change detection, etc; Octree relies on pcl_common module.

  2. Case code

//
// Created by aiden on 21-11-24.
//

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

#include <iostream>
#include <vector>
#include <ctime>
#include <iomanip>

void octree_serch();
void octree_change_detection();
void print_point(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &idxs, std::vector<float> &dis);

int main(int argc, char **argv){
//    octree_serch();
    octree_change_detection();
    return 0;
}

void print_point(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &idxs, std::vector<float> &dis){
    for (size_t i = 0; i < dis.size(); ++i){
        std::cout << "  point " << i + 1 << ", x:" << cloud->points[idxs[i] ].x
                  << ", y:" << cloud->points[idxs[i] ].y
                  << ", z:" << cloud->points[idxs[i] ].z
                  << ", distance:" << dis.at(i)
                  << std::endl;
    }
}

void octree_serch(){
    srand((unsigned int)time(nullptr));
    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.f * rand() / (RAND_MAX + 1.f);
        cloud->points[i].y = 1024.f * rand() / (RAND_MAX + 1.f);
        cloud->points[i].z = 1024.f * rand() / (RAND_MAX + 1.f);
    }
    float resolution = 128.f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    pcl::PointXYZ search_point;
    search_point.x = 1024.f * rand() / (RAND_MAX + 1.f);
    search_point.y = 1024.f * rand() / (RAND_MAX + 1.f);
    search_point.z = 1024.f * rand() / (RAND_MAX + 1.f);
    std::cout << "Nearest neighbor search at ( x:" << search_point.x << ", y:"
              << search_point.y << ", z:" << search_point.z << ")" << std::endl;

    std::cout << "voxelSearch: " << std::endl;
    std::vector<int> point_idx_vec;
    if (octree.voxelSearch(search_point, point_idx_vec)){
    //if (octree.voxelSearch(cloud->points[0], point_idx_vec)){
        std::vector<float> v = {0.f};
        print_point(cloud, point_idx_vec, v);
    }

    std::cout << "nearestKSearch: " << std::endl;
    int k = 10;
    std::vector<int> point_idx_nkn_search;
    std::vector<float> point_idx_nkn_distance;
    if (0 < octree.nearestKSearch(search_point, k, point_idx_nkn_search, point_idx_nkn_distance)){
        print_point(cloud, point_idx_nkn_search, point_idx_nkn_distance);
    }

    std::cout << "radiusSearch: " << std::endl;
    std::vector<int> point_idx_radius_search;
    std::vector<float> point_idx_radius_distance;
    float radius = 256.f * rand() / (RAND_MAX + 1.f);
    if (0 < octree.radiusSearch(search_point, radius, point_idx_radius_search, point_idx_radius_distance)){
        print_point(cloud, point_idx_radius_search, point_idx_radius_distance);
    }
}

void octree_change_detection(){
    srand((unsigned int)time(nullptr));

    float resolution = 32.f;
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    cloud->width = 128;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i){
        cloud->points[i].x = 64.f * rand()/(RAND_MAX + 1.f);
        cloud->points[i].y = 64.f * rand()/(RAND_MAX + 1.f);
        cloud->points[i].z = 64.f * rand()/(RAND_MAX + 1.f);
    }

    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    octree.switchBuffers();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_2->width = 128;
    cloud_2->height = 1;
    cloud_2->points.resize(cloud_2->width * cloud_2->height);
    for (size_t i = 0; i < cloud_2->points.size(); ++i){
        cloud_2->points[i].x = 64.f * rand()/(RAND_MAX + 1.f);
        cloud_2->points[i].y = 64.f * rand()/(RAND_MAX + 1.f);
        cloud_2->points[i].z = 64.f * rand()/(RAND_MAX + 1.f);
    }

    octree.setInputCloud(cloud_2);
    octree.addPointsFromInputCloud();
    std::vector<int> new_point_idx_v;
    octree.getPointIndicesFromNewVoxels(new_point_idx_v);

    std::cout << "Output from getPointIndicesFromNewVoxels: " << std::endl;
    std::vector<float> tmp(new_point_idx_v.size());
    print_point(cloud_2, new_point_idx_v, tmp);
}

Tags: PCL

Posted on Thu, 25 Nov 2021 17:14:50 -0500 by mrgrinch12