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.
- 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
approxNearestSearchmethod 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.
- 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.
#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;
}
Last modified 29d ago