Comment on page

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;
}