Skip to content

Commit 2dfd5cc

Browse files
neka-natneka-nat
committed
kdtree cuda
1 parent 9fd8171 commit 2dfd5cc

22 files changed

+2403
-220
lines changed

‎CMakeLists.txt

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -106,7 +106,13 @@ set(CUDA_ARCH -gencode arch=compute_30,code=sm_30
106106
-gencode arch=compute_60,code=sm_60
107107
-gencode arch=compute_61,code=sm_61
108108
-gencode arch=compute_70,code=sm_70)
109-
set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} --expt-relaxed-constexpr --expt-extended-lambda ${CUDA_ARCH})
109+
set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS}
110+
--expt-relaxed-constexpr
111+
--expt-extended-lambda
112+
-Xcudafe "--diag_suppress=integer_sign_change"
113+
-Xcudafe "--diag_suppress=partial_override"
114+
-Xcudafe "--diag_suppress=virtual_function_decl_hidden"
115+
${CUDA_ARCH})
110116

111117
add_subdirectory(third_party)
112118

‎LICENSE

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
MIT License
2+
3+
Copyright (c) 2019 neka-nat
4+
5+
Permission is hereby granted, free of charge, to any person obtaining a copy
6+
of this software and associated documentation files (the "Software"), to deal
7+
in the Software without restriction, including without limitation the rights
8+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9+
copies of the Software, and to permit persons to whom the Software is
10+
furnished to do so, subject to the following conditions:
11+
12+
The above copyright notice and this permission notice shall be included in all
13+
copies or substantial portions of the Software.
14+
15+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21+
SOFTWARE.

‎src/cupoc/geometry/CMakeLists.txt

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,6 @@
1-
cuda_add_library(cupoc_geometry pointcloud.cu estimate_normals.cu geometry3d.cu kdtree_flann.cu)
2-
target_link_libraries(cupoc_geometry cupoc_utility)
1+
cuda_add_library(cupoc_geometry pointcloud.cu
2+
estimate_normals.cu
3+
down_sample.cu
4+
geometry3d.cu
5+
kdtree_flann.cu)
6+
target_link_libraries(cupoc_geometry cupoc_utility flann_cuda)

‎src/cupoc/geometry/down_sample.cu

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
#include "cupoc/geometry/pointcloud.h"
2+
#include "cupoc/geometry/geometry3d.h"
3+
#include "cupoc/utility/console.h"
4+
#include "cupoc/utility/helper.h"
5+
#include <thrust/gather.h>
6+
7+
8+
using namespace cupoc;
9+
using namespace cupoc::geometry;
10+
11+
namespace {
12+
13+
struct compute_key_functor {
14+
compute_key_functor(const Eigen::Vector3f& voxel_min_bound, float voxel_size)
15+
: voxel_min_bound_(voxel_min_bound), voxel_size_(voxel_size) {};
16+
const Eigen::Vector3f voxel_min_bound_;
17+
const float voxel_size_;
18+
__device__
19+
Eigen::Vector3i operator()(const Eigen::Vector3f_u& pt) {
20+
auto ref_coord = (pt - voxel_min_bound_) / voxel_size_;
21+
return Eigen::Vector3i(int(floor(ref_coord(0))), int(floor(ref_coord(1))), int(floor(ref_coord(2))));
22+
}
23+
};
24+
25+
template<typename OutputIterator, class... Args>
26+
__host__
27+
int CalcAverageByKey(thrust::device_vector<Eigen::Vector3i>& keys,
28+
OutputIterator buf_begins, OutputIterator output_begins) {
29+
const size_t n = keys.size();
30+
thrust::sort_by_key(keys.begin(), keys.end(), buf_begins);
31+
32+
thrust::device_vector<Eigen::Vector3i> keys_out(n);
33+
thrust::device_vector<int> counts(n);
34+
auto end1 = thrust::reduce_by_key(keys.begin(), keys.end(),
35+
thrust::make_constant_iterator(1),
36+
keys_out.begin(), counts.begin());
37+
int n_out = static_cast<int>(end1.second - counts.begin());
38+
counts.resize(n_out);
39+
40+
thrust::equal_to<Eigen::Vector3i> binary_pred;
41+
add_tuple_functor<Args...> add_func;
42+
auto end2 = thrust::reduce_by_key(keys.begin(), keys.end(), buf_begins,
43+
keys_out.begin(), output_begins,
44+
binary_pred, add_func);
45+
46+
devided_tuple_functor<Args...> dv_func;
47+
thrust::transform(output_begins, output_begins + n_out,
48+
counts.begin(), output_begins,
49+
dv_func);
50+
return n_out;
51+
}
52+
53+
}
54+
55+
utility::shared_ptr<PointCloud> PointCloud::SelectDownSample(const thrust::device_vector<size_t> &indices, bool invert) const {
56+
auto output = utility::shared_ptr<PointCloud>(new PointCloud());
57+
const bool has_normals = HasNormals();
58+
const bool has_colors = HasColors();
59+
60+
output->points_.resize(indices.size());
61+
thrust::gather(indices.begin(), indices.end(), points_.begin(), output->points_.begin());
62+
return output;
63+
}
64+
65+
utility::shared_ptr<PointCloud> PointCloud::VoxelDownSample(float voxel_size) const {
66+
auto output = utility::shared_ptr<PointCloud>(new PointCloud());
67+
if (voxel_size <= 0.0) {
68+
utility::LogWarning("[VoxelDownSample] voxel_size <= 0.\n");
69+
return output;
70+
}
71+
72+
const Eigen::Vector3f voxel_size3 = Eigen::Vector3f(voxel_size, voxel_size, voxel_size);
73+
const Eigen::Vector3f voxel_min_bound = GetMinBound() - voxel_size3 * 0.5;
74+
const Eigen::Vector3f voxel_max_bound = GetMaxBound() + voxel_size3 * 0.5;
75+
76+
if (voxel_size * std::numeric_limits<int>::max() < (voxel_max_bound - voxel_min_bound).maxCoeff()) {
77+
utility::LogWarning("[VoxelDownSample] voxel_size is too small.\n");
78+
return output;
79+
}
80+
81+
const int n = points_.size();
82+
const bool has_normals = HasNormals();
83+
const bool has_colors = HasColors();
84+
compute_key_functor ck_func(voxel_min_bound, voxel_size);
85+
thrust::device_vector<Eigen::Vector3i> keys(n);
86+
thrust::transform(points_.begin(), points_.end(), keys.begin(), ck_func);
87+
88+
thrust::device_vector<Eigen::Vector3f_u> sorted_points = points_;
89+
output->points_.resize(n);
90+
if (!has_normals && !has_colors) {
91+
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
92+
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
93+
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u>(keys,
94+
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin())),
95+
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin())));
96+
output->points_.resize(n_out);
97+
} else if (has_normals && !has_colors) {
98+
thrust::device_vector<Eigen::Vector3f_u> sorted_normals = normals_;
99+
output->normals_.resize(n);
100+
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
101+
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
102+
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
103+
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_normals.begin())),
104+
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->normals_.begin())));
105+
output->points_.resize(n_out);
106+
output->normals_.resize(n_out);
107+
} else if (!has_normals && has_colors) {
108+
thrust::device_vector<Eigen::Vector3f_u> sorted_colors = colors_;
109+
output->colors_.resize(n);
110+
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
111+
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
112+
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
113+
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_colors.begin())),
114+
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->colors_.begin())));
115+
output->points_.resize(n_out);
116+
output->colors_.resize(n_out);
117+
} else {
118+
thrust::device_vector<Eigen::Vector3f_u> sorted_normals = normals_;
119+
thrust::device_vector<Eigen::Vector3f_u> sorted_colors = colors_;
120+
output->normals_.resize(n);
121+
output->colors_.resize(n);
122+
typedef thrust::tuple<thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator, thrust::device_vector<Eigen::Vector3f_u>::iterator> IteratorTuple;
123+
typedef thrust::zip_iterator<IteratorTuple> ZipIterator;
124+
auto n_out = CalcAverageByKey<ZipIterator, Eigen::Vector3f_u, Eigen::Vector3f_u, Eigen::Vector3f_u>(keys,
125+
thrust::make_zip_iterator(thrust::make_tuple(sorted_points.begin(), sorted_normals.begin(), sorted_colors.begin())),
126+
thrust::make_zip_iterator(thrust::make_tuple(output->points_.begin(), output->normals_.begin(), output->colors_.begin())));
127+
output->points_.resize(n_out);
128+
output->normals_.resize(n_out);
129+
output->colors_.resize(n_out);
130+
}
131+
132+
utility::LogDebug(
133+
"Pointcloud down sampled from {:d} points to {:d} points.\n",
134+
(int)points_.size(), (int)output->points_.size());
135+
return output;
136+
}

0 commit comments

Comments
 (0)