Point Cloud Library (PCL)  1.14.1-dev
approx_nearest_utils.hpp
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2020-, Open Perception
6  *
7  * All rights reserved
8  */
9 
10 #pragma once
11 
12 #include "morton.hpp"
13 #include <assert.h>
14 
15 #include <cstdint>
16 #include <limits>
17 #include <tuple>
18 #include <bitset>
19 
20 namespace pcl {
21 namespace device {
22 
23 __device__ __host__ __forceinline__ unsigned
24 getBitsNum(const unsigned integer)
25 {
26  #ifdef __CUDA_ARCH__
27  return __popc(integer);
28  #else
29  return std::bitset<8*sizeof(integer)> (integer).count();
30  #endif
31 }
32 
33 __host__ __device__ __forceinline__ std::pair<uint3, std::uint8_t>
34 nearestVoxel(const float3 query,
35  const unsigned& level,
36  const std::uint8_t& mask,
37  const float3& minp,
38  const float3& maxp,
39  const uint3& index)
40 {
41  assert(mask != 0);
42  // identify closest voxel
43  float closest_distance = std::numeric_limits<float>::max();
44  unsigned closest_index = 0;
45  uint3 closest = make_uint3(0, 0, 0);
46 
47  for (unsigned i = 0; i < 8; ++i) {
48  if ((mask & (1 << i)) == 0) // no child
49  continue;
50 
51  const uint3 child = make_uint3((index.x << 1) + (i & 1),
52  (index.y << 1) + ((i >> 1) & 1),
53  (index.z << 1) + ((i >> 2) & 1));
54 
55  // find center of child cell
56  const unsigned voxel_width_scale_factor = 1 << (level + 2);
57  const float3 voxel_center =
58  make_float3(minp.x + (maxp.x - minp.x) * (2 * child.x + 1) / voxel_width_scale_factor,
59  minp.y + (maxp.y - minp.y) * (2 * child.y + 1) / voxel_width_scale_factor,
60  minp.z + (maxp.z - minp.z) * (2 * child.z + 1) / voxel_width_scale_factor);
61 
62  // compute distance to centroid
63  const float3 dist = make_float3(
64  voxel_center.x - query.x, voxel_center.y - query.y, voxel_center.z - query.z);
65 
66  const float distance_to_query = dist.x * dist.x + dist.y * dist.y + dist.z * dist.z;
67 
68  // compare distance
69  if (distance_to_query < closest_distance) {
70  closest_distance = distance_to_query;
71  closest_index = i;
72  closest = child;
73  }
74  }
75 
76  return {closest, 1 << closest_index};
77 }
78 
79 #pragma hd_warning_disable
80 template <typename T>
81 __device__ __host__ int
82 findNode(const float3 minp, const float3 maxp, const float3 query, const T nodes)
83 {
84  size_t node_idx = 0;
85  const auto code = CalcMorton(minp, maxp)(query);
86  unsigned level = 0;
87 
88  bool voxel_traversal = false;
89  uint3 index = Morton::decomposeCode(code);
90  std::uint8_t mask_pos;
91 
92  while (true) {
93  const auto node = nodes[node_idx];
94  const std::uint8_t mask = node & 0xFF;
95 
96  if (!mask) // leaf
97  return node_idx;
98 
99  if (voxel_traversal) // empty voxel already encountered, performing nearest-centroid
100  // based traversal
101  {
102  const auto nearest_voxel = nearestVoxel(query, level, mask, minp, maxp, index);
103  index = nearest_voxel.first;
104  mask_pos = nearest_voxel.second;
105  }
106  else {
107  mask_pos = 1 << Morton::extractLevelCode(code, level);
108 
109  if (!(mask & mask_pos)) // child doesn't exist
110  {
111  const auto remaining_depth = Morton::levels - level;
112  index.x >>= remaining_depth;
113  index.y >>= remaining_depth;
114  index.z >>= remaining_depth;
115 
116  voxel_traversal = true;
117  const auto nearest_voxel = nearestVoxel(query, level, mask, minp, maxp, index);
118  index = nearest_voxel.first;
119  mask_pos = nearest_voxel.second;
120  }
121  }
122  node_idx = (node >> 8) + getBitsNum(mask & (mask_pos - 1));
123  ++level;
124  }
125 }
126 } // namespace device
127 } // namespace pcl
__device__ __host__ int findNode(const float3 minp, const float3 maxp, const float3 query, const T nodes)
__device__ __host__ __forceinline__ unsigned getBitsNum(const unsigned integer)
__host__ __device__ __forceinline__ std::pair< uint3, std::uint8_t > nearestVoxel(const float3 query, const unsigned &level, const std::uint8_t &mask, const float3 &minp, const float3 &maxp, const uint3 &index)
Definition: inftrees.h:24
__device__ __host__ static __forceinline__ void decomposeCode(code_t code, int &cell_x, int &cell_y, int &cell_z)
Definition: morton.hpp:84
__host__ __device__ static __forceinline__ code_t extractLevelCode(code_t code, int level)
Definition: morton.hpp:98
static const int levels
Definition: morton.hpp:47