Octree

K-D Tree may be good for high-dimensional space; however, for 3D point clouds, Octree may provide better performance in some cases. In the following, we report some benchmark results on LiDAR point clouds.

Experiment 1: Ouster LiDAR cloud

• Target cloud size = 65536, source cloud size = 3000.

• For each point in the source cloud, find its neighbor point in the target cloud.

• The scale is roughly 10-50 meters, in a structured environment.

• 13-gen Intel evo i7 CPU (dell xps laptop), single threading.

• Both data structures are implemented in PCL.

Operation	K-D Tree	Octree (leaf size 0.01 m)	Octree (leaf size 0.001m)
Build Tree	7 ms	8 ms	12 ms
Approximate NN Search (k=1)	(not support)	94 ms	96 ms
K-NN Search (k=1)	124 ms	390 ms	408 ms
K-NN Search (k=10)	124 ms	395 ms	452 ms
K-NN Search (k=100)	134 ms	573 ms	698 ms
Radius Search (r=0.01 m)	1789 ms	126 ms	126 ms
Radius Search (r=0.1 m)	1799 ms	129 ms	130 ms
Radius Search (r=1 m)	2011 ms	415 ms	861 ms

Other observations from ablation study:

• Time consumption of tree building is linear in target cloud size.

• Time consumption of each search method is also almost linear in source/query cloud size. (i.e. size = 30000 can roughly take 10x more time than size = 3000).

• The location and shape of source query cloud does not affect much on running time.

• PCL Point Type (PointXYZ vs. PointXYZRGBNormal) does not affect much on running time.

Other notes:

• This result can be accelerated by multi-threading, e.g., OpenMP.

• The approxNearestSearch method in Octree can only find the nearest point (k=1 case), and uses tree nodes as the approximate result similar to the radius search to speed up.

Experiment 2: Velodyne LiDAR Cloud (VLP-16)

• Target cloud size = 28209 (NaN points removed), source cloud size = 3000.

• For each point in the source cloud, find its neighbor point in the target cloud.

• The scale is roughly 10-50 meters, in a structured environment.

• 13-gen Intel evo i7 CPU (dell xps laptop), single threading.

• Both data structures are implemented in PCL.

Operation	K-D Tree	Octree (leaf size 0.01 m)	Octree (leaf size 0.001m)
Build Tree	4 ms	6 ms	9 ms
Approximate NN Search (k=1)	(not support)	1.5 ms	1.5 ms
K-NN Search (k=1)	3 ms	23 ms	23 ms
K-NN Search (k=10)	5 ms	41 ms	57 ms
K-NN Search (k=100)	21 ms	407 ms	539 ms
Radius Search (r=0.01 m)	1 ms	2 ms	2 ms
Radius Search (r=0.1 m)	2 ms	4 ms	5 ms
Radius Search (r=1 m)	34 ms	59 ms	138 ms

Notes (based on new observations):

• If using method K-NN Search (k=1), Octree and K-D Tree can get almost the same neighbor points as the output result.

• If using method Approximate NN Search (k=1) from Octree, the result will be approximated and most likely different from the K-NN Search (k=1) result. This method is OK to be used in finding the neighbor points (to constitute a submap) from a global map, but it should not be used internally in an ICP algorithm in each iteration, as it can make the algorithm not to converge.

Benchmark Code

#include <iostream>
#include <chrono>
#include <iomanip>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/kdtree/kdtree_flann.h>

using PointT = pcl::PointXYZRGBNormal;
using CloudT = pcl::PointCloud<PointT>;
using CloudTPtr = pcl::PointCloud<PointT>::Ptr;

void LoadPointCloud(const std::string& filename, CloudTPtr cloud) {
  if (filename.substr(filename.find_last_of(".") + 1) == "pcd") {
    pcl::io::loadPCDFile(filename, *cloud);
  } else {
    pcl::io::loadPLYFile(filename, *cloud);
  }
}

template<typename SearchTree>
double TimeTakenForSearch(SearchTree& tree, CloudTPtr source, int k) {
  std::vector<int> pointIdxNKNSearch;
  std::vector<float> pointNKNSquaredDistance;

  auto start_time = std::chrono::high_resolution_clock::now();
  for (const auto& pt : source->points) {
    tree.nearestKSearch(pt, k, pointIdxNKNSearch, pointNKNSquaredDistance);
  }
  auto end_time = std::chrono::high_resolution_clock::now();
  return std::chrono::duration<double, std::micro>(end_time - start_time).count() / 1000.0;
}

template<typename SearchTree>
double TimeTakenForRadiusSearch(SearchTree& tree, CloudTPtr source, float radius) {
  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;

  auto start_time = std::chrono::high_resolution_clock::now();
  for (const auto& pt : source->points) {
    tree.radiusSearch(pt, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
  }
  auto end_time = std::chrono::high_resolution_clock::now();
  return std::chrono::duration<double, std::micro>(end_time - start_time).count() / 1000.0;
}

int main(int argc, char** argv) {
  if (argc != 3) {
    std::cerr << "Usage: " << argv[0] << " <source.pcd/ply> <target.pcd/ply>" << std::endl;
    return -1;
  }

  CloudTPtr source_original(new CloudT);
  CloudTPtr target(new CloudT);
  LoadPointCloud(argv[1], source_original);
  LoadPointCloud(argv[2], target);

  std::cout << "Source cloud size = " << source_original->size() << std::endl;
  std::cout << "Target cloud size = " << target->size() << std::endl;

  // Reduce source cloud size
  CloudTPtr source(new CloudT);
  int limit = std::min(3000, static_cast<int>(source_original->size()));
  for (int i = 0; i < limit; i++) {
    source->points.push_back(source_original->points[i]);
  }
  source->width = source->points.size();
  source->height = 1;
  source->is_dense = true;
  std::cout << "Reduced source cloud size = " << source->points.size() << std::endl;

  // Octree benchmark
  pcl::octree::OctreePointCloudSearch<PointT> octree(0.01f);
  auto start_time = std::chrono::high_resolution_clock::now();
  octree.setInputCloud(target);
  octree.addPointsFromInputCloud();
  auto end_time = std::chrono::high_resolution_clock::now();
  auto duration = std::chrono::duration<double, std::micro>(end_time - start_time).count() / 1000.0;
  std::cout << std::fixed << std::setprecision(3) << "Octree build took: " << duration << "ms" << std::endl;

  // Approximate nearest neighbor search using octree for entire source cloud
  start_time = std::chrono::high_resolution_clock::now();
  int result_index;
  float sqr_distance;
  for (const auto& pt : source->points) {
    octree.approxNearestSearch(pt, result_index, sqr_distance);
  }
  end_time = std::chrono::high_resolution_clock::now();
  duration = std::chrono::duration<double, std::micro>(end_time - start_time).count() / 1000.0;
  std::cout << "Octree approximate nearest neighbor search for entire source took: " << duration << "ms" << std::endl;

  std::cout << "Octree k=1 search for entire source took: " << TimeTakenForSearch(octree, source, 1) << "ms" << std::endl;
  std::cout << "Octree k=10 search for entire source took: " << TimeTakenForSearch(octree, source, 10) << "ms" << std::endl;
  std::cout << "Octree k=100 search for entire source took: " << TimeTakenForSearch(octree, source, 100) << "ms" << std::endl;
  std::cout << "Octree radius search 0.01 for entire source took: " << TimeTakenForRadiusSearch(octree, source, 0.01f) << "ms" << std::endl;
  std::cout << "Octree radius search 0.1 for entire source took: " << TimeTakenForRadiusSearch(octree, source, 0.1f) << "ms" << std::endl;
  std::cout << "Octree radius search 1.0 for entire source took: " << TimeTakenForRadiusSearch(octree, source, 1.0f) << "ms" << std::endl;

  // KdTree benchmark
  pcl::KdTreeFLANN<PointT> kdtree;
  start_time = std::chrono::high_resolution_clock::now();
  kdtree.setInputCloud(target);
  end_time = std::chrono::high_resolution_clock::now();
  duration = std::chrono::duration<double, std::micro>(end_time - start_time).count() / 1000.0;
  std::cout << "KdTree build took: " << duration << "ms" << std::endl;

  std::cout << "KdTree k=1 search for entire source took: " << TimeTakenForSearch(kdtree, source, 1) << "ms" << std::endl;
  std::cout << "KdTree k=10 search for entire source took: " << TimeTakenForSearch(kdtree, source, 10) << "ms" << std::endl;
  std::cout << "KdTree k=100 search for entire source took: " << TimeTakenForSearch(kdtree, source, 100) << "ms" << std::endl;
  std::cout << "KdTree radius search 0.01 for entire source took: " << TimeTakenForRadiusSearch(kdtree, source, 0.01f) << "ms" << std::endl;
  std::cout << "KdTree radius search 0.1 for entire source took: " << TimeTakenForRadiusSearch(kdtree, source, 0.1f) << "ms" << std::endl;
  std::cout << "KdTree radius search 1.0 for entire source took: " << TimeTakenForRadiusSearch(kdtree, source, 1.0f) << "ms" << std::endl;

  return 0;
}