KdTree Search Using Intel® oneAPI Base Toolkit's KdTree FLANN

Intel® oneAPI Base Toolkit's KdTree is similar to pcl::KdTreeFLANN except that Intel® oneAPI Base Toolkit's KdTree is able to search the entire point cloud in a single call. Intel® oneAPI Base Toolkit's KdTree returns a two-dimensional vector of indices and distances for the entire point cloud search. Both nearestKSearch and radiusSearch are supported.

See kdtree_search for details.

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 kdtree && cd kdtree
   

2. Create the file oneapi_kdtree.cpp:

   vim oneapi_kdtree.cpp
   

3. Place the following inside the file:

   #include <pcl/oneapi/search/kdtree.h> // for KdTree
   #include <pcl/point_cloud.h>
   
   #include <vector>
   #include <iostream>
   
   int
   main (int argc, char** argv)
   {
     srand (time (NULL));
   
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   
     // Generate pointcloud data
     cloud->width = 1000;
     cloud->height = 1;
     cloud->points.resize (cloud->width * cloud->height);
   
     for (std::size_t i = 0; i < cloud->size (); ++i)
     {
       (*cloud)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
       (*cloud)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
       (*cloud)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
     }
   
     pcl::PointCloud<pcl::PointXYZ>::Ptr searchPoints (new pcl::PointCloud<pcl::PointXYZ>);
   
     // Generate pointcloud data
     searchPoints->width = 3;
     searchPoints->height = 1;
     searchPoints->points.resize (searchPoints->width * searchPoints->height);
   
     for (std::size_t i = 0; i < searchPoints->size (); ++i)
     {
       (*searchPoints)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
       (*searchPoints)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
       (*searchPoints)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
     }
   
     pcl::oneapi::search::KdTree<pcl::PointXYZ> kdtree;
     kdtree.setInputCloud (cloud);
   
     // K nearest neighbor search
     int K = 5;
   
     std::vector< std::vector< float > > pointsSquaredDistance (searchPoints->size()) ;
     std::vector< pcl::Indices > pointsIdxKnnSearch (searchPoints->size());
   
     kdtree.nearestKSearch(*searchPoints, K, pointsIdxKnnSearch, pointsSquaredDistance);
   
     for (std::size_t j = 0; j < pointsIdxKnnSearch.size(); ++j)
     {
       std::cout << "K=" << K << " neighbors from (" << (*searchPoints)[j].x << ","
                                                    << (*searchPoints)[j].y << ","
                                                    << (*searchPoints)[j].z << ")" << std::endl;
       for (std::size_t i = 0; i < pointsIdxKnnSearch.at(j).size(); ++i)
       {
         std::cout << "    "  <<   (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].x
                   << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].y
                   << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].z
                   << " (squared distance: " << pointsSquaredDistance.at(j)[i] << ")" << std::endl;
       }
     }
   
     // Neighbors within radius search
     float radius = 100.f;
   
     std::vector< std::vector< float > > pointsRadiusSquaredDistance (searchPoints->size()) ;
     std::vector< pcl::Indices > pointsIdxRadiusSearch (searchPoints->size());
   
     kdtree.radiusSearch(*searchPoints, radius, pointsIdxRadiusSearch,  pointsRadiusSquaredDistance, 10);
   
     for (std::size_t j = 0; j < pointsIdxRadiusSearch.size(); ++j)
     {
       std::cout << "Radius=" << radius << " neighbors from (" << (*searchPoints)[j].x << ","
                                                    << (*searchPoints)[j].y << ","
                                                    << (*searchPoints)[j].z << ")" << std::endl;
       for (std::size_t i = 0; i < pointsIdxRadiusSearch.at(j).size(); ++i)
       {
         std::cout << "    "  <<   (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].x
                   << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].y
                   << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].z
                   << " (squared distance: " << pointsRadiusSquaredDistance.at(j)[i] << ")" << std::endl;
       }
     }
   
     return 0;
   }
   

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_kdtree)
   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_kdtree.cpp)
   target_link_libraries (${target} sycl -lpcl_oneapi_search -lpcl_search)
   

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/kdtree/
   mkdir build && cd build
   cmake ../
   make -j
   

8. Run the binary:

   ./oneapi_kdtree
   

Expected results example:

K=5 neighbors from (868.536,165.24,588.71)

895.824 185.838 555.685 (squared distance: 2259.57) 877.741 116.544 656.683 (squared distance: 7076.32) 906.9 102.777 515.255 (squared distance: 10769.1) 817.828 258.588 546.829 (squared distance: 13039.3) 767.465 164.017 644.785 (squared distance: 13361.4)

K=5 neighbors from (169.766,772.676,607.395)

219.717 774.625 586.534 (squared distance: 2934.17) 137.948 729.391 630.512 (squared distance: 3420.39) 211.662 726.615 591.354 (squared distance: 4134.23) 241.534 720.475 579.8 (squared distance: 8637.12) 236.382 811.854 548.706 (squared distance: 9417.13)

K=5 neighbors from (974.478,854.754,392.108)

1001.23 881.896 424.253 (squared distance: 2485.66) 1002.05 882.791 460.627 (squared distance: 6241.17) 980.62 864.419 471.809 (squared distance: 6483.47) 891.607 840.559 334.935 (squared distance: 10337.9) 875.04 824.699 399.918 (squared distance: 10852.3)

Radius=100 neighbors from (868.536,165.24,588.71)

895.824 185.838 555.685 (squared distance: 2259.57) 877.741 116.544 656.683 (squared distance: 7076.32)

Radius=100 neighbors from (169.766,772.676,607.395)

219.717 774.625 586.534 (squared distance: 2934.17) 137.948 729.391 630.512 (squared distance: 3420.39) 211.662 726.615 591.354 (squared distance: 4134.23) 241.534 720.475 579.8 (squared distance: 8637.12) 236.382 811.854 548.706 (squared distance: 9417.13)

Radius=100 neighbors from (974.478,854.754,392.108)

1001.23 881.896 424.253 (squared distance: 2485.66) 1002.05 882.791 460.627 (squared distance: 6241.17) 980.62 864.419 471.809 (squared distance: 6483.47)

Code Explanation

Intel® oneAPI Base Toolkit's KdTree requires this header.

#include <pcl/point_cloud.h>

Seed rand() with the system time, create and fill a point cloud with random data (cloud), create and fill another point cloud with random coordinates (searchPoints), and search three coordinates using a single call.

  srand (time (NULL));

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // Generate pointcloud data
  cloud->width = 1000;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (std::size_t i = 0; i < cloud->size (); ++i)
  {
    (*cloud)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
    (*cloud)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
    (*cloud)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr searchPoints (new pcl::PointCloud<pcl::PointXYZ>);

  // Generate pointcloud data
  searchPoints->width = 3;
  searchPoints->height = 1;
  searchPoints->points.resize (searchPoints->width * searchPoints->height);

  for (std::size_t i = 0; i < searchPoints->size (); ++i)
  {
    (*searchPoints)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
    (*searchPoints)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
    (*searchPoints)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
  }

Create the KdTree object, and set cloud as the input.

  pcl::oneapi::search::KdTree<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud (cloud);

Create an integer ‘K’ and set it equal to five, and create two two-dimensional vectors for storing our K nearest neighbors from the search.

  // K nearest neighbor search
  int K = 5;

  std::vector< std::vector< float > > pointsSquaredDistance (searchPoints->size()) ;
  std::vector< pcl::Indices > pointsIdxKnnSearch (searchPoints->size());

If KdTree returns more than 0 closest neighbors, print the locations of all K of the closest neighbors to searchPoints.

  kdtree.nearestKSearch(*searchPoints, K, pointsIdxKnnSearch, pointsSquaredDistance);

  for (std::size_t j = 0; j < pointsIdxKnnSearch.size(); ++j)
  {
    std::cout << "K=" << K << " neighbors from (" << (*searchPoints)[j].x << ","
                                                 << (*searchPoints)[j].y << ","
                                                 << (*searchPoints)[j].z << ")" << std::endl;
    for (std::size_t i = 0; i < pointsIdxKnnSearch.at(j).size(); ++i)
    {
      std::cout << "    "  <<   (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].x
                << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].y
                << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].z
                << " (squared distance: " << pointsSquaredDistance.at(j)[i] << ")" << std::endl;
    }
  }

Find all neighbors to searchPoints within a given radius, and create two two-dimensional vectors for storing the information about our neighbors.

  // Neighbors within radius search
  float radius = 100.f;

  std::vector< std::vector< float > > pointsRadiusSquaredDistance (searchPoints->size()) ;
  std::vector< pcl::Indices > pointsIdxRadiusSearch (searchPoints->size());

If KdTree returns more than 0 closest neighbors within the specified radius, print the locations of these points.

  kdtree.radiusSearch(*searchPoints, radius, pointsIdxRadiusSearch,  pointsRadiusSquaredDistance, 10);

  for (std::size_t j = 0; j < pointsIdxRadiusSearch.size(); ++j)
  {
    std::cout << "Radius=" << radius << " neighbors from (" << (*searchPoints)[j].x << ","
                                                 << (*searchPoints)[j].y << ","
                                                 << (*searchPoints)[j].z << ")" << std::endl;
    for (std::size_t i = 0; i < pointsIdxRadiusSearch.at(j).size(); ++i)
    {
      std::cout << "    "  <<   (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].x
                << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].y
                << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].z
                << " (squared distance: " << pointsRadiusSquaredDistance.at(j)[i] << ")" << std::endl;
    }
  }