# 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.

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

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.

<table><thead><tr><th width="278.3333333333333">Operation</th><th>K-D Tree</th><th>Octree (leaf size 0.01 m)</th><th>Octree (leaf size 0.001m)</th></tr></thead><tbody><tr><td>Build Tree</td><td>4 ms</td><td>6 ms</td><td>9 ms</td></tr><tr><td>Approximate NN Search (k=1)</td><td>(not support)</td><td>1.5 ms</td><td>1.5 ms</td></tr><tr><td>K-NN Search (k=1)</td><td>3 ms</td><td>23 ms</td><td>23 ms</td></tr><tr><td>K-NN Search (k=10)</td><td>5 ms</td><td>41 ms</td><td>57 ms</td></tr><tr><td>K-NN Search (k=100)</td><td>21 ms</td><td>407 ms</td><td>539 ms</td></tr><tr><td>Radius Search (r=0.01 m)</td><td>1 ms</td><td>2 ms</td><td>2 ms</td></tr><tr><td>Radius Search (r=0.1 m)</td><td>2 ms</td><td>4 ms</td><td>5 ms</td></tr><tr><td>Radius Search (r=1 m)</td><td>34 ms</td><td>59 ms</td><td>138 ms</td></tr></tbody></table>

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

```cpp
#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;
}
```


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://wiki.hanzheteng.com/algorithm/slam/octree.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.