octree_search.hpp

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 */
 
#ifndef PCL_OCTREE_SEARCH_IMPL_H_
#define PCL_OCTREE_SEARCH_IMPL_H_
 
#include <cassert>
#include <type_traits> // for std::false_type, std::true_type, std::void_t
#include <utility>     // for std::declval
 
namespace pcl {
 
namespace octree {
 
namespace detail {
template <class T, class = void>
struct has_getPointIndicesVector : std::false_type {};
 
template <class T>
struct has_getPointIndicesVector<
    T,
    std::void_t<decltype(std::declval<const T&>().getPointIndicesVector())>>
: std::true_type {};
 
/** If leaf has function getPointIndicesVector(), call it. Otherwise call
 * leaf.getPointIndices().
 */
template <class T, std::enable_if_t<has_getPointIndicesVector<T>::value, int> = 0>
decltype(auto)
getPointIndices(const T& leaf)
{
  return leaf.getPointIndicesVector();
}
 
/** If leaf has function getPointIndicesVector(), call it. Otherwise call
 * leaf.getPointIndices().
 */
template <class T, std::enable_if_t<!has_getPointIndicesVector<T>::value, int> = 0>
pcl::Indices
getPointIndices(const T& leaf)
{
  pcl::Indices decoded_point_vector;
  leaf.getPointIndices(decoded_point_vector);
  return decoded_point_vector;
}
} // namespace detail
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
    const PointT& point, Indices& point_idx_data)
{
  assert(isFinite(point) &&
         "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
  OctreeKey key;
  bool b_success = false;
 
  // generate key
  this->genOctreeKeyforPoint(point, key);
 
  LeafContainerT* leaf = this->findLeaf(key);
 
  if (leaf) {
    (*leaf).getPointIndices(point_idx_data);
    b_success = true;
  }
 
  return (b_success);
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
    const uindex_t index, Indices& point_idx_data)
{
  const PointT search_point = this->getPointByIndex(index);
  return (this->voxelSearch(search_point, point_idx_data));
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
    const PointT& p_q,
    uindex_t k,
    Indices& k_indices,
    std::vector<float>& k_sqr_distances)
{
  assert(this->leaf_count_ > 0);
  assert(isFinite(p_q) &&
         "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
 
  k_indices.clear();
  k_sqr_distances.clear();
 
  if (k < 1)
    return 0;
 
  prioPointQueueEntry point_entry;
  std::vector<prioPointQueueEntry> point_candidates;
  point_candidates.reserve(k);
 
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  // initialize smallest point distance in search with high value
  double smallest_dist = std::numeric_limits<double>::max();
 
  getKNearestNeighborRecursive(
      p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
 
  const auto result_count = static_cast<uindex_t>(point_candidates.size());
 
  k_indices.resize(result_count);
  k_sqr_distances.resize(result_count);
 
  for (uindex_t i = 0; i < result_count; ++i) {
    k_indices[i] = point_candidates[i].point_idx_;
    k_sqr_distances[i] = point_candidates[i].point_distance_;
  }
 
  return k_indices.size();
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
    uindex_t index, uindex_t k, Indices& k_indices, std::vector<float>& k_sqr_distances)
{
  const PointT search_point = this->getPointByIndex(index);
  return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
    const PointT& p_q, index_t& result_index, float& sqr_distance)
{
  assert(this->leaf_count_ > 0);
  assert(isFinite(p_q) &&
         "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
 
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  approxNearestSearchRecursive(
      p_q, this->root_node_, key, 1, result_index, sqr_distance);
 
  return;
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::approxNearestSearch(
    uindex_t query_index, index_t& result_index, float& sqr_distance)
{
  const PointT search_point = this->getPointByIndex(query_index);
 
  return (approxNearestSearch(search_point, result_index, sqr_distance));
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
    const PointT& p_q,
    const double radius,
    Indices& k_indices,
    std::vector<float>& k_sqr_distances,
    uindex_t max_nn) const
{
  assert(isFinite(p_q) &&
         "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  k_indices.clear();
  k_sqr_distances.clear();
 
  getNeighborsWithinRadiusRecursive(p_q,
                                    radius * radius,
                                    this->root_node_,
                                    key,
                                    1,
                                    k_indices,
                                    k_sqr_distances,
                                    max_nn);
 
  return k_indices.size();
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
    uindex_t index,
    const double radius,
    Indices& k_indices,
    std::vector<float>& k_sqr_distances,
    uindex_t max_nn) const
{
  const PointT search_point = this->getPointByIndex(index);
 
  return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearch(
    const Eigen::Vector3f& min_pt,
    const Eigen::Vector3f& max_pt,
    Indices& k_indices) const
{
 
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  k_indices.clear();
 
  boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
 
  return k_indices.size();
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
double
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getKNearestNeighborRecursive(
        const PointT& point,
        uindex_t K,
        const BranchNode* node,
        const OctreeKey& key,
        uindex_t tree_depth,
        const double squared_search_radius,
        std::vector<prioPointQueueEntry>& point_candidates) const
{
  std::vector<prioBranchQueueEntry> search_heap;
  search_heap.resize(8);
 
  OctreeKey new_key;
 
  double smallest_squared_dist = squared_search_radius;
 
  // get spatial voxel information
  double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
 
  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    if (this->branchHasChild(*node, child_idx)) {
      PointT voxel_center;
 
      search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
      search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
      search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
 
      // generate voxel center point for voxel at key
      this->genVoxelCenterFromOctreeKey(
          search_heap[child_idx].key, tree_depth, voxel_center);
 
      // generate new priority queue element
      search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
      search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
    }
    else {
      search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
    }
  }
 
  std::sort(search_heap.begin(), search_heap.end());
 
  // iterate over all children in priority queue
  // check if the distance to search candidate is smaller than the best point distance
  // (smallest_squared_dist)
  while ((!search_heap.empty()) &&
         (search_heap.back().point_distance <
          smallest_squared_dist + voxelSquaredDiameter / 4.0 +
              sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
    const OctreeNode* child_node;
 
    // read from priority queue element
    child_node = search_heap.back().node;
    new_key = search_heap.back().key;
 
    if (child_node->getNodeType() == BRANCH_NODE) {
      // we have not reached maximum tree depth
      smallest_squared_dist =
          getKNearestNeighborRecursive(point,
                                       K,
                                       static_cast<const BranchNode*>(child_node),
                                       new_key,
                                       tree_depth + 1,
                                       smallest_squared_dist,
                                       point_candidates);
    }
    else {
      // we reached leaf node level
      Indices decoded_point_vector;
 
      const auto* child_leaf = static_cast<const LeafNode*>(child_node);
 
      // decode leaf node into decoded_point_vector
      (*child_leaf)->getPointIndices(decoded_point_vector);
 
      // Linearly iterate over all decoded (unsorted) points
      for (const auto& point_index : decoded_point_vector) {
 
        const PointT& candidate_point = this->getPointByIndex(point_index);
 
        // calculate point distance to search point
        float squared_dist = pointSquaredDist(candidate_point, point);
 
        const auto insert_into_queue = [&] {
          point_candidates.emplace(
              std::upper_bound(point_candidates.begin(),
                               point_candidates.end(),
                               squared_dist,
                               [](float dist, const prioPointQueueEntry& ent) {
                                 return dist < ent.point_distance_;
                               }),
              point_index,
              squared_dist);
        };
        if (point_candidates.size() < K) {
          insert_into_queue();
        }
        else if (point_candidates.back().point_distance_ > squared_dist) {
          point_candidates.pop_back();
          insert_into_queue();
        }
      }
 
      if (point_candidates.size() == K)
        smallest_squared_dist = point_candidates.back().point_distance_;
    }
    // pop element from priority queue
    search_heap.pop_back();
  }
 
  return (smallest_squared_dist);
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getNeighborsWithinRadiusRecursive(const PointT& point,
                                      const double radiusSquared,
                                      const BranchNode* node,
                                      const OctreeKey& key,
                                      uindex_t tree_depth,
                                      Indices& k_indices,
                                      std::vector<float>& k_sqr_distances,
                                      uindex_t max_nn) const
{
  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    if (!this->branchHasChild(*node, child_idx))
      continue;
 
    const OctreeNode* child_node;
    child_node = this->getBranchChildPtr(*node, child_idx);
 
    OctreeKey new_key;
    float squared_dist;
 
    // generate new key for current branch voxel
    new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
    new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
    new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
 
    // compute min distance between query point and any point in this child node, to
    // decide whether we can skip it
    Eigen::Vector3f min_pt, max_pt;
    this->genVoxelBoundsFromOctreeKey(new_key, tree_depth, min_pt, max_pt);
    squared_dist = 0.0f;
    if (point.x < min_pt.x())
      squared_dist += std::pow(point.x - min_pt.x(), 2);
    else if (point.x > max_pt.x())
      squared_dist += std::pow(point.x - max_pt.x(), 2);
    if (point.y < min_pt.y())
      squared_dist += std::pow(point.y - min_pt.y(), 2);
    else if (point.y > max_pt.y())
      squared_dist += std::pow(point.y - max_pt.y(), 2);
    if (point.z < min_pt.z())
      squared_dist += std::pow(point.z - min_pt.z(), 2);
    else if (point.z > max_pt.z())
      squared_dist += std::pow(point.z - max_pt.z(), 2);
    if (squared_dist < (radiusSquared + this->epsilon_)) {
      if (child_node->getNodeType() == BRANCH_NODE) {
        // we have not reached maximum tree depth
        getNeighborsWithinRadiusRecursive(point,
                                          radiusSquared,
                                          static_cast<const BranchNode*>(child_node),
                                          new_key,
                                          tree_depth + 1,
                                          k_indices,
                                          k_sqr_distances,
                                          max_nn);
        if (max_nn != 0 && k_indices.size() == max_nn)
          return;
      }
      else {
        // we reached leaf node level
        const auto* child_leaf = static_cast<const LeafNode*>(child_node);
 
        // Linearly iterate over all points in the leaf
        for (const auto& index : detail::getPointIndices(*(*child_leaf))) {
          const PointT& candidate_point = this->getPointByIndex(index);
 
          // calculate point distance to search point
          squared_dist = pointSquaredDist(candidate_point, point);
 
          // check if a match is found
          if (squared_dist > radiusSquared)
            continue;
 
          // add point to result vector
          k_indices.push_back(index);
          k_sqr_distances.push_back(squared_dist);
 
          if (max_nn != 0 && k_indices.size() == max_nn)
            return;
        }
      }
    }
  }
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    approxNearestSearchRecursive(const PointT& point,
                                 const BranchNode* node,
                                 const OctreeKey& key,
                                 uindex_t tree_depth,
                                 index_t& result_index,
                                 float& sqr_distance)
{
  OctreeKey minChildKey;
  OctreeKey new_key;
 
  const OctreeNode* child_node;
 
  // set minimum voxel distance to maximum value
  double min_voxel_center_distance = std::numeric_limits<double>::max();
 
  unsigned char min_child_idx = 0xFF;
 
  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    if (!this->branchHasChild(*node, child_idx))
      continue;
 
    PointT voxel_center;
    double voxelPointDist;
 
    new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
    new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
    new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
 
    // generate voxel center point for voxel at key
    this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
 
    voxelPointDist = pointSquaredDist(voxel_center, point);
 
    // search for child voxel with shortest distance to search point
    if (voxelPointDist >= min_voxel_center_distance)
      continue;
 
    min_voxel_center_distance = voxelPointDist;
    min_child_idx = child_idx;
    minChildKey = new_key;
  }
 
  // make sure we found at least one branch child
  assert(min_child_idx < 8);
 
  child_node = this->getBranchChildPtr(*node, min_child_idx);
 
  if (child_node->getNodeType() == BRANCH_NODE) {
    // we have not reached maximum tree depth
    approxNearestSearchRecursive(point,
                                 static_cast<const BranchNode*>(child_node),
                                 minChildKey,
                                 tree_depth + 1,
                                 result_index,
                                 sqr_distance);
  }
  else {
    // we reached leaf node level
    Indices decoded_point_vector;
 
    const auto* child_leaf = static_cast<const LeafNode*>(child_node);
 
    float smallest_squared_dist = std::numeric_limits<float>::max();
 
    // decode leaf node into decoded_point_vector
    (**child_leaf).getPointIndices(decoded_point_vector);
 
    // Linearly iterate over all decoded (unsorted) points
    for (const auto& index : decoded_point_vector) {
      const PointT& candidate_point = this->getPointByIndex(index);
 
      // calculate point distance to search point
      float squared_dist = pointSquaredDist(candidate_point, point);
 
      // check if a closer match is found
      if (squared_dist >= smallest_squared_dist)
        continue;
 
      result_index = index;
      smallest_squared_dist = squared_dist;
      sqr_distance = squared_dist;
    }
  }
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
float
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist(
    const PointT& point_a, const PointT& point_b) const
{
  return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::boxSearchRecursive(
    const Eigen::Vector3f& min_pt,
    const Eigen::Vector3f& max_pt,
    const BranchNode* node,
    const OctreeKey& key,
    uindex_t tree_depth,
    Indices& k_indices) const
{
  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
 
    const OctreeNode* child_node;
    child_node = this->getBranchChildPtr(*node, child_idx);
 
    if (!child_node)
      continue;
 
    OctreeKey new_key;
    // generate new key for current branch voxel
    new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
    new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
    new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
 
    // voxel corners
    Eigen::Vector3f lower_voxel_corner;
    Eigen::Vector3f upper_voxel_corner;
    // get voxel coordinates
    this->genVoxelBoundsFromOctreeKey(
        new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
 
    // test if search region overlap with voxel space
 
    if ((lower_voxel_corner(0) <= max_pt(0)) && (min_pt(0) <= upper_voxel_corner(0)) &&
        (lower_voxel_corner(1) <= max_pt(1)) && (min_pt(1) <= upper_voxel_corner(1)) &&
        (lower_voxel_corner(2) <= max_pt(2)) && (min_pt(2) <= upper_voxel_corner(2))) {
 
      if (child_node->getNodeType() == BRANCH_NODE) {
        // we have not reached maximum tree depth
        boxSearchRecursive(min_pt,
                           max_pt,
                           static_cast<const BranchNode*>(child_node),
                           new_key,
                           tree_depth + 1,
                           k_indices);
      }
      else {
        // we reached leaf node level
        Indices decoded_point_vector;
 
        const auto* child_leaf = static_cast<const LeafNode*>(child_node);
 
        // decode leaf node into decoded_point_vector
        (**child_leaf).getPointIndices(decoded_point_vector);
 
        // Linearly iterate over all decoded (unsorted) points
        for (const auto& index : decoded_point_vector) {
          const PointT& candidate_point = this->getPointByIndex(index);
 
          // check if point falls within search box
          bool bInBox =
              ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
               (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
               (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
 
          if (bInBox)
            // add to result vector
            k_indices.push_back(index);
        }
      }
    }
  }
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getIntersectedVoxelCenters(Eigen::Vector3f origin,
                               Eigen::Vector3f direction,
                               AlignedPointTVector& voxel_center_list,
                               uindex_t max_voxel_count) const
{
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  voxel_center_list.clear();
 
  // Voxel child_idx remapping
  unsigned char a = 0;
 
  double min_x, min_y, min_z, max_x, max_y, max_z;
 
  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
 
  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
    return getIntersectedVoxelCentersRecursive(min_x,
                                               min_y,
                                               min_z,
                                               max_x,
                                               max_y,
                                               max_z,
                                               a,
                                               this->root_node_,
                                               key,
                                               voxel_center_list,
                                               max_voxel_count);
 
  return (0);
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getIntersectedVoxelIndices(Eigen::Vector3f origin,
                               Eigen::Vector3f direction,
                               Indices& k_indices,
                               uindex_t max_voxel_count) const
{
  OctreeKey key;
  key.x = key.y = key.z = 0;
 
  k_indices.clear();
 
  // Voxel child_idx remapping
  unsigned char a = 0;
  double min_x, min_y, min_z, max_x, max_y, max_z;
 
  initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
 
  if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
    return getIntersectedVoxelIndicesRecursive(min_x,
                                               min_y,
                                               min_z,
                                               max_x,
                                               max_y,
                                               max_z,
                                               a,
                                               this->root_node_,
                                               key,
                                               k_indices,
                                               max_voxel_count);
  return (0);
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getIntersectedVoxelCentersRecursive(double min_x,
                                        double min_y,
                                        double min_z,
                                        double max_x,
                                        double max_y,
                                        double max_z,
                                        unsigned char a,
                                        const OctreeNode* node,
                                        const OctreeKey& key,
                                        AlignedPointTVector& voxel_center_list,
                                        uindex_t max_voxel_count) const
{
  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
    return (0);
 
  // If leaf node, get voxel center and increment intersection count
  if (node->getNodeType() == LEAF_NODE) {
    PointT newPoint;
 
    this->genLeafNodeCenterFromOctreeKey(key, newPoint);
 
    voxel_center_list.push_back(newPoint);
 
    return (1);
  }
 
  // Voxel intersection count for branches children
  uindex_t voxel_count = 0;
 
  // Voxel mid lines
  double mid_x = 0.5 * (min_x + max_x);
  double mid_y = 0.5 * (min_y + max_y);
  double mid_z = 0.5 * (min_z + max_z);
 
  // First voxel node ray will intersect
  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
 
  // Child index, node and key
  unsigned char child_idx;
  OctreeKey child_key;
 
  do {
    if (curr_node != 0)
      child_idx = static_cast<unsigned char>(curr_node ^ a);
    else
      child_idx = a;
 
    // child_node == 0 if child_node doesn't exist
    const OctreeNode* child_node =
        this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
 
    // Generate new key for current branch voxel
    child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
    child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
    child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
 
    // Recursively call each intersected child node, selecting the next
    //   node intersected by the ray.  Children that do not intersect will
    //   not be traversed.
 
    switch (curr_node) {
    case 0:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(min_x,
                                                           min_y,
                                                           min_z,
                                                           mid_x,
                                                           mid_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
      break;
 
    case 1:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(min_x,
                                                           min_y,
                                                           mid_z,
                                                           mid_x,
                                                           mid_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
      break;
 
    case 2:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(min_x,
                                                           mid_y,
                                                           min_z,
                                                           mid_x,
                                                           max_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
      break;
 
    case 3:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(min_x,
                                                           mid_y,
                                                           mid_z,
                                                           mid_x,
                                                           max_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
      break;
 
    case 4:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
                                                           min_y,
                                                           min_z,
                                                           max_x,
                                                           mid_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
      break;
 
    case 5:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
                                                           min_y,
                                                           mid_z,
                                                           max_x,
                                                           mid_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
      break;
 
    case 6:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
                                                           mid_y,
                                                           min_z,
                                                           max_x,
                                                           max_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
      break;
 
    case 7:
      if (child_node)
        voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
                                                           mid_y,
                                                           mid_z,
                                                           max_x,
                                                           max_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           voxel_center_list,
                                                           max_voxel_count);
      curr_node = 8;
      break;
    }
  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
  return (voxel_count);
}
 
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
uindex_t
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
    getIntersectedVoxelIndicesRecursive(double min_x,
                                        double min_y,
                                        double min_z,
                                        double max_x,
                                        double max_y,
                                        double max_z,
                                        unsigned char a,
                                        const OctreeNode* node,
                                        const OctreeKey& key,
                                        Indices& k_indices,
                                        uindex_t max_voxel_count) const
{
  if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
    return (0);
 
  // If leaf node, get voxel center and increment intersection count
  if (node->getNodeType() == LEAF_NODE) {
    const auto* leaf = static_cast<const LeafNode*>(node);
 
    // decode leaf node into k_indices
    (*leaf)->getPointIndices(k_indices);
 
    return (1);
  }
 
  // Voxel intersection count for branches children
  uindex_t voxel_count = 0;
 
  // Voxel mid lines
  double mid_x = 0.5 * (min_x + max_x);
  double mid_y = 0.5 * (min_y + max_y);
  double mid_z = 0.5 * (min_z + max_z);
 
  // First voxel node ray will intersect
  auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
 
  // Child index, node and key
  unsigned char child_idx;
  OctreeKey child_key;
  do {
    if (curr_node != 0)
      child_idx = static_cast<unsigned char>(curr_node ^ a);
    else
      child_idx = a;
 
    // child_node == 0 if child_node doesn't exist
    const OctreeNode* child_node =
        this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
    // Generate new key for current branch voxel
    child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2)));
    child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1)));
    child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0)));
 
    // Recursively call each intersected child node, selecting the next
    //   node intersected by the ray.  Children that do not intersect will
    //   not be traversed.
    switch (curr_node) {
    case 0:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
                                                           min_y,
                                                           min_z,
                                                           mid_x,
                                                           mid_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
      break;
 
    case 1:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
                                                           min_y,
                                                           mid_z,
                                                           mid_x,
                                                           mid_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
      break;
 
    case 2:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
                                                           mid_y,
                                                           min_z,
                                                           mid_x,
                                                           max_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
      break;
 
    case 3:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
                                                           mid_y,
                                                           mid_z,
                                                           mid_x,
                                                           max_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
      break;
 
    case 4:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
                                                           min_y,
                                                           min_z,
                                                           max_x,
                                                           mid_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
      break;
 
    case 5:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
                                                           min_y,
                                                           mid_z,
                                                           max_x,
                                                           mid_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
      break;
 
    case 6:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
                                                           mid_y,
                                                           min_z,
                                                           max_x,
                                                           max_y,
                                                           mid_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
      break;
 
    case 7:
      if (child_node)
        voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
                                                           mid_y,
                                                           mid_z,
                                                           max_x,
                                                           max_y,
                                                           max_z,
                                                           a,
                                                           child_node,
                                                           child_key,
                                                           k_indices,
                                                           max_voxel_count);
      curr_node = 8;
      break;
    }
  } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
 
  return (voxel_count);
}
 
} // namespace octree
} // namespace pcl
 
#define PCL_INSTANTIATE_OctreePointCloudSearch(T)                                      \
  template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
 
#endif // PCL_OCTREE_SEARCH_IMPL_H_