Attention

You are viewing an older version of the documentation. The latest version is 2.2.

KdTree search using oneapi KdTree FLANN

OneAPI KdTree is similar to pcl::KdTreeFLANN, except OneAPI KdTree can search the entire point cloud in a single call, while pcl::KdTreeFLANN performs a search for one point at a time. The OneAPI version returns a two-dimensional vector of indices and distances for the entire point cloud search or single dimensional vector of indices, distances, splits for entire point cloud search. Both nearestKSearch and radiusSearch are supported.

OneAPI includes an additional higher-performance fixed-radius search, which performs a radius search from a hash table. Unlike pcl::KdTreeFLANN’s radius search, which supports searching with different radius values once a KdTree has been built, the fixed-radius search requires specifying a radius to build the hash table. The hash table can only search for a fixed radius. The time required to build a fixed-radius hash table is much shorter than that of FLANN KdTree. Another difference is that only FLANN KdTree supports returning K elements from radius search, while fixed-radius search always returns all elements. For the same radius value, the results of both FLANN and fixed-radius search should match.

This document will not describe KdTree in detail, and please refer to original kdtree_search for more detail KdTree.

Note

This tutorial can be run both inside and outside a Docker* image. We assume that the pcl-oneapi-tutorial Deb package has been installed, and the user has copied the tutorial directory from /opt/intel/pcl/oneapi/tutorials/ to a user-writable directory.

  1. Prepare the environment:

    cd <path-to-oneapi-tutorials>/kdtree
    Copy to clipboard

  2. oneapi_kdtree.cpp should be in the directory with following content:

      1#include <pcl/oneapi/search/kdtree.h> // for KdTree
  2#include <pcl/point_cloud.h>
  3
  4#include <vector>
  5#include <iostream>
  6
  7int
  8main (int argc, char** argv)
  9{
 10  srand (time (NULL));
 11
 12  std::cout << "Running on device: " << dpct::get_default_queue().get_device().get_info<sycl::info::device::name>() << "\n";
 13
 14  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 15
 16  // Generate pointcloud data
 17  cloud->width = 1000;
 18  cloud->height = 1;
 19  cloud->points.resize (cloud->width * cloud->height);
 20
 21  for (std::size_t i = 0; i < cloud->size (); ++i)
 22  {
 23    (*cloud)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 24    (*cloud)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 25    (*cloud)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 26  }
 27
 28  pcl::PointCloud<pcl::PointXYZ>::Ptr searchPoints (new pcl::PointCloud<pcl::PointXYZ>);
 29
 30  // Generate pointcloud data
 31  searchPoints->width = 3;
 32  searchPoints->height = 1;
 33  searchPoints->points.resize (searchPoints->width * searchPoints->height);
 34
 35  for (std::size_t i = 0; i < searchPoints->size (); ++i)
 36  {
 37    (*searchPoints)[i].x = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 38    (*searchPoints)[i].y = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 39    (*searchPoints)[i].z = static_cast<float>(1024.0f * rand () / (RAND_MAX + 1.0));
 40  }
 41
 42  pcl::oneapi::search::KdTree<pcl::PointXYZ> kdtree;
 43  kdtree.setInputCloud (cloud);
 44
 45  // K nearest neighbor search
 46  int K = 5;
 47
 48  std::vector< std::vector< float > > pointsSquaredDistance (searchPoints->size()) ;
 49  std::vector< pcl::Indices > pointsIdxKnnSearch (searchPoints->size());
 50
 51  kdtree.nearestKSearch(*searchPoints, K, pointsIdxKnnSearch, pointsSquaredDistance);
 52
 53  for (std::size_t j = 0; j < pointsIdxKnnSearch.size(); ++j)
 54  {
 55    std::cout << "K=" << K << " neighbors from (" << (*searchPoints)[j].x << ","
 56                                                 << (*searchPoints)[j].y << ","
 57                                                 << (*searchPoints)[j].z << ")" << std::endl;
 58    for (std::size_t i = 0; i < pointsIdxKnnSearch.at(j).size(); ++i)
 59    {
 60      std::cout << "    "  <<   (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].x
 61                << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].y
 62                << " " <<  (*cloud)[ pointsIdxKnnSearch.at(j)[i] ].z
 63                << " (squared distance: " << pointsSquaredDistance.at(j)[i] << ")" << std::endl;
 64    }
 65  }
 66
 67  // Neighbors within radius search
 68  float radius = 100.f;
 69
 70  std::vector< std::vector< float > > pointsRadiusSquaredDistance (searchPoints->size()) ;
 71  std::vector< pcl::Indices > pointsIdxRadiusSearch (searchPoints->size());
 72
 73  kdtree.radiusSearch(*searchPoints, radius, pointsIdxRadiusSearch,  pointsRadiusSquaredDistance, 10);
 74
 75  for (std::size_t j = 0; j < pointsIdxRadiusSearch.size(); ++j)
 76  {
 77    std::cout << "Kdtree Radius Search Radius=" << radius << " neighbors from (" << (*searchPoints)[j].x << ","
 78                                                 << (*searchPoints)[j].y << ","
 79                                                 << (*searchPoints)[j].z << ")" << std::endl;
 80    for (std::size_t i = 0; i < pointsIdxRadiusSearch.at(j).size(); ++i)
 81    {
 82      std::cout << "    "  <<   (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].x
 83                << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].y
 84                << " " <<  (*cloud)[ pointsIdxRadiusSearch.at(j)[i] ].z
 85                << " (squared distance: " << pointsRadiusSquaredDistance.at(j)[i] << ")" << std::endl;
 86    }
 87  }
 88
 89  // Fixed radius search
 90  // Only support radius use to build the table
 91
 92  std::vector<int> pointsFixedRadiusIdx;
 93  std::vector<float> pointsFixedRadiusSquaredDistance;
 94  std::vector<int> pointsFixedRadiusSplit;
 95
 96  kdtree.setInputCloud(cloud, radius);
 97
 98  kdtree.fixedRadiusSearch(searchPoints, pointsFixedRadiusIdx, pointsFixedRadiusSquaredDistance,
 99      pointsFixedRadiusSplit);
100
101  for (std::size_t j = 0; j < pointsFixedRadiusSplit.size() - 1; ++j)
102  {
103    std::cout << "Fixed Radius Search Radius=" << radius << " neighbors from (" << (*searchPoints)[j].x << ","
104                                                 << (*searchPoints)[j].y << ","
105                                                 << (*searchPoints)[j].z << ")" << std::endl;
106
107    std::size_t cur_idx = pointsFixedRadiusSplit[j];
108    std::size_t elements = pointsFixedRadiusSplit[j+1] - cur_idx;
109    for (std::size_t i = 0; i < elements; ++i)
110    {
111      std::cout << "    "  <<   (*cloud)[pointsFixedRadiusIdx.at(cur_idx+i)].x
112                << " " <<  (*cloud)[ pointsFixedRadiusIdx.at(cur_idx+i)].y
113                << " " <<  (*cloud)[ pointsFixedRadiusIdx.at(cur_idx+i)].z
114                << " (squared distance: " << pointsFixedRadiusSquaredDistance.at(cur_idx+i) << ")" << std::endl;
115
116    }
117  }
118
119  return 0;
120}
    Copy to clipboard

  3. Source the Intel® oneAPI Base Toolkit environment:

    source /opt/intel/oneapi/setvars.sh
    Copy to clipboard

  4. (Optional) Setup proxy setting to download test data:

    export http_proxy="http://<http_proxy>:port"
export https_proxy="http://<https_proxy>:port"
    Copy to clipboard

  5. Build the code:

    mkdir build && cd build
cmake ../
make -j
    Copy to clipboard

  6. Run the binary:

    ./oneapi_kdtree
    Copy to clipboard

  7. Expected results example:

    K=5 neighbors from (911.933,321.699,128.681)
  870.884 350.103 160.644 (squared distance: 3513.43)
  856.565 374.562 132.514 (squared distance: 5874.84)
  960.284 267.917 173.756 (squared distance: 7262)
  987.681 359.434 104.805 (squared distance: 7731.77)
  929.787 396.96 84.1415 (squared distance: 7966.85)
K=5 neighbors from (39.0119,923.789,113.278)
  36.4024 943.423 84.1814 (squared distance: 1238.91)
  68.2891 927.112 150.885 (squared distance: 2282.5)
  79.0735 887.262 63.1519 (squared distance: 5451.72)
  119.306 1021.4 127.402 (squared distance: 16175.4)
  116.103 800.647 145.881 (squared distance: 22169.9)
K=5 neighbors from (542.853,566.777,4.23339)
  481.511 597.7 27.2575 (squared distance: 5249.24)
  570.652 585.804 69.9711 (squared distance: 5456.25)
  574.454 498.665 37.4542 (squared distance: 6741.42)
  569.49 480.913 6.70625 (squared distance: 8088.2)
  649.963 597.5 25.601 (squared distance: 12872.9)
Kdtree Radius Search Radius=100 neighbors from (911.933,321.699,128.681)
  870.884 350.103 160.644 (squared distance: 3513.43)
  856.565 374.562 132.514 (squared distance: 5874.84)
  960.284 267.917 173.756 (squared distance: 7262)
  987.681 359.434 104.805 (squared distance: 7731.77)
  929.787 396.96 84.1415 (squared distance: 7966.85)
  967.947 352.763 64.8979 (squared distance: 8170.85)
  974.103 248.606 116.676 (squared distance: 9351.69)
  842.968 389.302 109.632 (squared distance: 9689.34)
Kdtree Radius Search Radius=100 neighbors from (39.0119,923.789,113.278)
  36.4024 943.423 84.1814 (squared distance: 1238.91)
  68.2891 927.112 150.885 (squared distance: 2282.5)
  79.0735 887.262 63.1519 (squared distance: 5451.72)
Kdtree Radius Search Radius=100 neighbors from (542.853,566.777,4.23339)
  481.511 597.7 27.2575 (squared distance: 5249.24)
  570.652 585.804 69.9711 (squared distance: 5456.25)
  574.454 498.665 37.4542 (squared distance: 6741.42)
  569.49 480.913 6.70625 (squared distance: 8088.2)
Fixed Radius Search Radius=100 neighbors from (911.933,321.699,128.681)
  870.884 350.103 160.644 (squared distance: 3513.43)
  856.565 374.562 132.514 (squared distance: 5874.84)
  960.284 267.917 173.756 (squared distance: 7262)
  987.681 359.434 104.805 (squared distance: 7731.77)
  929.787 396.96 84.1415 (squared distance: 7966.85)
  967.947 352.763 64.8979 (squared distance: 8170.85)
  974.103 248.606 116.676 (squared distance: 9351.69)
  842.968 389.302 109.632 (squared distance: 9689.34)
Fixed Radius Search Radius=100 neighbors from (39.0119,923.789,113.278)
  36.4024 943.423 84.1814 (squared distance: 1238.91)
  68.2891 927.112 150.885 (squared distance: 2282.49)
  79.0735 887.262 63.1519 (squared distance: 5451.72)
Fixed Radius Search Radius=100 neighbors from (542.853,566.777,4.23339)
  481.511 597.7 27.2575 (squared distance: 5249.24)
  570.652 585.804 69.9711 (squared distance: 5456.25)
  574.454 498.665 37.4542 (squared distance: 6741.42)
  569.49 480.913 6.70625 (squared distance: 8088.2)
    Copy to clipboard

Code Explanation

Compare to original version, the OneAPI version requires to use the OneAPI KdTree header.

#include <pcl/oneapi/search/kdtree.h> // for KdTree
Copy to clipboard

The following code first seeds the ‘rand()’ function with the system time and then creates and fills a PointCloud with random data. Another set of search points with random coordinates is created as well. The objective here is to perform a search for 3 coordinates using a single call.

  srand (time (NULL));

  std::cout << "Running on device: " << dpct::get_default_queue().get_device().get_info<sycl::info::device::name>() << "\n";

  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));
  }
Copy to clipboard

This section of code initializes the KdTree object and sets the randomly created cloud as the input.

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

An integer variable is created (set to 5), along with two two-dimensional vectors designed to store the 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());

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

Assuming that the KdTree returns more than 0 closest neighbors, it proceeds to print out the locations of all 5 closest neighbors to the random searchPoints, which have been stored in the previously created vectors.

  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;
    }
  }
Copy to clipboard

The code then demonstrates the process of finding all neighbors to the given searchPoints within a specified radius. Two vectors are created to store information about these neighbors.

  // 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);
Copy to clipboard

Similar to the previous scenario, if the KdTree returns more than 0 neighbors within the specified radius, it prints out the coordinates of these points, which have been stored in the vectors.

  for (std::size_t j = 0; j < pointsIdxRadiusSearch.size(); ++j)
  {
    std::cout << "Kdtree Radius Search 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;
    }
  }
Copy to clipboard

Additionally, a fixed radius search method is provided through a hash table. As this is a fixed radius search, it is necessary to provide the radius to build the table.

  // Fixed radius search
  // Only support radius use to build the table

  std::vector<int> pointsFixedRadiusIdx;
  std::vector<float> pointsFixedRadiusSquaredDistance;
  std::vector<int> pointsFixedRadiusSplit;

  kdtree.setInputCloud(cloud, radius);
Copy to clipboard

The fixed radius search returns a one-dimensional vector containing both indices and squared distances for the neighbors. To access the respective searchPoints, the returned split vector is used. The example below demonstrates how to access each searchPoint.

  kdtree.fixedRadiusSearch(searchPoints, pointsFixedRadiusIdx, pointsFixedRadiusSquaredDistance,
      pointsFixedRadiusSplit);

  for (std::size_t j = 0; j < pointsFixedRadiusSplit.size() - 1; ++j)
  {
    std::cout << "Fixed Radius Search Radius=" << radius << " neighbors from (" << (*searchPoints)[j].x << ","
                                                 << (*searchPoints)[j].y << ","
                                                 << (*searchPoints)[j].z << ")" << std::endl;

    std::size_t cur_idx = pointsFixedRadiusSplit[j];
    std::size_t elements = pointsFixedRadiusSplit[j+1] - cur_idx;
    for (std::size_t i = 0; i < elements; ++i)
    {
      std::cout << "    "  <<   (*cloud)[pointsFixedRadiusIdx.at(cur_idx+i)].x
                << " " <<  (*cloud)[ pointsFixedRadiusIdx.at(cur_idx+i)].y
                << " " <<  (*cloud)[ pointsFixedRadiusIdx.at(cur_idx+i)].z
                << " (squared distance: " << pointsFixedRadiusSquaredDistance.at(cur_idx+i) << ")" << std::endl;

    }
  }
Copy to clipboard