Open3D (C++ API)  0.16.1
VoxelPooling.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// The MIT License (MIT)
5//
6// Copyright (c) 2018-2021 www.open3d.org
7//
8// Permission is hereby granted, free of charge, to any person obtaining a copy
9// of this software and associated documentation files (the "Software"), to deal
10// in the Software without restriction, including without limitation the rights
11// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12// copies of the Software, and to permit persons to whom the Software is
13// furnished to do so, subject to the following conditions:
14//
15// The above copyright notice and this permission notice shall be included in
16// all copies or substantial portions of the Software.
17//
18// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24// IN THE SOFTWARE.
25// ----------------------------------------------------------------------------
26
27#pragma once
28
29#include <tbb/task_group.h>
30
31#include <Eigen/Core>
32#include <unordered_map>
33
35
36namespace open3d {
37namespace ml {
38namespace impl {
39
41
42template <class TReal,
43 class TFeat,
44 AccumulationFn POS_FN,
45 AccumulationFn FEAT_FN>
47public:
49 : count_(0),
50 min_sqr_dist_to_center_(std::numeric_limits<TReal>::max()),
51 position_(0, 0, 0) {
52 static_assert(POS_FN != MAX, "MAX is not allowed for point positions");
53 static_assert(FEAT_FN != CENTER,
54 "CENTER is not allowed for feature vectors");
55 }
56
57 template <class Derived, class Derived2, class Derived3>
58 inline void AddPoint(const Eigen::MatrixBase<Derived>& pos,
59 const Eigen::MatrixBase<Derived2>& voxel_center,
60 const Eigen::ArrayBase<Derived3>& feat) {
61 bool new_nearest_neighbor = false;
62 TReal sqr_d = 0;
63 if (POS_FN == NEAREST_NEIGHBOR || FEAT_FN == NEAREST_NEIGHBOR) {
64 sqr_d = (voxel_center - pos).squaredNorm();
65 if (sqr_d < min_sqr_dist_to_center_) {
66 new_nearest_neighbor = true;
67 min_sqr_dist_to_center_ = sqr_d;
68 }
69 }
70 if (POS_FN == AVERAGE) {
71 position_ += pos.array();
72 } else if (POS_FN == NEAREST_NEIGHBOR && new_nearest_neighbor) {
73 position_ = pos;
74 } else if (POS_FN == CENTER) {
75 if (count_ == 0) position_ = voxel_center;
76 }
77
78 if (count_ == 0) {
79 features_.resizeLike(feat);
80 features_.setZero();
81 }
82 if (FEAT_FN == AVERAGE) {
83 features_ += feat;
84 } else if (FEAT_FN == NEAREST_NEIGHBOR && new_nearest_neighbor) {
85 features_ = feat;
86 } else if (FEAT_FN == MAX) {
87 features_ = features_.max(feat);
88 }
89 ++count_;
90 }
91
92 inline Eigen::Array<TReal, 3, 1> Position() const {
93 if (POS_FN == AVERAGE) {
94 return position_ / count_;
95 } else // if( POS_FN == NEAREST_NEIGHBOR || POS_FN == CENTER )
96 {
97 return position_;
98 }
99 }
100
101 inline Eigen::Array<TFeat, Eigen::Dynamic, 1> Features() const {
102 if (FEAT_FN == AVERAGE) {
103 return features_ / count_;
104 } else // if( FEAT_FN == NEAREST_NEIGHBOR || FEAT_FN == MAX )
105 {
106 return features_;
107 }
108 }
109
110 inline int Count() const { return count_; }
111
112private:
113 int count_;
114 TReal min_sqr_dist_to_center_;
115 Eigen::Array<TReal, 3, 1> position_;
116 Eigen::Array<TFeat, Eigen::Dynamic, 1> features_;
117}; // namespace
118
119template <class TReal,
120 class TFeat,
121 AccumulationFn POS_FN,
122 AccumulationFn FEAT_FN>
124public:
126 : count_(0),
127 min_sqr_dist_to_center_(std::numeric_limits<TReal>::max()),
128 position_(0, 0, 0) {
129 static_assert(POS_FN != MAX, "MAX is not allowed for point positions");
130 static_assert(FEAT_FN != CENTER,
131 "CENTER is not allowed for feature vectors");
132 }
133
134 template <class Derived, class Derived2, class Derived3>
135 inline void AddPoint(const Eigen::MatrixBase<Derived>& pos,
136 const Eigen::MatrixBase<Derived2>& voxel_center,
137 const Eigen::ArrayBase<Derived3>& feat,
138 const size_t idx) {
139 bool new_nearest_neighbor = false;
140 TReal sqr_d = 0;
141 if (POS_FN == NEAREST_NEIGHBOR || FEAT_FN == NEAREST_NEIGHBOR) {
142 sqr_d = (voxel_center - pos).squaredNorm();
143 if (sqr_d < min_sqr_dist_to_center_) {
144 new_nearest_neighbor = true;
145 min_sqr_dist_to_center_ = sqr_d;
146 }
147 }
148
149 if (POS_FN == AVERAGE) {
150 position_ += pos.array();
151 } else if (POS_FN == NEAREST_NEIGHBOR && new_nearest_neighbor) {
152 position_ = pos;
153 } else if (POS_FN == CENTER) {
154 if (count_ == 0) position_ = voxel_center;
155 }
156
157 if (count_ == 0) {
158 features_.resizeLike(feat);
159 features_.setZero();
160 if (FEAT_FN == NEAREST_NEIGHBOR) {
161 features_ = feat;
162 index_.resize(1);
163 index_(0) = idx;
164 ++count_;
165 return;
166 } else if (FEAT_FN == MAX) {
167 features_ = feat;
168 index_.resizeLike(feat);
169 index_ = idx;
170 ++count_;
171 return;
172 }
173 }
174 if (FEAT_FN == AVERAGE) {
175 features_ += feat;
176 } else if (FEAT_FN == NEAREST_NEIGHBOR && new_nearest_neighbor) {
177 features_ = feat;
178 index_(0) = idx;
179 } else if (FEAT_FN == MAX) {
180 for (int i = 0; i < features_.rows(); ++i) {
181 if (feat(i) > features_(i)) {
182 features_(i) = feat(i);
183 index_(i) = idx;
184 }
185 }
186 }
187 ++count_;
188 }
189
190 inline Eigen::Array<TReal, 3, 1> Position() const {
191 if (POS_FN == AVERAGE) {
192 return position_ / count_;
193 } else // if( POS_FN == NEAREST_NEIGHBOR || POS_FN == CENTER )
194 {
195 return position_;
196 }
197 }
198
199 inline Eigen::Array<TFeat, Eigen::Dynamic, 1> Features() const {
200 if (FEAT_FN == AVERAGE) {
201 return features_ / count_;
202 } else // if( FEAT_FN == NEAREST_NEIGHBOR || FEAT_FN == MAX )
203 {
204 return features_;
205 }
206 }
207
208 inline int Count() const { return count_; }
209
210 inline Eigen::Array<size_t, Eigen::Dynamic, 1> Index() const {
211 return index_;
212 }
213
214private:
215 int count_;
216 TReal min_sqr_dist_to_center_;
217 Eigen::Array<TReal, 3, 1> position_;
218 Eigen::Array<TFeat, Eigen::Dynamic, 1> features_;
219 Eigen::Array<size_t, Eigen::Dynamic, 1> index_;
220};
221
223template <class T>
224bool CheckVoxelSize(std::string& err,
225 const size_t num_positions,
226 const T* const positions,
227 const T voxel_size) {
228 typedef Eigen::Array<double, 3, 1> Vec3_t;
229 if (num_positions == 0) {
230 return true;
231 }
232
233 Vec3_t bb_min, bb_max;
234 bb_min << positions[0], positions[1], positions[2];
235 bb_max = bb_min;
236
237 Vec3_t voxel_size3(voxel_size, voxel_size, voxel_size);
238
239 for (size_t i = 1; i < num_positions; ++i) {
240 Vec3_t pos(positions[i * 3 + 0], positions[i * 3 + 1],
241 positions[i * 3 + 2]);
242 bb_min = bb_min.min(pos);
243 bb_max = bb_max.max(pos);
244 }
245
246 // the min and max bounding box shall be a multiple of the voxel size
247 bb_min /= voxel_size3;
248 bb_min = bb_min.floor() * voxel_size3;
249 bb_max /= voxel_size3;
250 bb_max = bb_max.ceil() * voxel_size3;
251
252 if (voxel_size * double(std::numeric_limits<int>::max()) <
253 bb_max.maxCoeff() ||
254 voxel_size * double(std::numeric_limits<int>::min()) >
255 bb_min.maxCoeff()) {
256 err = "voxel_size is too small\n";
257 return false;
258 }
259 return true;
260}
261
267template <class TDerived>
268Eigen::Vector3i ComputeVoxelIndex(
269 const Eigen::ArrayBase<TDerived>& pos,
270 const typename TDerived::Scalar& inv_voxel_size) {
271 typedef typename TDerived::Scalar Scalar_t;
272 Eigen::Array<Scalar_t, 3, 1> ref_coord = pos * inv_voxel_size;
273
274 Eigen::Vector3i voxel_index;
275 voxel_index = ref_coord.floor().template cast<int>();
276 return voxel_index;
277}
278
279// implementation for VoxelPooling with template parameter for the accumulator.
280template <class TReal, class TFeat, class ACCUMULATOR, class OUTPUT_ALLOCATOR>
281void _VoxelPooling(size_t num_inp,
282 const TReal* const inp_positions,
283 int in_channels,
284 const TFeat* inp_features,
285 TReal voxel_size,
286 OUTPUT_ALLOCATOR& output_allocator) {
287 if (num_inp == 0) {
288 TReal* out_pos_ptr;
289 TFeat* out_feat_ptr;
290 output_allocator.AllocPooledPositions(&out_pos_ptr, 0);
291 output_allocator.AllocPooledFeatures(&out_feat_ptr, 0, in_channels);
292 return;
293 }
294
295 typedef Eigen::Array<TReal, 3, 1> Vec3_t;
296 typedef Eigen::Array<TFeat, Eigen::Dynamic, 1> FeatureVec_t;
297
298 std::unordered_map<Eigen::Vector3i, ACCUMULATOR,
300 voxelindex_to_accpoint;
301
302 Vec3_t voxel_center;
303 Eigen::Vector3i voxel_index;
304 TReal inv_voxel_size = 1 / voxel_size;
305 TReal half_voxel_size = 0.5 * voxel_size;
306 for (size_t i = 0; i < num_inp; ++i) {
307 Eigen::Map<const Vec3_t> pos(inp_positions + i * 3);
308
309 voxel_index = ComputeVoxelIndex(pos, inv_voxel_size);
310
311 voxel_center << voxel_index(0) * voxel_size + half_voxel_size,
312 voxel_index(1) * voxel_size + half_voxel_size,
313 voxel_index(2) * voxel_size + half_voxel_size;
314
315 Eigen::Map<const FeatureVec_t> feat(inp_features + in_channels * i,
316 in_channels);
317 voxelindex_to_accpoint[voxel_index].AddPoint(
318 pos.matrix(), voxel_center.matrix(), feat);
319 }
320
321 const size_t num_out = voxelindex_to_accpoint.size();
322
323 TReal* out_pos_ptr;
324 TFeat* out_feat_ptr;
325 output_allocator.AllocPooledPositions(&out_pos_ptr, num_out);
326 output_allocator.AllocPooledFeatures(&out_feat_ptr, num_out, in_channels);
327
328 size_t i = 0;
329 for (const auto point : voxelindex_to_accpoint) {
330 Vec3_t pos = point.second.Position();
331 Eigen::Map<Vec3_t> out_pos(out_pos_ptr + i * 3);
332 out_pos = pos;
333
334 Eigen::Map<FeatureVec_t> out_feat(out_feat_ptr + i * in_channels,
335 in_channels);
336 out_feat = point.second.Features();
337 ++i;
338 }
339}
340
341// implementation for VoxelPoolingBackprop with template parameter for the
342// accumulator.
343template <class TReal, class TFeat, class ACCUMULATOR, AccumulationFn FEAT_FN>
344void _VoxelPoolingBackprop(TFeat* features_backprop,
345 size_t num_inp,
346 const TReal* const inp_positions,
347 int in_channels,
348 const TFeat* const inp_features,
349 size_t num_pooled,
350 const TReal* const pooled_positions,
351 const TFeat* const pooled_features_gradient,
352 TReal voxel_size) {
353 if (num_inp == 0) {
354 return;
355 }
356 memset(features_backprop, 0, sizeof(TFeat) * num_inp * in_channels);
357
358 typedef Eigen::Array<TReal, 3, 1> Vec3_t;
359 typedef Eigen::Array<TFeat, Eigen::Dynamic, 1> FeatureVec_t;
360
361 Vec3_t voxel_size3(voxel_size, voxel_size, voxel_size);
362
363 // create the two hash maps in parallel
364 tbb::task_group task_group;
365
366 std::unordered_map<Eigen::Vector3i, ACCUMULATOR,
368 voxelindex_to_accpoint;
369
370 task_group.run([&] {
371 Vec3_t voxel_center;
372 Eigen::Vector3i voxel_index;
373 TReal inv_voxel_size = 1 / voxel_size;
374 TReal half_voxel_size = 0.5 * voxel_size;
375 for (size_t i = 0; i < num_inp; ++i) {
376 Eigen::Map<const Vec3_t> pos(inp_positions + i * 3);
377
378 voxel_index = ComputeVoxelIndex(pos, inv_voxel_size);
379
380 voxel_center << voxel_index(0) * voxel_size + half_voxel_size,
381 voxel_index(1) * voxel_size + half_voxel_size,
382 voxel_index(2) * voxel_size + half_voxel_size;
383
384 Eigen::Map<const FeatureVec_t> feat(inp_features + in_channels * i,
385 in_channels);
386 voxelindex_to_accpoint[voxel_index].AddPoint(
387 pos.matrix(), voxel_center.matrix(), feat, i);
388 }
389 });
390
391 std::unordered_map<Eigen::Vector3i, size_t,
393 voxelindex_to_gradindex;
394
395 task_group.run([&] {
396 Eigen::Vector3i voxel_index;
397 TReal inv_voxel_size = 1 / voxel_size;
398 for (size_t i = 0; i < num_pooled; ++i) {
399 Eigen::Map<const Vec3_t> pos(pooled_positions + i * 3);
400
401 voxel_index = ComputeVoxelIndex(pos, inv_voxel_size);
402
403 voxelindex_to_gradindex[voxel_index] = i;
404 }
405 });
406
407 task_group.wait();
408
409 if (FEAT_FN == AVERAGE) {
410 Eigen::Vector3i voxel_index;
411 TReal inv_voxel_size = 1 / voxel_size;
412 for (size_t i = 0; i < num_inp; ++i) {
413 Eigen::Map<const Vec3_t> pos(inp_positions + i * 3);
414
415 voxel_index = ComputeVoxelIndex(pos, inv_voxel_size);
416
417 Eigen::Map<FeatureVec_t> feat_bp(
418 features_backprop + in_channels * i, in_channels);
419
420 size_t grad_idx = voxelindex_to_gradindex[voxel_index];
421 int count = voxelindex_to_accpoint[voxel_index].Count();
422 Eigen::Map<const FeatureVec_t> grad(
423 pooled_features_gradient + in_channels * grad_idx,
424 in_channels);
425 feat_bp = grad / count;
426 }
427 }
428
429 if (FEAT_FN == NEAREST_NEIGHBOR) {
430 for (const auto point : voxelindex_to_accpoint) {
431 size_t idx = point.second.Index()(0);
432 Eigen::Map<FeatureVec_t> feat_bp(
433 features_backprop + in_channels * idx, in_channels);
434
435 size_t grad_idx = voxelindex_to_gradindex[point.first];
436 Eigen::Map<const FeatureVec_t> grad(
437 pooled_features_gradient + in_channels * grad_idx,
438 in_channels);
439 feat_bp = grad;
440 }
441 }
442
443 if (FEAT_FN == MAX) {
444 for (const auto point : voxelindex_to_accpoint) {
445 size_t grad_idx = voxelindex_to_gradindex[point.first];
446 Eigen::Map<const FeatureVec_t> grad(
447 pooled_features_gradient + in_channels * grad_idx,
448 in_channels);
449 for (int i = 0; i < in_channels; ++i) {
450 size_t idx = point.second.Index()(i);
451 Eigen::Map<FeatureVec_t> feat_bp(
452 features_backprop + in_channels * idx, in_channels);
453 feat_bp(i) = grad(i);
454 }
455 }
456 }
457}
458
503template <class TReal, class TFeat, class OUTPUT_ALLOCATOR>
504void VoxelPooling(size_t num_inp,
505 const TReal* const inp_positions,
506 int in_channels,
507 const TFeat* inp_features,
508 TReal voxel_size,
509 OUTPUT_ALLOCATOR& output_allocator,
510 AccumulationFn position_fn,
511 AccumulationFn feature_fn) {
512#define CALL_TEMPLATE(POS_FN, FEAT_FN) \
513 if (POS_FN == position_fn && FEAT_FN == feature_fn) { \
514 _VoxelPooling<TReal, TFeat, \
515 Accumulator<TReal, TFeat, POS_FN, FEAT_FN>>( \
516 num_inp, inp_positions, in_channels, inp_features, voxel_size, \
517 output_allocator); \
518 }
519
529
530#undef CALL_TEMPLATE
531}
532
548template <class TReal, class TFeat>
549void VoxelPoolingBackprop(TFeat* features_backprop,
550 size_t num_inp,
551 const TReal* const inp_positions,
552 int in_channels,
553 const TFeat* const inp_features,
554 size_t num_pooled,
555 const TReal* const pooled_positions,
556 const TFeat* const pooled_features_gradient,
557 TReal voxel_size,
558 AccumulationFn position_fn,
559 AccumulationFn feature_fn) {
560#define CALL_TEMPLATE(POS_FN, FEAT_FN) \
561 if (POS_FN == position_fn && FEAT_FN == feature_fn) { \
562 _VoxelPoolingBackprop< \
563 TReal, TFeat, \
564 AccumulatorBackprop<TReal, TFeat, POS_FN, FEAT_FN>, FEAT_FN>( \
565 features_backprop, num_inp, inp_positions, in_channels, \
566 inp_features, num_pooled, pooled_positions, \
567 pooled_features_gradient, voxel_size); \
568 }
569
579
580#undef CALL_TEMPLATE
581}
582
583} // namespace impl
584} // namespace ml
585} // namespace open3d
#define CALL_TEMPLATE(POS_FN, FEAT_FN)
Definition: VoxelPooling.h:123
Eigen::Array< TReal, 3, 1 > Position() const
Definition: VoxelPooling.h:190
void AddPoint(const Eigen::MatrixBase< Derived > &pos, const Eigen::MatrixBase< Derived2 > &voxel_center, const Eigen::ArrayBase< Derived3 > &feat, const size_t idx)
Definition: VoxelPooling.h:135
Eigen::Array< size_t, Eigen::Dynamic, 1 > Index() const
Definition: VoxelPooling.h:210
int Count() const
Definition: VoxelPooling.h:208
AccumulatorBackprop()
Definition: VoxelPooling.h:125
Eigen::Array< TFeat, Eigen::Dynamic, 1 > Features() const
Definition: VoxelPooling.h:199
Definition: VoxelPooling.h:46
int Count() const
Definition: VoxelPooling.h:110
void AddPoint(const Eigen::MatrixBase< Derived > &pos, const Eigen::MatrixBase< Derived2 > &voxel_center, const Eigen::ArrayBase< Derived3 > &feat)
Definition: VoxelPooling.h:58
Eigen::Array< TReal, 3, 1 > Position() const
Definition: VoxelPooling.h:92
Eigen::Array< TFeat, Eigen::Dynamic, 1 > Features() const
Definition: VoxelPooling.h:101
Accumulator()
Definition: VoxelPooling.h:48
int count
Definition: FilePCD.cpp:61
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample playback_handle k4a_logging_message_cb_t void min_level device_handle k4a_imu_sample_t timeout_in_ms capture_handle capture_handle capture_handle image_handle temperature_c k4a_image_t image_handle uint8_t image_handle image_handle image_handle image_handle image_handle timestamp_usec white_balance image_handle k4a_device_configuration_t config device_handle char size_t serial_number_size bool int32_t int32_t int32_t int32_t k4a_color_control_mode_t default_mode value const const k4a_calibration_t calibration char size_t
Definition: K4aPlugin.cpp:738
AccumulationFn
Definition: VoxelPooling.h:40
@ CENTER
Definition: VoxelPooling.h:40
@ NEAREST_NEIGHBOR
Definition: VoxelPooling.h:40
@ MAX
Definition: VoxelPooling.h:40
@ AVERAGE
Definition: VoxelPooling.h:40
void VoxelPoolingBackprop(TFeat *features_backprop, size_t num_inp, const TReal *const inp_positions, int in_channels, const TFeat *const inp_features, size_t num_pooled, const TReal *const pooled_positions, const TFeat *const pooled_features_gradient, TReal voxel_size, AccumulationFn position_fn, AccumulationFn feature_fn)
Definition: VoxelPooling.h:549
bool CheckVoxelSize(std::string &err, const size_t num_positions, const T *const positions, const T voxel_size)
Function for debugging. Checks if the voxel size is too small.
Definition: VoxelPooling.h:224
void VoxelPooling(size_t num_inp, const TReal *const inp_positions, int in_channels, const TFeat *inp_features, TReal voxel_size, OUTPUT_ALLOCATOR &output_allocator, AccumulationFn position_fn, AccumulationFn feature_fn)
Definition: VoxelPooling.h:504
void _VoxelPoolingBackprop(TFeat *features_backprop, size_t num_inp, const TReal *const inp_positions, int in_channels, const TFeat *const inp_features, size_t num_pooled, const TReal *const pooled_positions, const TFeat *const pooled_features_gradient, TReal voxel_size)
Definition: VoxelPooling.h:344
void _VoxelPooling(size_t num_inp, const TReal *const inp_positions, int in_channels, const TFeat *inp_features, TReal voxel_size, OUTPUT_ALLOCATOR &output_allocator)
Definition: VoxelPooling.h:281
HOST_DEVICE utility::MiniVec< int, 3 > ComputeVoxelIndex(const TVecf &pos, const typename TVecf::Scalar_t &inv_voxel_size)
Definition: NeighborSearchCommon.h:61
Definition: PinholeCameraIntrinsic.cpp:35
Definition: Device.h:126
Definition: Helper.h:90