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