Spatial Partitioning and Search Operations with Octrees

This tutorial performs a “Neighbors within Radius” search.

This tutorial shows how to use these optimizations inside a Docker* image. For the same functionality outside of Docker* images, see PCL Optimizations Outside of Docker* Images.

1. Prepare the environment:

   cd <edge_insights_for_amr_path>/Edge_Insights_for_Autonomous_Mobile_Robots_<version>/AMR_containers
   ./run_interactive_docker.sh eiforamr-full-flavour-sdk:2023.1 root -c full_flavor
   mkdir octree && cd octree
   

2. Create the file oneapi_octree_search.cpp:

   vim oneapi_octree_search.cpp
   

3. Place the following inside the file:

   #include <iostream>
   #include <fstream>
   #include <numeric>
   #include <pcl/oneapi/octree/octree.hpp>
   #include <pcl/oneapi/containers/device_array.h>
   #include <pcl/point_cloud.h>
   
   using namespace pcl::oneapi;
   
   float dist(Octree::PointType p, Octree::PointType q) {
       return std::sqrt((p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y) + (p.z-q.z)*(p.z-q.z));
   }
   
   int main (int argc, char** argv)
   {
       std::size_t data_size = 871000;
       std::size_t query_size = 10000;
       float cube_size = 1024.f;
       float max_radius    = cube_size / 30.f;
       float shared_radius = cube_size / 30.f;
       const int max_answers = 5;
       const int k = 5;
       std::size_t i;
       std::vector<Octree::PointType> points;
       std::vector<Octree::PointType> queries;
       std::vector<float> radiuses;
       std::vector<int> indices;
   
       //Generate point cloud data, queries, radiuses, indices
       srand (0);
       points.resize(data_size);
       for(i = 0; i < data_size; ++i)
       {
           points[i].x = ((float)rand())/(float)RAND_MAX * cube_size;
           points[i].y = ((float)rand())/(float)RAND_MAX * cube_size;
           points[i].z = ((float)rand())/(float)RAND_MAX * cube_size;
       }
   
       queries.resize(query_size);
       radiuses.resize(query_size);
       for (i = 0; i < query_size; ++i)
       {
           queries[i].x = ((float)rand())/(float)RAND_MAX * cube_size;
           queries[i].y = ((float)rand())/(float)RAND_MAX * cube_size;
           queries[i].z = ((float)rand())/(float)RAND_MAX * cube_size;
           radiuses[i]  = ((float)rand())/(float)RAND_MAX * max_radius;
       };
       
       indices.resize(query_size / 2);
       for(i = 0; i < query_size / 2; ++i)
       {
           indices[i] = i * 2;
       }
       
       //Prepare oneAPI cloud
       pcl::oneapi::Octree::PointCloud cloud_device;
       cloud_device.upload(points);
   
       //oneAPI build 
       pcl::oneapi::Octree octree_device;                
       octree_device.setCloud(cloud_device);	    
       octree_device.build();
   
       //Upload queries and radiuses
       pcl::oneapi::Octree::Queries queries_device;
       pcl::oneapi::Octree::Radiuses radiuses_device;
       queries_device.upload(queries);                
       radiuses_device.upload(radiuses);
       
       //Prepare output buffers on device
       pcl::oneapi::NeighborIndices result_device1(queries_device.size(), max_answers);
       pcl::oneapi::NeighborIndices result_device2(queries_device.size(), max_answers);
       pcl::oneapi::NeighborIndices result_device3(indices.size(), max_answers);
       pcl::oneapi::NeighborIndices result_device_ann(queries_device.size(), 1);
       pcl::oneapi::Octree::ResultSqrDists dists_device_ann;
       pcl::oneapi::NeighborIndices result_device_knn(queries_device.size(), k);
       pcl::oneapi::Octree::ResultSqrDists dists_device_knn;
   
       //oneAPI octree radius search with shared radius
       octree_device.radiusSearch(queries_device, shared_radius, max_answers, result_device1);
   
       //oneAPI octree radius search with individual radius
       octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device2);
   
       //oneAPI octree radius search with shared radius using indices to specify 
       //the queries.
       pcl::oneapi::Octree::Indices cloud_indices;
       cloud_indices.upload(indices);
       octree_device.radiusSearch(queries_device, cloud_indices, shared_radius, max_answers, result_device3);
   
       //oneAPI octree ANN search
       //if neighbor points distances results are not required, can just call
       //octree_device.approxNearestSearch(queries_device, result_device_ann)
       octree_device.approxNearestSearch(queries_device, result_device_ann, dists_device_ann);
   
       //oneAPI octree KNN search
       //if neighbor points distances results are not required, can just call
       //octree_device.nearestKSearchBatch(queries_device, k, result_device_knn)
       octree_device.nearestKSearchBatch(queries_device, k, result_device_knn, dists_device_knn);
   
       //Download results
       std::vector<int> sizes1;
       std::vector<int> sizes2;
       std::vector<int> sizes3;
       result_device1.sizes.download(sizes1);
       result_device2.sizes.download(sizes2);
       result_device3.sizes.download(sizes3);
   
       std::vector<int> downloaded_buffer1, downloaded_buffer2, downloaded_buffer3, results_batch;
       result_device1.data.download(downloaded_buffer1);
       result_device2.data.download(downloaded_buffer2);
       result_device3.data.download(downloaded_buffer3);
       
       int query_idx = 2;
       std::cout << "Neighbors within shared radius search at (" 
                 << queries[query_idx].x << " " 
                 << queries[query_idx].y << " " 
                 << queries[query_idx].z << ") with radius=" << shared_radius << std::endl; 
       for (i = 0; i < sizes1[query_idx]; ++i)
       {
           std::cout << "    "  << points[downloaded_buffer1[max_answers * query_idx + i]].x 
                     << " "     << points[downloaded_buffer1[max_answers * query_idx + i]].y 
                     << " "     << points[downloaded_buffer1[max_answers * query_idx + i]].z 
                     << " (distance: " << dist(points[downloaded_buffer1[max_answers * query_idx + i]], queries[query_idx])  << ")" << std::endl;
       }
       
       std::cout << "Neighbors within individual radius search at (" 
                 << queries[query_idx].x << " " 
                 << queries[query_idx].y << " " 
                 << queries[query_idx].z << ") with radius=" << radiuses[query_idx] << std::endl; 
       for (i = 0; i < sizes2[query_idx]; ++i)
       {
           std::cout << "    "  << points[downloaded_buffer2[max_answers * query_idx + i]].x 
                     << " "     << points[downloaded_buffer2[max_answers * query_idx + i]].y 
                     << " "     << points[downloaded_buffer2[max_answers * query_idx + i]].z 
                     << " (distance: " << dist(points[downloaded_buffer2[max_answers * query_idx + i]], queries[query_idx])  << ")" << std::endl;
       }
       
       std::cout << "Neighbors within indices radius search at (" 
                 << queries[query_idx].x << " " 
                 << queries[query_idx].y << " " 
                 << queries[query_idx].z << ") with radius=" << shared_radius << std::endl;    
       for (i = 0; i < sizes3[query_idx/2]; ++i)
       {
           std::cout << "    "  << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].x 
                     << " "     << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].y 
                     << " "     << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].z 
                     << " (distance: " << dist(points[downloaded_buffer3[max_answers * query_idx / 2 + i]], queries[2])  << ")" << std::endl;
       }
   
       std::cout << "Approximate nearest neighbor at ("
                 << queries[query_idx].x << " "
                 << queries[query_idx].y << " "
                 << queries[query_idx].z << ")" << std::endl;
       std::cout << "    "  << points[result_device_ann.data[query_idx]].x
                 << " "     << points[result_device_ann.data[query_idx]].y
                 << " "     << points[result_device_ann.data[query_idx]].z
                 << " (distance: " << std::sqrt(dists_device_ann[query_idx])  << ")" << std::endl;
   
       std::cout << "K-nearest neighbors (k = " << k << ") at ("
                 << queries[query_idx].x << " "
                 << queries[query_idx].y << " "
                 << queries[query_idx].z << ")" << std::endl;
       for (i = query_idx * k; i < (query_idx + 1) * k; ++i)
       {
           std::cout << "    "  << points[result_device_knn.data[i]].x
                     << " "     << points[result_device_knn.data[i]].y
                     << " "     << points[result_device_knn.data[i]].z
                     << " (distance: " << std::sqrt(dists_device_knn[i])  << ")" << std::endl;
       }
   }
   

4. Create a CMakeLists.txt file:

   vim CMakeLists.txt
   

5. Place the following inside the file:

   cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
   set(target oneapi_octree_search)
   set(CMAKE_CXX_COMPILER dpcpp)
   set(CMAKE_CXX_STANDARD 17)
   set(CMAKE_CXX_FLAGS "-Wall -Wpedantic -Wno-unknown-pragmas -Wno-pass-failed -Wno-unneeded-internal-declaration -Wno-unused-function -Wno-gnu-anonymous-struct -Wno-nested-anon-types -Wno-extra-semi -Wno-unused-local-typedef -fsycl -fsycl-unnamed-lambda -ferror-limit=1")
   project(${target})
   
   find_package(PCL 1.12 REQUIRED)
   find_package(PCL-ONEAPI 1.12 REQUIRED)
   
   include_directories(${PCL_INCLUDE_DIRS} ${PCL-ONEAPI_INCLUDE_DIRS})
   link_directories(${PCL_LIBRARY_DIRS} ${PCL-ONEAPI_LIBRARY_DIRS})
   add_definitions(${PCL_DEFINITIONS} ${PCL-ONEAPI_DEFINITIONS})
   
   add_executable (${target} oneapi_octree_search.cpp)
   target_link_libraries (${target} sycl pcl_oneapi_containers pcl_oneapi_octree pcl_octree)
   

6. Source the Intel® oneAPI Base Toolkit environment:

   export PATH=/home/eiforamr/workspace/lib/pcl/share/pcl-1.12:/home/eiforamr/workspace/lib/pcl/share/pcl-oneapi-1.12:$PATH
   source /opt/intel/oneapi/setvars.sh
   

7. Build the code:

   cd /home/eiforamr/workspace/octree/
   mkdir build && cd build
   cmake ../
   make -j
   

8. Run the binary:

   ./oneapi_octree_search
   

Expected results example:

Neighbors within shared radius search at (671.675 733.78 466.178) with radius=34.1333

660.296 725.957 439.677 (distance: 29.8829) 665.768 721.884 442.919 (distance: 26.7846) 683.988 714.608 445.164 (distance: 30.9962) 677.927 725.08 446.531 (distance: 22.3788) 695.066 723.509 445.762 (distance: 32.7028)

Neighbors within individual radius search at (671.675 733.78 466.178) with radius=19.3623

672.71 736.679 447.835 (distance: 18.6) 664.46 731.504 452.074 (distance: 16.0048) 671.238 725.881 461.408 (distance: 9.23819) 667.707 718.527 466.622 (distance: 15.7669) 654.552 733.636 467.795 (distance: 17.1993)

Neighbors within indices radius search at (671.675 733.78 466.178) with radius=34.1333

660.296 725.957 439.677 (distance: 29.8829) 665.768 721.884 442.919 (distance: 26.7846) 683.988 714.608 445.164 (distance: 30.9962) 677.927 725.08 446.531 (distance: 22.3788) 695.066 723.509 445.762 (distance: 32.7028)

The search only finds the first five neighbors (as specified by max_answers), so a different radius finds different points.

Code Explanation

Generate point cloud data, queries, radiuses, indices with a random number.

    //Generate point cloud data, queries, radiuses, indices
    srand (0);
    points.resize(data_size);
    for(i = 0; i < data_size; ++i)
    {
        points[i].x = ((float)rand())/(float)RAND_MAX * cube_size;
        points[i].y = ((float)rand())/(float)RAND_MAX * cube_size;
        points[i].z = ((float)rand())/(float)RAND_MAX * cube_size;
    }

    queries.resize(query_size);
    radiuses.resize(query_size);
    for (i = 0; i < query_size; ++i)
    {
        queries[i].x = ((float)rand())/(float)RAND_MAX * cube_size;
        queries[i].y = ((float)rand())/(float)RAND_MAX * cube_size;
        queries[i].z = ((float)rand())/(float)RAND_MAX * cube_size;
        radiuses[i]  = ((float)rand())/(float)RAND_MAX * max_radius;
    };
    
    indices.resize(query_size / 2);
    for(i = 0; i < query_size / 2; ++i)
    {
        indices[i] = i * 2;

Create and build the Intel® oneAPI Base Toolkit point cloud; then upload the queries and radiuses to a Intel® oneAPI Base Toolkit device.

    
    //Prepare oneAPI cloud
    pcl::oneapi::Octree::PointCloud cloud_device;
    cloud_device.upload(points);

    //oneAPI build 
    pcl::oneapi::Octree octree_device;                
    octree_device.setCloud(cloud_device);	    
    octree_device.build();

    //Upload queries and radiuses
    pcl::oneapi::Octree::Queries queries_device;
    pcl::oneapi::Octree::Radiuses radiuses_device;
    queries_device.upload(queries);                

Create output buffers where we can download output from the Intel® oneAPI Base Toolkit device.

    
    //Prepare output buffers on device
    pcl::oneapi::NeighborIndices result_device1(queries_device.size(), max_answers);
    pcl::oneapi::NeighborIndices result_device2(queries_device.size(), max_answers);
    pcl::oneapi::NeighborIndices result_device3(indices.size(), max_answers);
    pcl::oneapi::NeighborIndices result_device_ann(queries_device.size(), 1);
    pcl::oneapi::Octree::ResultSqrDists dists_device_ann;
    pcl::oneapi::NeighborIndices result_device_knn(queries_device.size(), k);
    pcl::oneapi::Octree::ResultSqrDists dists_device_knn;

The fist radius search method is “search with shared radius”. In this search method, all queries use the same radius to find the neighbors.

    //oneAPI octree radius search with shared radius
    octree_device.radiusSearch(queries_device, shared_radius, max_answers, result_device1);

The second radius search method is “search with individual radius”. In this search method, each query uses its own specific radius to find the neighbors.

    //oneAPI octree radius search with individual radius
    octree_device.radiusSearch(queries_device, radiuses_device, max_answers, result_device2);

The third radius search method is “search with shared radius using indices”. In this search method, all queries use the same radius, and indices specify the queries.

    //oneAPI octree radius search with shared radius using indices to specify 
    //the queries.
    pcl::oneapi::Octree::Indices cloud_indices;
    cloud_indices.upload(indices);
    octree_device.radiusSearch(queries_device, cloud_indices, shared_radius, max_answers, result_device3);

Download the search results from the Intel® oneAPI Base Toolkit device. The size vector contains the size of found neighbors for each query. The downloaded_buffer vector contains the index of all found neighbors for each query.

    //Download results
    std::vector<int> sizes1;
    std::vector<int> sizes2;
    std::vector<int> sizes3;
    result_device1.sizes.download(sizes1);
    result_device2.sizes.download(sizes2);
    result_device3.sizes.download(sizes3);

    std::vector<int> downloaded_buffer1, downloaded_buffer2, downloaded_buffer3, results_batch;
    result_device1.data.download(downloaded_buffer1);
    result_device2.data.download(downloaded_buffer2);
    result_device3.data.download(downloaded_buffer3);

Print the query, radius, and found neighbors to verify that the result is correct.

    int query_idx = 2;
    std::cout << "Neighbors within shared radius search at (" 
              << queries[query_idx].x << " " 
              << queries[query_idx].y << " " 
              << queries[query_idx].z << ") with radius=" << shared_radius << std::endl; 
    for (i = 0; i < sizes1[query_idx]; ++i)
    {
        std::cout << "    "  << points[downloaded_buffer1[max_answers * query_idx + i]].x 
                  << " "     << points[downloaded_buffer1[max_answers * query_idx + i]].y 
                  << " "     << points[downloaded_buffer1[max_answers * query_idx + i]].z 
                  << " (distance: " << dist(points[downloaded_buffer1[max_answers * query_idx + i]], queries[query_idx])  << ")" << std::endl;
    }
    
    std::cout << "Neighbors within individual radius search at (" 
              << queries[query_idx].x << " " 
              << queries[query_idx].y << " " 
              << queries[query_idx].z << ") with radius=" << radiuses[query_idx] << std::endl; 
    for (i = 0; i < sizes2[query_idx]; ++i)
    {
        std::cout << "    "  << points[downloaded_buffer2[max_answers * query_idx + i]].x 
                  << " "     << points[downloaded_buffer2[max_answers * query_idx + i]].y 
                  << " "     << points[downloaded_buffer2[max_answers * query_idx + i]].z 
                  << " (distance: " << dist(points[downloaded_buffer2[max_answers * query_idx + i]], queries[query_idx])  << ")" << std::endl;
    }
    
    std::cout << "Neighbors within indices radius search at (" 
              << queries[query_idx].x << " " 
              << queries[query_idx].y << " " 
              << queries[query_idx].z << ") with radius=" << shared_radius << std::endl;    
    for (i = 0; i < sizes3[query_idx/2]; ++i)
    {
        std::cout << "    "  << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].x 
                  << " "     << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].y 
                  << " "     << points[downloaded_buffer3[max_answers * query_idx / 2 + i]].z 
                  << " (distance: " << dist(points[downloaded_buffer3[max_answers * query_idx / 2 + i]], queries[2])  << ")" << std::endl;