Open3D (C++ API)  0.12.0
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) 2020 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 
34 #include "open3d/utility/Helper.h"
35 
36 namespace open3d {
37 namespace ml {
38 namespace impl {
39 
41 
42 template <class TReal,
43  class TFeat,
44  AccumulationFn POS_FN,
45  AccumulationFn FEAT_FN>
46 class Accumulator {
47 public:
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 
112 private:
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 
119 template <class TReal,
120  class TFeat,
121  AccumulationFn POS_FN,
122  AccumulationFn FEAT_FN>
124 public:
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 
214 private:
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 
223 template <class T>
224 bool 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 
267 template <class TDerived>
268 Eigen::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.
280 template <class TReal, class TFeat, class ACCUMULATOR, class OUTPUT_ALLOCATOR>
281 void _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.
343 template <class TReal, class TFeat, class ACCUMULATOR, AccumulationFn FEAT_FN>
344 void _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 
503 template <class TReal, class TFeat, class OUTPUT_ALLOCATOR>
504 void 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 
526  CALL_TEMPLATE(CENTER, AVERAGE)
528  CALL_TEMPLATE(CENTER, MAX)
529 
530 #undef CALL_TEMPLATE
531 }
532 
548 template <class TReal, class TFeat>
549 void 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 
576  CALL_TEMPLATE(CENTER, AVERAGE)
578  CALL_TEMPLATE(CENTER, MAX)
579 
580 #undef CALL_TEMPLATE
581 }
582 
583 } // namespace impl
584 } // namespace ml
585 } // namespace open3d
Definition: VoxelPooling.h:40
Eigen::Array< TFeat, Eigen::Dynamic, 1 > Features() const
Definition: VoxelPooling.h:101
Definition: VoxelPooling.h:40
Eigen::Array< TFeat, Eigen::Dynamic, 1 > Features() const
Definition: VoxelPooling.h:199
Eigen::Array< TReal, 3, 1 > Position() const
Definition: VoxelPooling.h:92
#define CALL_TEMPLATE(POS_FN, FEAT_FN)
Definition: Optional.h:912
Definition: VoxelPooling.h:40
HOST_DEVICE utility::MiniVec< int, 3 > ComputeVoxelIndex(const TVecf &pos, const typename TVecf::Scalar_t &inv_voxel_size)
Definition: NeighborSearchCommon.h:61
Definition: VoxelPooling.h:46
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
int Count() const
Definition: VoxelPooling.h:110
Definition: Helper.h:86
int Count() const
Definition: VoxelPooling.h:208
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 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
AccumulationFn
Definition: VoxelPooling.h:40
Definition: VoxelPooling.h:123
void AddPoint(const Eigen::MatrixBase< Derived > &pos, const Eigen::MatrixBase< Derived2 > &voxel_center, const Eigen::ArrayBase< Derived3 > &feat)
Definition: VoxelPooling.h:58
int count
Definition: FilePCD.cpp:61
Definition: VoxelPooling.h:40
Definition: PinholeCameraIntrinsic.cpp:35
Accumulator()
Definition: VoxelPooling.h:48
Eigen::Array< size_t, Eigen::Dynamic, 1 > Index() const
Definition: VoxelPooling.h:210
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
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:724
AccumulatorBackprop()
Definition: VoxelPooling.h:125
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 _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
Eigen::Array< TReal, 3, 1 > Position() const
Definition: VoxelPooling.h:190