29 #include <tbb/task_group.h> 32 #include <unordered_map> 42 template <
class TReal,
50 min_sqr_dist_to_center_(
std::numeric_limits<TReal>::max()),
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");
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;
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;
71 position_ += pos.array();
74 }
else if (POS_FN == CENTER) {
75 if (count_ == 0) position_ = voxel_center;
79 features_.resizeLike(feat);
86 }
else if (FEAT_FN ==
MAX) {
87 features_ = features_.max(feat);
92 inline Eigen::Array<TReal, 3, 1>
Position()
const {
94 return position_ / count_;
101 inline Eigen::Array<TFeat, Eigen::Dynamic, 1>
Features()
const {
103 return features_ / count_;
110 inline int Count()
const {
return count_; }
114 TReal min_sqr_dist_to_center_;
115 Eigen::Array<TReal, 3, 1> position_;
116 Eigen::Array<TFeat, Eigen::Dynamic, 1> features_;
119 template <
class TReal,
127 min_sqr_dist_to_center_(
std::numeric_limits<TReal>::max()),
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");
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,
139 bool new_nearest_neighbor =
false;
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;
150 position_ += pos.array();
153 }
else if (POS_FN == CENTER) {
154 if (count_ == 0) position_ = voxel_center;
158 features_.resizeLike(feat);
166 }
else if (FEAT_FN ==
MAX) {
168 index_.resizeLike(feat);
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);
190 inline Eigen::Array<TReal, 3, 1>
Position()
const {
192 return position_ / count_;
199 inline Eigen::Array<TFeat, Eigen::Dynamic, 1>
Features()
const {
201 return features_ / count_;
208 inline int Count()
const {
return count_; }
210 inline Eigen::Array<size_t, Eigen::Dynamic, 1>
Index()
const {
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_;
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) {
233 Vec3_t bb_min, bb_max;
234 bb_min << positions[0], positions[1], positions[2];
237 Vec3_t voxel_size3(voxel_size, voxel_size, voxel_size);
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);
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;
252 if (voxel_size *
double(std::numeric_limits<int>::max()) <
254 voxel_size * double(std::numeric_limits<int>::min()) >
256 err =
"voxel_size is too small\n";
267 template <
class TDerived>
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;
274 Eigen::Vector3i voxel_index;
275 voxel_index = ref_coord.floor().template cast<int>();
280 template <
class TReal,
class TFeat,
class ACCUMULATOR,
class OUTPUT_ALLOCATOR>
282 const TReal*
const inp_positions,
284 const TFeat* inp_features,
286 OUTPUT_ALLOCATOR& output_allocator) {
290 output_allocator.AllocPooledPositions(&out_pos_ptr, 0);
291 output_allocator.AllocPooledFeatures(&out_feat_ptr, 0, in_channels);
295 typedef Eigen::Array<TReal, 3, 1> Vec3_t;
296 typedef Eigen::Array<TFeat, Eigen::Dynamic, 1> FeatureVec_t;
298 std::unordered_map<Eigen::Vector3i, ACCUMULATOR,
300 voxelindex_to_accpoint;
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);
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;
315 Eigen::Map<const FeatureVec_t> feat(inp_features + in_channels * i,
317 voxelindex_to_accpoint[voxel_index].AddPoint(
318 pos.matrix(), voxel_center.matrix(), feat);
321 const size_t num_out = voxelindex_to_accpoint.size();
325 output_allocator.AllocPooledPositions(&out_pos_ptr, num_out);
326 output_allocator.AllocPooledFeatures(&out_feat_ptr, num_out, in_channels);
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);
334 Eigen::Map<FeatureVec_t> out_feat(out_feat_ptr + i * in_channels,
336 out_feat = point.second.Features();
343 template <
class TReal,
class TFeat,
class ACCUMULATOR, AccumulationFn FEAT_FN>
346 const TReal*
const inp_positions,
348 const TFeat*
const inp_features,
350 const TReal*
const pooled_positions,
351 const TFeat*
const pooled_features_gradient,
356 memset(features_backprop, 0,
sizeof(TFeat) * num_inp * in_channels);
358 typedef Eigen::Array<TReal, 3, 1> Vec3_t;
359 typedef Eigen::Array<TFeat, Eigen::Dynamic, 1> FeatureVec_t;
361 Vec3_t voxel_size3(voxel_size, voxel_size, voxel_size);
364 tbb::task_group task_group;
366 std::unordered_map<Eigen::Vector3i, ACCUMULATOR,
368 voxelindex_to_accpoint;
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);
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;
384 Eigen::Map<const FeatureVec_t> feat(inp_features + in_channels * i,
386 voxelindex_to_accpoint[voxel_index].AddPoint(
387 pos.matrix(), voxel_center.matrix(), feat, i);
391 std::unordered_map<Eigen::Vector3i,
size_t,
393 voxelindex_to_gradindex;
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);
403 voxelindex_to_gradindex[voxel_index] = i;
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);
417 Eigen::Map<FeatureVec_t> feat_bp(
418 features_backprop + in_channels * i, in_channels);
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,
425 feat_bp = grad /
count;
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);
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,
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,
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);
503 template <
class TReal,
class TFeat,
class OUTPUT_ALLOCATOR>
505 const TReal*
const inp_positions,
507 const TFeat* inp_features,
509 OUTPUT_ALLOCATOR& output_allocator,
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, \ 548 template <
class TReal,
class TFeat>
551 const TReal*
const inp_positions,
553 const TFeat*
const inp_features,
555 const TReal*
const pooled_positions,
556 const TFeat*
const pooled_features_gradient,
560 #define CALL_TEMPLATE(POS_FN, FEAT_FN) \ 561 if (POS_FN == position_fn && FEAT_FN == feature_fn) { \ 562 _VoxelPoolingBackprop< \ 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); \ 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
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