Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
surface_normal_modality.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
40#include <pcl/recognition/quantizable_modality.h>
41#include <pcl/recognition/distance_map.h>
42
43#include <pcl/pcl_base.h>
44#include <pcl/point_cloud.h>
45#include <pcl/point_types.h>
46#include <pcl/features/linear_least_squares_normal.h>
47
48#include <algorithm>
49#include <cmath>
50#include <cstddef>
51// #include <iostream>
52#include <limits>
53#include <list>
54#include <vector>
55
56namespace pcl
57{
58
59 /** \brief Map that stores orientations.
60 * \author Stefan Holzer
61 */
62 struct PCL_EXPORTS LINEMOD_OrientationMap
63 {
64 public:
65 /** \brief Constructor. */
66 inline LINEMOD_OrientationMap () : width_ (0), height_ (0) {}
67 /** \brief Destructor. */
68 inline ~LINEMOD_OrientationMap () = default;
69
70 /** \brief Returns the width of the modality data map. */
71 inline std::size_t
72 getWidth () const
73 {
74 return width_;
75 }
76
77 /** \brief Returns the height of the modality data map. */
78 inline std::size_t
79 getHeight () const
80 {
81 return height_;
82 }
83
84 /** \brief Resizes the map to the specific width and height and initializes
85 * all new elements with the specified value.
86 * \param[in] width the width of the resized map.
87 * \param[in] height the height of the resized map.
88 * \param[in] value the value all new elements will be initialized with.
89 */
90 inline void
91 resize (const std::size_t width, const std::size_t height, const float value)
92 {
93 width_ = width;
94 height_ = height;
95 map_.clear ();
96 map_.resize (width*height, value);
97 }
98
99 /** \brief Operator to access elements of the map.
100 * \param[in] col_index the column index of the element to access.
101 * \param[in] row_index the row index of the element to access.
102 */
103 inline float &
104 operator() (const std::size_t col_index, const std::size_t row_index)
105 {
106 return map_[row_index * width_ + col_index];
107 }
108
109 /** \brief Operator to access elements of the map.
110 * \param[in] col_index the column index of the element to access.
111 * \param[in] row_index the row index of the element to access.
112 */
113 inline const float &
114 operator() (const std::size_t col_index, const std::size_t row_index) const
115 {
116 return map_[row_index * width_ + col_index];
117 }
118
119 private:
120 /** \brief The width of the map. */
121 std::size_t width_;
122 /** \brief The height of the map. */
123 std::size_t height_;
124 /** \brief Storage for the data of the map. */
125 std::vector<float> map_;
126
127 };
128
129 /** \brief Look-up-table for fast surface normal quantization.
130 * \author Stefan Holzer
131 */
133 {
134 /** \brief The range of the LUT in x-direction. */
136 /** \brief The range of the LUT in y-direction. */
138 /** \brief The range of the LUT in z-direction. */
140
141 /** \brief The offset in x-direction. */
143 /** \brief The offset in y-direction. */
145 /** \brief The offset in z-direction. */
147
148 /** \brief The size of the LUT in x-direction. */
150 /** \brief The size of the LUT in y-direction. */
152 /** \brief The size of the LUT in z-direction. */
154
155 /** \brief The LUT data. */
156 unsigned char * lut;
157
158 /** \brief Constructor. */
160 range_x (-1), range_y (-1), range_z (-1),
161 offset_x (-1), offset_y (-1), offset_z (-1),
162 size_x (-1), size_y (-1), size_z (-1), lut (nullptr)
163 {}
164
165 /** \brief Destructor. */
167 {
168 delete[] lut;
169 }
170
171 /** \brief Initializes the LUT.
172 * \param[in] range_x_arg the range of the LUT in x-direction.
173 * \param[in] range_y_arg the range of the LUT in y-direction.
174 * \param[in] range_z_arg the range of the LUT in z-direction.
175 */
176 void
177 initializeLUT (const int range_x_arg, const int range_y_arg, const int range_z_arg)
178 {
179 range_x = range_x_arg;
180 range_y = range_y_arg;
181 range_z = range_z_arg;
182 size_x = range_x_arg+1;
183 size_y = range_y_arg+1;
184 size_z = range_z_arg+1;
185 offset_x = range_x_arg/2;
186 offset_y = range_y_arg/2;
187 offset_z = range_z_arg;
188
189 //if (lut != NULL) free16(lut);
190 //lut = malloc16(size_x*size_y*size_z);
191
192 delete[] lut;
193 lut = new unsigned char[size_x*size_y*size_z];
194
195 constexpr int nr_normals = 8;
196 pcl::PointCloud<PointXYZ>::VectorType ref_normals (nr_normals);
197
198 constexpr float normal0_angle = 40.0f * 3.14f / 180.0f;
199 ref_normals[0].x = std::cos (normal0_angle);
200 ref_normals[0].y = 0.0f;
201 ref_normals[0].z = -sinf (normal0_angle);
202
203 constexpr float inv_nr_normals = 1.0f / static_cast<float>(nr_normals);
204 for (int normal_index = 1; normal_index < nr_normals; ++normal_index)
205 {
206 const float angle = 2.0f * static_cast<float> (M_PI * normal_index * inv_nr_normals);
207
208 ref_normals[normal_index].x = std::cos (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y;
209 ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + std::cos (angle) * ref_normals[0].y;
210 ref_normals[normal_index].z = ref_normals[0].z;
211 }
212
213 // normalize normals
214 for (int normal_index = 0; normal_index < nr_normals; ++normal_index)
215 {
216 const float length = std::sqrt (ref_normals[normal_index].x * ref_normals[normal_index].x +
217 ref_normals[normal_index].y * ref_normals[normal_index].y +
218 ref_normals[normal_index].z * ref_normals[normal_index].z);
219
220 const float inv_length = 1.0f / length;
221
222 ref_normals[normal_index].x *= inv_length;
223 ref_normals[normal_index].y *= inv_length;
224 ref_normals[normal_index].z *= inv_length;
225 }
226
227 // set LUT
228 for (int z_index = 0; z_index < size_z; ++z_index)
229 {
230 for (int y_index = 0; y_index < size_y; ++y_index)
231 {
232 for (int x_index = 0; x_index < size_x; ++x_index)
233 {
234 PointXYZ normal (static_cast<float> (x_index - range_x/2),
235 static_cast<float> (y_index - range_y/2),
236 static_cast<float> (z_index - range_z));
237 const float length = std::sqrt (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
238 const float inv_length = 1.0f / (length + 0.00001f);
239
240 normal.x *= inv_length;
241 normal.y *= inv_length;
242 normal.z *= inv_length;
243
244 float max_response = -1.0f;
245 int max_index = -1;
246
247 for (int normal_index = 0; normal_index < nr_normals; ++normal_index)
248 {
249 const float response = normal.x * ref_normals[normal_index].x +
250 normal.y * ref_normals[normal_index].y +
251 normal.z * ref_normals[normal_index].z;
252
253 const float abs_response = std::abs (response);
254 if (max_response < abs_response)
255 {
256 max_response = abs_response;
257 max_index = normal_index;
258 }
259
260 lut[z_index*size_y*size_x + y_index*size_x + x_index] = static_cast<unsigned char> (0x1 << max_index);
261 }
262 }
263 }
264 }
265 }
266
267 /** \brief Operator to access an element in the LUT.
268 * \param[in] x the x-component of the normal.
269 * \param[in] y the y-component of the normal.
270 * \param[in] z the z-component of the normal.
271 */
272 inline unsigned char
273 operator() (const float x, const float y, const float z) const
274 {
275 const auto x_index = static_cast<std::size_t> (x * static_cast<float> (offset_x) + static_cast<float> (offset_x));
276 const auto y_index = static_cast<std::size_t> (y * static_cast<float> (offset_y) + static_cast<float> (offset_y));
277 const auto z_index = static_cast<std::size_t> (z * static_cast<float> (range_z) + static_cast<float> (range_z));
278
279 const std::size_t index = z_index*size_y*size_x + y_index*size_x + x_index;
280
281 return (lut[index]);
282 }
283
284 /** \brief Operator to access an element in the LUT.
285 * \param[in] index the index of the element.
286 */
287 inline unsigned char
288 operator() (const int index) const
289 {
290 return (lut[index]);
291 }
292 };
293
294
295 /** \brief Modality based on surface normals.
296 * \author Stefan Holzer
297 * \ingroup recognition
298 */
299 template <typename PointInT>
300 class SurfaceNormalModality : public QuantizableModality, public PCLBase<PointInT>
301 {
302 protected:
303 using PCLBase<PointInT>::input_;
304
305 /** \brief Candidate for a feature (used in feature extraction methods). */
307 {
308 /** \brief Constructor. */
309 Candidate () : distance (0.0f), bin_index (0), x (0), y (0) {}
310
311 /** \brief Normal. */
313 /** \brief Distance to the next different quantized value. */
314 float distance;
315
316 /** \brief Quantized value. */
317 unsigned char bin_index;
318
319 /** \brief x-position of the feature. */
320 std::size_t x;
321 /** \brief y-position of the feature. */
322 std::size_t y;
323
324 /** \brief Compares two candidates based on their distance to the next different quantized value.
325 * \param[in] rhs the candidate to compare with.
326 */
327 bool
328 operator< (const Candidate & rhs) const
329 {
330 return (distance > rhs.distance);
331 }
332 };
333
334 public:
336
337 /** \brief Constructor. */
339 /** \brief Destructor. */
341
342 /** \brief Sets the spreading size.
343 * \param[in] spreading_size the spreading size.
344 */
345 inline void
346 setSpreadingSize (const std::size_t spreading_size)
347 {
348 spreading_size_ = spreading_size;
349 }
350
351 /** \brief Enables/disables the use of extracting a variable number of features.
352 * \param[in] enabled specifies whether extraction of a variable number of features will be enabled/disabled.
353 */
354 inline void
355 setVariableFeatureNr (const bool enabled)
356 {
357 variable_feature_nr_ = enabled;
358 }
359
360 /** \brief Returns the surface normals. */
363 {
364 return surface_normals_;
365 }
366
367 /** \brief Returns the surface normals. */
368 inline const pcl::PointCloud<pcl::Normal> &
370 {
371 return surface_normals_;
372 }
373
374 /** \brief Returns a reference to the internal quantized map. */
375 inline QuantizedMap &
376 getQuantizedMap () override
377 {
378 return (filtered_quantized_surface_normals_);
379 }
380
381 /** \brief Returns a reference to the internal spread quantized map. */
382 inline QuantizedMap &
384 {
385 return (spreaded_quantized_surface_normals_);
386 }
387
388 /** \brief Returns a reference to the orientation map. */
391 {
392 return (surface_normal_orientations_);
393 }
394
395 /** \brief Extracts features from this modality within the specified mask.
396 * \param[in] mask defines the areas where features are searched in.
397 * \param[in] nr_features defines the number of features to be extracted
398 * (might be less if not sufficient information is present in the modality).
399 * \param[in] modality_index the index which is stored in the extracted features.
400 * \param[out] features the destination for the extracted features.
401 */
402 void
403 extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index,
404 std::vector<QuantizedMultiModFeature> & features) const override;
405
406 /** \brief Extracts all possible features from the modality within the specified mask.
407 * \param[in] mask defines the areas where features are searched in.
408 * \param[in] nr_features IGNORED (TODO: remove this parameter).
409 * \param[in] modality_index the index which is stored in the extracted features.
410 * \param[out] features the destination for the extracted features.
411 */
412 void
413 extractAllFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index,
414 std::vector<QuantizedMultiModFeature> & features) const override;
415
416 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
417 * \param[in] cloud the const boost shared pointer to a PointCloud message
418 */
419 void
420 setInputCloud (const typename PointCloudIn::ConstPtr & cloud) override
421 {
422 input_ = cloud;
423 }
424
425 /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */
426 virtual void
428
429 /** \brief Processes the input data assuming that everything up to filtering is already done/available
430 * (so only spreading is performed). */
431 virtual void
433
434 protected:
435
436 /** \brief Computes the surface normals from the input cloud. */
437 void
439
440 /** \brief Computes and quantizes the surface normals. */
441 void
443
444 /** \brief Computes and quantizes the surface normals. */
445 void
447
448 /** \brief Quantizes the surface normals. */
449 void
451
452 /** \brief Filters the quantized surface normals. */
453 void
455
456 /** \brief Computes a distance map from the supplied input mask.
457 * \param[in] input the mask for which a distance map will be computed.
458 * \param[out] output the destination for the distance map.
459 */
460 void
461 computeDistanceMap (const MaskMap & input, DistanceMap & output) const;
462
463 private:
464
465 /** \brief Determines whether variable numbers of features are extracted or not. */
466 bool variable_feature_nr_;
467
468 /** \brief The feature distance threshold. */
469 float feature_distance_threshold_;
470 /** \brief Minimum distance of a feature to a border. */
471 float min_distance_to_border_;
472
473 /** \brief Look-up-table for quantizing surface normals. */
474 QuantizedNormalLookUpTable normal_lookup_;
475
476 /** \brief The spreading size. */
477 std::size_t spreading_size_;
478
479 /** \brief Point cloud holding the computed surface normals. */
480 pcl::PointCloud<pcl::Normal> surface_normals_;
481 /** \brief Quantized surface normals. */
482 pcl::QuantizedMap quantized_surface_normals_;
483 /** \brief Filtered quantized surface normals. */
484 pcl::QuantizedMap filtered_quantized_surface_normals_;
485 /** \brief Spread quantized surface normals. */
486 pcl::QuantizedMap spreaded_quantized_surface_normals_;
487
488 /** \brief Map containing surface normal orientations. */
489 pcl::LINEMOD_OrientationMap surface_normal_orientations_;
490
491 };
492
493}
494
495//////////////////////////////////////////////////////////////////////////////////////////////
496template <typename PointInT>
499 : variable_feature_nr_ (false)
500 , feature_distance_threshold_ (2.0f)
501 , min_distance_to_border_ (2.0f)
502 , spreading_size_ (8)
503{
504}
505
506//////////////////////////////////////////////////////////////////////////////////////////////
507template <typename PointInT>
509
510//////////////////////////////////////////////////////////////////////////////////////////////
511template <typename PointInT> void
513{
514 // compute surface normals
515 //computeSurfaceNormals ();
516
517 // quantize surface normals
518 //quantizeSurfaceNormals ();
519
520 computeAndQuantizeSurfaceNormals2 ();
521
522 // filter quantized surface normals
523 filterQuantizedSurfaceNormals ();
524
525 // spread quantized surface normals
526 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_,
527 spreaded_quantized_surface_normals_,
528 spreading_size_);
529}
530
531//////////////////////////////////////////////////////////////////////////////////////////////
532template <typename PointInT> void
534{
535 // spread quantized surface normals
536 pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_,
537 spreaded_quantized_surface_normals_,
538 spreading_size_);
539}
540
541//////////////////////////////////////////////////////////////////////////////////////////////
542template <typename PointInT> void
544{
545 // compute surface normals
547 ne.setMaxDepthChangeFactor(0.05f);
548 ne.setNormalSmoothingSize(5.0f);
550 ne.setInputCloud (input_);
551 ne.compute (surface_normals_);
552}
553
554//////////////////////////////////////////////////////////////////////////////////////////////
555template <typename PointInT> void
557{
558 // compute surface normals
559 //pcl::LinearLeastSquaresNormalEstimation<PointInT, pcl::Normal> ne;
560 //ne.setMaxDepthChangeFactor(0.05f);
561 //ne.setNormalSmoothingSize(5.0f);
562 //ne.setDepthDependentSmoothing(false);
563 //ne.setInputCloud (input_);
564 //ne.compute (surface_normals_);
565
566
567 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
568
569 const int width = input_->width;
570 const int height = input_->height;
571
572 surface_normals_.resize (width*height);
573 surface_normals_.width = width;
574 surface_normals_.height = height;
575 surface_normals_.is_dense = false;
576
577 quantized_surface_normals_.resize (width, height);
578
579 // we compute the normals as follows:
580 // ----------------------------------
581 //
582 // for the depth-gradient you can make the following first-order Taylor approximation:
583 // D(x + dx) - D(x) = dx^T \Delta D + h.o.t.
584 //
585 // build linear system by stacking up equation for 8 neighbor points:
586 // Y = X \Delta D
587 //
588 // => \Delta D = (X^T X)^{-1} X^T Y
589 // => \Delta D = (A)^{-1} b
590
591 for (int y = 0; y < height; ++y)
592 {
593 for (int x = 0; x < width; ++x)
594 {
595 const int index = y * width + x;
596
597 const float px = (*input_)[index].x;
598 const float py = (*input_)[index].y;
599 const float pz = (*input_)[index].z;
600
601 if (std::isnan(px) || pz > 2.0f)
602 {
603 surface_normals_[index].normal_x = bad_point;
604 surface_normals_[index].normal_y = bad_point;
605 surface_normals_[index].normal_z = bad_point;
606 surface_normals_[index].curvature = bad_point;
607
608 quantized_surface_normals_ (x, y) = 0;
609
610 continue;
611 }
612
613 const int smoothingSizeInt = 5;
614
615 float matA0 = 0.0f;
616 float matA1 = 0.0f;
617 float matA3 = 0.0f;
618
619 float vecb0 = 0.0f;
620 float vecb1 = 0.0f;
621
622 for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
623 {
624 for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
625 {
626 if (u < 0 || u >= width || v < 0 || v >= height) continue;
627
628 const std::size_t index2 = v * width + u;
629
630 const float qx = (*input_)[index2].x;
631 const float qy = (*input_)[index2].y;
632 const float qz = (*input_)[index2].z;
633
634 if (std::isnan(qx)) continue;
635
636 const float delta = qz - pz;
637 const float i = qx - px;
638 const float j = qy - py;
639
640 const float f = std::abs(delta) < 0.05f ? 1.0f : 0.0f;
641
642 matA0 += f * i * i;
643 matA1 += f * i * j;
644 matA3 += f * j * j;
645 vecb0 += f * i * delta;
646 vecb1 += f * j * delta;
647 }
648 }
649
650 const float det = matA0 * matA3 - matA1 * matA1;
651 const float ddx = matA3 * vecb0 - matA1 * vecb1;
652 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
653
654 const float nx = ddx;
655 const float ny = ddy;
656 const float nz = -det * pz;
657
658 const float length = nx * nx + ny * ny + nz * nz;
659
660 if (length <= 0.0f)
661 {
662 surface_normals_[index].normal_x = bad_point;
663 surface_normals_[index].normal_y = bad_point;
664 surface_normals_[index].normal_z = bad_point;
665 surface_normals_[index].curvature = bad_point;
666
667 quantized_surface_normals_ (x, y) = 0;
668 }
669 else
670 {
671 const float normInv = 1.0f / std::sqrt (length);
672
673 const float normal_x = nx * normInv;
674 const float normal_y = ny * normInv;
675 const float normal_z = nz * normInv;
676
677 surface_normals_[index].normal_x = normal_x;
678 surface_normals_[index].normal_y = normal_y;
679 surface_normals_[index].normal_z = normal_z;
680 surface_normals_[index].curvature = bad_point;
681
682 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
683
684 if (angle < 0.0f) angle += 360.0f;
685 if (angle >= 360.0f) angle -= 360.0f;
686
687 int bin_index = static_cast<int> (angle*8.0f/360.0f + 1);
688 bin_index = (bin_index < 1) ? 1 : (8 < bin_index) ? 8 : bin_index;
689
690 quantized_surface_normals_ (x, y) = static_cast<unsigned char> (bin_index);
691 }
692 }
693 }
694}
695
696
697//////////////////////////////////////////////////////////////////////////////////////////////
698// Contains GRANULARITY and NORMAL_LUT
699//#include "normal_lut.i"
700
701static void accumBilateral(long delta, long i, long j, long * A, long * b, int threshold)
702{
703 long f = std::abs(delta) < threshold ? 1 : 0;
704
705 const long fi = f * i;
706 const long fj = f * j;
707
708 A[0] += fi * i;
709 A[1] += fi * j;
710 A[3] += fj * j;
711 b[0] += fi * delta;
712 b[1] += fj * delta;
713}
714
715/**
716 * \brief Compute quantized normal image from depth image.
717 *
718 * Implements section 2.6 "Extension to Dense Depth Sensors."
719 *
720 * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask?
721 */
722template <typename PointInT> void
724{
725 const int width = input_->width;
726 const int height = input_->height;
727
728 auto * lp_depth = new unsigned short[width*height]{};
729 auto * lp_normals = new unsigned char[width*height]{};
730
731 surface_normal_orientations_.resize (width, height, 0.0f);
732
733 for (int row_index = 0; row_index < height; ++row_index)
734 {
735 for (int col_index = 0; col_index < width; ++col_index)
736 {
737 const float value = (*input_)[row_index*width + col_index].z;
738 if (std::isfinite (value))
739 {
740 lp_depth[row_index*width + col_index] = static_cast<unsigned short> (value * 1000.0f);
741 }
742 else
743 {
744 lp_depth[row_index*width + col_index] = 0;
745 }
746 }
747 }
748
749 const int l_W = width;
750 const int l_H = height;
751
752 constexpr int l_r = 5; // used to be 7
753 //const int l_offset0 = -l_r - l_r * l_W;
754 //const int l_offset1 = 0 - l_r * l_W;
755 //const int l_offset2 = +l_r - l_r * l_W;
756 //const int l_offset3 = -l_r;
757 //const int l_offset4 = +l_r;
758 //const int l_offset5 = -l_r + l_r * l_W;
759 //const int l_offset6 = 0 + l_r * l_W;
760 //const int l_offset7 = +l_r + l_r * l_W;
761
762 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
763 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
764 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
765 , offsets_i[1] + offsets_j[1] * l_W
766 , offsets_i[2] + offsets_j[2] * l_W
767 , offsets_i[3] + offsets_j[3] * l_W
768 , offsets_i[4] + offsets_j[4] * l_W
769 , offsets_i[5] + offsets_j[5] * l_W
770 , offsets_i[6] + offsets_j[6] * l_W
771 , offsets_i[7] + offsets_j[7] * l_W };
772
773
774 //const int l_offsetx = GRANULARITY / 2;
775 //const int l_offsety = GRANULARITY / 2;
776
777 constexpr int difference_threshold = 50;
778 constexpr int distance_threshold = 2000;
779
780 //const double scale = 1000.0;
781 //const double difference_threshold = 0.05 * scale;
782 //const double distance_threshold = 2.0 * scale;
783
784 for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
785 {
786 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
787 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
788
789 for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
790 {
791 long l_d = lp_line[0];
792 //float l_d = (*input_)[(l_y * l_W + l_r) + l_x].z;
793 //float px = (*input_)[(l_y * l_W + l_r) + l_x].x;
794 //float py = (*input_)[(l_y * l_W + l_r) + l_x].y;
795
796 if (l_d < distance_threshold)
797 {
798 // accum
799 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
800 long l_b[2]; l_b[0] = l_b[1] = 0;
801 //double l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
802 //double l_b[2]; l_b[0] = l_b[1] = 0;
803
804 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
805 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
806 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
807 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
808 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
809 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
810 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
811 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
812
813 //for (std::size_t index = 0; index < 8; ++index)
814 //{
815 // //accumBilateral(lp_line[offsets[index]] - l_d, offsets_i[index], offsets_j[index], l_A, l_b, difference_threshold);
816
817 // //const long delta = lp_line[offsets[index]] - l_d;
818 // //const long i = offsets_i[index];
819 // //const long j = offsets_j[index];
820 // //long * A = l_A;
821 // //long * b = l_b;
822 // //const int threshold = difference_threshold;
823
824 // //const long f = std::abs(delta) < threshold ? 1 : 0;
825
826 // //const long fi = f * i;
827 // //const long fj = f * j;
828
829 // //A[0] += fi * i;
830 // //A[1] += fi * j;
831 // //A[3] += fj * j;
832 // //b[0] += fi * delta;
833 // //b[1] += fj * delta;
834
835
836 // const double delta = 1000.0f * ((*input_)[(l_y * l_W + l_r) + l_x + offsets[index]].z - l_d);
837 // const double i = offsets_i[index];
838 // const double j = offsets_j[index];
839 // //const float i = (*input_)[(l_y * l_W + l_r) + l_x + offsets[index]].x - px;//offsets_i[index];
840 // //const float j = (*input_)[(l_y * l_W + l_r) + l_x + offsets[index]].y - py;//offsets_j[index];
841 // double * A = l_A;
842 // double * b = l_b;
843 // const double threshold = difference_threshold;
844
845 // const double f = std::fabs(delta) < threshold ? 1.0f : 0.0f;
846
847 // const double fi = f * i;
848 // const double fj = f * j;
849
850 // A[0] += fi * i;
851 // A[1] += fi * j;
852 // A[3] += fj * j;
853 // b[0] += fi * delta;
854 // b[1] += fj * delta;
855 //}
856
857 //long f = std::abs(delta) < threshold ? 1 : 0;
858
859 //const long fi = f * i;
860 //const long fj = f * j;
861
862 //A[0] += fi * i;
863 //A[1] += fi * j;
864 //A[3] += fj * j;
865 //b[0] += fi * delta;
866 //b[1] += fj * delta;
867
868
869 // solve
870 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
871 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
872 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
873
874 /// @todo Magic number 1150 is focal length? This is something like
875 /// f in SXGA mode, but in VGA is more like 530.
876 float l_nx = static_cast<float>(1150 * l_ddx);
877 float l_ny = static_cast<float>(1150 * l_ddy);
878 float l_nz = static_cast<float>(-l_det * l_d);
879
880 //// solve
881 //double l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
882 //double l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
883 //double l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
884
885 ///// @todo Magic number 1150 is focal length? This is something like
886 ///// f in SXGA mode, but in VGA is more like 530.
887 //const double dummy_focal_length = 1150.0f;
888 //double l_nx = l_ddx * dummy_focal_length;
889 //double l_ny = l_ddy * dummy_focal_length;
890 //double l_nz = -l_det * l_d;
891
892 float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
893
894 if (l_sqrt > 0)
895 {
896 float l_norminv = 1.0f / (l_sqrt);
897
898 l_nx *= l_norminv;
899 l_ny *= l_norminv;
900 l_nz *= l_norminv;
901
902 float angle = 11.25f + std::atan2 (l_ny, l_nx) * 180.0f / 3.14f;
903
904 if (angle < 0.0f) angle += 360.0f;
905 if (angle >= 360.0f) angle -= 360.0f;
906
907 int bin_index = static_cast<int> (angle*8.0f/360.0f);
908
909 surface_normal_orientations_ (l_x, l_y) = angle;
910
911 //*lp_norm = std::abs(l_nz)*255;
912
913 //int l_val1 = static_cast<int>(l_nx * l_offsetx + l_offsetx);
914 //int l_val2 = static_cast<int>(l_ny * l_offsety + l_offsety);
915 //int l_val3 = static_cast<int>(l_nz * GRANULARITY + GRANULARITY);
916
917 //*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1];
918 *lp_norm = static_cast<unsigned char> (0x1 << bin_index);
919 }
920 else
921 {
922 *lp_norm = 0; // Discard shadows from depth sensor
923 }
924 }
925 else
926 {
927 *lp_norm = 0; //out of depth
928 }
929 ++lp_line;
930 ++lp_norm;
931 }
932 }
933 /*cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);*/
934
935 unsigned char map[255]{};
936
937 map[0x1<<0] = 1;
938 map[0x1<<1] = 2;
939 map[0x1<<2] = 3;
940 map[0x1<<3] = 4;
941 map[0x1<<4] = 5;
942 map[0x1<<5] = 6;
943 map[0x1<<6] = 7;
944 map[0x1<<7] = 8;
945
946 quantized_surface_normals_.resize (width, height);
947 for (int row_index = 0; row_index < height; ++row_index)
948 {
949 for (int col_index = 0; col_index < width; ++col_index)
950 {
951 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
952 }
953 }
954
955 delete[] lp_depth;
956 delete[] lp_normals;
957}
958
959
960//////////////////////////////////////////////////////////////////////////////////////////////
961template <typename PointInT> void
963 const std::size_t nr_features,
964 const std::size_t modality_index,
965 std::vector<QuantizedMultiModFeature> & features) const
966{
967 const std::size_t width = mask.getWidth ();
968 const std::size_t height = mask.getHeight ();
969
970 //cv::Mat maskImage(height, width, CV_8U, mask.mask);
971 //cv::erode(maskImage, maskImage
972
973 // create distance maps for every quantization value
974 //cv::Mat distance_maps[8];
975 //for (int map_index = 0; map_index < 8; ++map_index)
976 //{
977 // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
978 //}
979
980 MaskMap mask_maps[8];
981 for (auto &mask_map : mask_maps)
982 mask_map.resize (width, height);
983
984 unsigned char map[255]{};
985
986 map[0x1<<0] = 0;
987 map[0x1<<1] = 1;
988 map[0x1<<2] = 2;
989 map[0x1<<3] = 3;
990 map[0x1<<4] = 4;
991 map[0x1<<5] = 5;
992 map[0x1<<6] = 6;
993 map[0x1<<7] = 7;
994
995 QuantizedMap distance_map_indices (width, height);
996 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
997
998 for (std::size_t row_index = 0; row_index < height; ++row_index)
999 {
1000 for (std::size_t col_index = 0; col_index < width; ++col_index)
1001 {
1002 if (mask (col_index, row_index) != 0)
1003 {
1004 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1005 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1006
1007 if (quantized_value == 0)
1008 continue;
1009 const int dist_map_index = map[quantized_value];
1010
1011 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1012 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1013 mask_maps[dist_map_index] (col_index, row_index) = 255;
1014 }
1015 }
1016 }
1017
1018 DistanceMap distance_maps[8];
1019 for (int map_index = 0; map_index < 8; ++map_index)
1020 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1021
1022 DistanceMap mask_distance_maps;
1023 computeDistanceMap (mask, mask_distance_maps);
1024
1025 std::list<Candidate> list1;
1026 std::list<Candidate> list2;
1027
1028 float weights[8] = {0,0,0,0,0,0,0,0};
1029
1030 constexpr std::size_t off = 4;
1031 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1032 {
1033 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1034 {
1035 if (mask (col_index, row_index) != 0)
1036 {
1037 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1038 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1039
1040 //const float nx = surface_normals_ (col_index, row_index).normal_x;
1041 //const float ny = surface_normals_ (col_index, row_index).normal_y;
1042 //const float nz = surface_normals_ (col_index, row_index).normal_z;
1043
1044 if (quantized_value != 0)// && !(std::isnan (nx) || std::isnan (ny) || std::isnan (nz)))
1045 {
1046 const int distance_map_index = map[quantized_value];
1047
1048 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1049 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1050 const float distance_to_border = mask_distance_maps (col_index, row_index);
1051
1052 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1053 {
1054 Candidate candidate;
1055
1056 candidate.distance = distance;
1057 candidate.x = col_index;
1058 candidate.y = row_index;
1059 candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1060
1061 list1.push_back (candidate);
1062
1063 ++weights[distance_map_index];
1064 }
1065 }
1066 }
1067 }
1068 }
1069
1070 for (auto iter = list1.begin (); iter != list1.end (); ++iter)
1071 iter->distance *= 1.0f / weights[iter->bin_index];
1072
1073 list1.sort ();
1074
1075 if (variable_feature_nr_)
1076 {
1077 int distance = static_cast<int> (list1.size ());
1078 bool feature_selection_finished = false;
1079 while (!feature_selection_finished)
1080 {
1081 const int sqr_distance = distance*distance;
1082 for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1083 {
1084 bool candidate_accepted = true;
1085 for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1086 {
1087 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1088 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1089 const int tmp_distance = dx*dx + dy*dy;
1090
1091 if (tmp_distance < sqr_distance)
1092 {
1093 candidate_accepted = false;
1094 break;
1095 }
1096 }
1097
1098
1099 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1100 float max_min_sqr_distance = 0;
1101 for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1102 {
1103 float min_sqr_distance = std::numeric_limits<float>::max ();
1104 for (auto iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1105 {
1106 if (iter2 == iter3)
1107 continue;
1108
1109 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1110 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1111
1112 const float sqr_distance = dx*dx + dy*dy;
1113
1114 if (sqr_distance < min_sqr_distance)
1115 {
1116 min_sqr_distance = sqr_distance;
1117 }
1118
1119 //std::cerr << min_sqr_distance;
1120 }
1121 //std::cerr << std::endl;
1122
1123 // check current feature
1124 {
1125 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1126 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1127
1128 const float sqr_distance = dx*dx + dy*dy;
1129
1130 if (sqr_distance < min_sqr_distance)
1131 {
1132 min_sqr_distance = sqr_distance;
1133 }
1134 }
1135
1136 if (min_sqr_distance < min_min_sqr_distance)
1137 min_min_sqr_distance = min_sqr_distance;
1138 if (min_sqr_distance > max_min_sqr_distance)
1139 max_min_sqr_distance = min_sqr_distance;
1140
1141 //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl;
1142 }
1143
1144 if (candidate_accepted)
1145 {
1146 //std::cerr << "feature_index: " << list2.size () << std::endl;
1147 //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl;
1148 //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl;
1149
1150 if (min_min_sqr_distance < 50)
1151 {
1152 feature_selection_finished = true;
1153 break;
1154 }
1155
1156 list2.push_back (*iter1);
1157 }
1158
1159 //if (list2.size () == nr_features)
1160 //{
1161 // feature_selection_finished = true;
1162 // break;
1163 //}
1164 }
1165 --distance;
1166 }
1167 }
1168 else
1169 {
1170 if (list1.size () <= nr_features)
1171 {
1172 features.reserve (list1.size ());
1173 for (auto iter = list1.begin (); iter != list1.end (); ++iter)
1174 {
1176
1177 feature.x = static_cast<int> (iter->x);
1178 feature.y = static_cast<int> (iter->y);
1179 feature.modality_index = modality_index;
1180 feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1181
1182 features.push_back (feature);
1183 }
1184
1185 return;
1186 }
1187
1188 int distance = static_cast<int> (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:!
1189 while (list2.size () != nr_features)
1190 {
1191 const int sqr_distance = distance*distance;
1192 for (auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1193 {
1194 bool candidate_accepted = true;
1195
1196 for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1197 {
1198 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1199 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1200 const int tmp_distance = dx*dx + dy*dy;
1201
1202 if (tmp_distance < sqr_distance)
1203 {
1204 candidate_accepted = false;
1205 break;
1206 }
1207 }
1208
1209 if (candidate_accepted)
1210 list2.push_back (*iter1);
1211
1212 if (list2.size () == nr_features) break;
1213 }
1214 --distance;
1215 }
1216 }
1217
1218 for (auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1219 {
1221
1222 feature.x = static_cast<int> (iter2->x);
1223 feature.y = static_cast<int> (iter2->y);
1224 feature.modality_index = modality_index;
1225 feature.quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1226
1227 features.push_back (feature);
1228 }
1229}
1230
1231//////////////////////////////////////////////////////////////////////////////////////////////
1232template <typename PointInT> void
1234 const MaskMap & mask, const std::size_t, const std::size_t modality_index,
1235 std::vector<QuantizedMultiModFeature> & features) const
1236{
1237 const std::size_t width = mask.getWidth ();
1238 const std::size_t height = mask.getHeight ();
1239
1240 //cv::Mat maskImage(height, width, CV_8U, mask.mask);
1241 //cv::erode(maskImage, maskImage
1242
1243 // create distance maps for every quantization value
1244 //cv::Mat distance_maps[8];
1245 //for (int map_index = 0; map_index < 8; ++map_index)
1246 //{
1247 // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U);
1248 //}
1249
1250 MaskMap mask_maps[8];
1251 for (auto &mask_map : mask_maps)
1252 mask_map.resize (width, height);
1253
1254 unsigned char map[255]{};
1255
1256 map[0x1<<0] = 0;
1257 map[0x1<<1] = 1;
1258 map[0x1<<2] = 2;
1259 map[0x1<<3] = 3;
1260 map[0x1<<4] = 4;
1261 map[0x1<<5] = 5;
1262 map[0x1<<6] = 6;
1263 map[0x1<<7] = 7;
1264
1265 QuantizedMap distance_map_indices (width, height);
1266 //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height);
1267
1268 for (std::size_t row_index = 0; row_index < height; ++row_index)
1269 {
1270 for (std::size_t col_index = 0; col_index < width; ++col_index)
1271 {
1272 if (mask (col_index, row_index) != 0)
1273 {
1274 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1275 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1276
1277 if (quantized_value == 0)
1278 continue;
1279 const int dist_map_index = map[quantized_value];
1280
1281 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1282 //distance_maps[dist_map_index].at<unsigned char>(row_index, col_index) = 255;
1283 mask_maps[dist_map_index] (col_index, row_index) = 255;
1284 }
1285 }
1286 }
1287
1288 DistanceMap distance_maps[8];
1289 for (int map_index = 0; map_index < 8; ++map_index)
1290 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1291
1292 DistanceMap mask_distance_maps;
1293 computeDistanceMap (mask, mask_distance_maps);
1294
1295 std::list<Candidate> list1;
1296 std::list<Candidate> list2;
1297
1298 float weights[8] = {0,0,0,0,0,0,0,0};
1299
1300 constexpr std::size_t off = 4;
1301 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1302 {
1303 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1304 {
1305 if (mask (col_index, row_index) != 0)
1306 {
1307 //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index);
1308 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1309
1310 //const float nx = surface_normals_ (col_index, row_index).normal_x;
1311 //const float ny = surface_normals_ (col_index, row_index).normal_y;
1312 //const float nz = surface_normals_ (col_index, row_index).normal_z;
1313
1314 if (quantized_value != 0)// && !(std::isnan (nx) || std::isnan (ny) || std::isnan (nz)))
1315 {
1316 const int distance_map_index = map[quantized_value];
1317
1318 //const float distance = distance_maps[distance_map_index].at<float> (row_index, col_index);
1319 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1320 const float distance_to_border = mask_distance_maps (col_index, row_index);
1321
1322 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1323 {
1324 Candidate candidate;
1325
1326 candidate.distance = distance;
1327 candidate.x = col_index;
1328 candidate.y = row_index;
1329 candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1330
1331 list1.push_back (candidate);
1332
1333 ++weights[distance_map_index];
1334 }
1335 }
1336 }
1337 }
1338 }
1339
1340 for (auto iter = list1.begin (); iter != list1.end (); ++iter)
1341 iter->distance *= 1.0f / weights[iter->bin_index];
1342
1343 list1.sort ();
1344
1345 features.reserve (list1.size ());
1346 for (auto iter = list1.begin (); iter != list1.end (); ++iter)
1347 {
1349
1350 feature.x = static_cast<int> (iter->x);
1351 feature.y = static_cast<int> (iter->y);
1352 feature.modality_index = modality_index;
1353 feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1354
1355 features.push_back (feature);
1356 }
1357}
1358
1359//////////////////////////////////////////////////////////////////////////////////////////////
1360template <typename PointInT> void
1362{
1363 const std::size_t width = input_->width;
1364 const std::size_t height = input_->height;
1365
1366 quantized_surface_normals_.resize (width, height);
1367
1368 for (std::size_t row_index = 0; row_index < height; ++row_index)
1369 {
1370 for (std::size_t col_index = 0; col_index < width; ++col_index)
1371 {
1372 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1373 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1374 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1375
1376 if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0 || (normal_x == 0 && normal_y == 0))
1377 {
1378 quantized_surface_normals_ (col_index, row_index) = 0;
1379 continue;
1380 }
1381
1382 //quantized_surface_normals_.data[row_index*width+col_index] =
1383 // normal_lookup_(normal_x, normal_y, normal_z);
1384
1385 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
1386
1387 if (angle < 0.0f) angle += 360.0f;
1388 if (angle >= 360.0f) angle -= 360.0f;
1389
1390 int bin_index = static_cast<int> (angle*8.0f/360.0f + 1);
1391 bin_index = (bin_index < 1) ? 1 : (8 < bin_index) ? 8 : bin_index;
1392
1393 //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index;
1394 quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index);
1395 }
1396 }
1397
1398 return;
1399}
1400
1401//////////////////////////////////////////////////////////////////////////////////////////////
1402template <typename PointInT> void
1404{
1405 const int width = input_->width;
1406 const int height = input_->height;
1407
1408 filtered_quantized_surface_normals_.resize (width, height);
1409
1410 //for (int row_index = 2; row_index < height-2; ++row_index)
1411 //{
1412 // for (int col_index = 2; col_index < width-2; ++col_index)
1413 // {
1414 // std::list<unsigned char> values;
1415 // values.reserve (25);
1416
1417 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1418 // values.push_back (dataPtr[0]);
1419 // values.push_back (dataPtr[1]);
1420 // values.push_back (dataPtr[2]);
1421 // values.push_back (dataPtr[3]);
1422 // values.push_back (dataPtr[4]);
1423 // dataPtr += width;
1424 // values.push_back (dataPtr[0]);
1425 // values.push_back (dataPtr[1]);
1426 // values.push_back (dataPtr[2]);
1427 // values.push_back (dataPtr[3]);
1428 // values.push_back (dataPtr[4]);
1429 // dataPtr += width;
1430 // values.push_back (dataPtr[0]);
1431 // values.push_back (dataPtr[1]);
1432 // values.push_back (dataPtr[2]);
1433 // values.push_back (dataPtr[3]);
1434 // values.push_back (dataPtr[4]);
1435 // dataPtr += width;
1436 // values.push_back (dataPtr[0]);
1437 // values.push_back (dataPtr[1]);
1438 // values.push_back (dataPtr[2]);
1439 // values.push_back (dataPtr[3]);
1440 // values.push_back (dataPtr[4]);
1441 // dataPtr += width;
1442 // values.push_back (dataPtr[0]);
1443 // values.push_back (dataPtr[1]);
1444 // values.push_back (dataPtr[2]);
1445 // values.push_back (dataPtr[3]);
1446 // values.push_back (dataPtr[4]);
1447
1448 // values.sort ();
1449
1450 // filtered_quantized_surface_normals_ (col_index, row_index) = values[12];
1451 // }
1452 //}
1453
1454
1455 //for (int row_index = 2; row_index < height-2; ++row_index)
1456 //{
1457 // for (int col_index = 2; col_index < width-2; ++col_index)
1458 // {
1459 // filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << (quantized_surface_normals_ (col_index, row_index) - 1));
1460 // }
1461 //}
1462
1463
1464 // filter data
1465 for (int row_index = 2; row_index < height-2; ++row_index)
1466 {
1467 for (int col_index = 2; col_index < width-2; ++col_index)
1468 {
1469 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1470
1471 //{
1472 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-1;
1473 // ++histogram[dataPtr[0]];
1474 // ++histogram[dataPtr[1]];
1475 // ++histogram[dataPtr[2]];
1476 //}
1477 //{
1478 // unsigned char * dataPtr = quantized_surface_normals_.getData () + row_index*width+col_index-1;
1479 // ++histogram[dataPtr[0]];
1480 // ++histogram[dataPtr[1]];
1481 // ++histogram[dataPtr[2]];
1482 //}
1483 //{
1484 // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-1;
1485 // ++histogram[dataPtr[0]];
1486 // ++histogram[dataPtr[1]];
1487 // ++histogram[dataPtr[2]];
1488 //}
1489
1490 {
1491 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1492 ++histogram[dataPtr[0]];
1493 ++histogram[dataPtr[1]];
1494 ++histogram[dataPtr[2]];
1495 ++histogram[dataPtr[3]];
1496 ++histogram[dataPtr[4]];
1497 }
1498 {
1499 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1500 ++histogram[dataPtr[0]];
1501 ++histogram[dataPtr[1]];
1502 ++histogram[dataPtr[2]];
1503 ++histogram[dataPtr[3]];
1504 ++histogram[dataPtr[4]];
1505 }
1506 {
1507 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1508 ++histogram[dataPtr[0]];
1509 ++histogram[dataPtr[1]];
1510 ++histogram[dataPtr[2]];
1511 ++histogram[dataPtr[3]];
1512 ++histogram[dataPtr[4]];
1513 }
1514 {
1515 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1516 ++histogram[dataPtr[0]];
1517 ++histogram[dataPtr[1]];
1518 ++histogram[dataPtr[2]];
1519 ++histogram[dataPtr[3]];
1520 ++histogram[dataPtr[4]];
1521 }
1522 {
1523 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1524 ++histogram[dataPtr[0]];
1525 ++histogram[dataPtr[1]];
1526 ++histogram[dataPtr[2]];
1527 ++histogram[dataPtr[3]];
1528 ++histogram[dataPtr[4]];
1529 }
1530
1531
1532 unsigned char max_hist_value = 0;
1533 int max_hist_index = -1;
1534
1535 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1536 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1537 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1538 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1539 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1540 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1541 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1542 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1543
1544 if (max_hist_index != -1 && max_hist_value >= 1)
1545 {
1546 filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1547 }
1548 else
1549 {
1550 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1551 }
1552
1553 //filtered_quantized_color_gradients_.data[row_index*width+col_index] = quantized_color_gradients_.data[row_index*width+col_index];
1554 }
1555 }
1556
1557
1558
1559 //cv::Mat data1(quantized_surface_normals_.height, quantized_surface_normals_.width, CV_8U, quantized_surface_normals_.data);
1560 //cv::Mat data2(filtered_quantized_surface_normals_.height, filtered_quantized_surface_normals_.width, CV_8U, filtered_quantized_surface_normals_.data);
1561
1562 //cv::medianBlur(data1, data2, 3);
1563
1564 //for (int row_index = 0; row_index < height; ++row_index)
1565 //{
1566 // for (int col_index = 0; col_index < width; ++col_index)
1567 // {
1568 // filtered_quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << filtered_quantized_surface_normals_.data[row_index*width+col_index];
1569 // }
1570 //}
1571}
1572
1573//////////////////////////////////////////////////////////////////////////////////////////////
1574template <typename PointInT> void
1576{
1577 const std::size_t width = input.getWidth ();
1578 const std::size_t height = input.getHeight ();
1579
1580 output.resize (width, height);
1581
1582 // compute distance map
1583 //float *distance_map = new float[input_->size ()];
1584 const unsigned char * mask_map = input.getData ();
1585 float * distance_map = output.getData ();
1586 for (std::size_t index = 0; index < width*height; ++index)
1587 {
1588 if (mask_map[index] == 0)
1589 distance_map[index] = 0.0f;
1590 else
1591 distance_map[index] = static_cast<float> (width + height);
1592 }
1593
1594 // first pass
1595 float * previous_row = distance_map;
1596 float * current_row = previous_row + width;
1597 for (std::size_t ri = 1; ri < height; ++ri)
1598 {
1599 for (std::size_t ci = 1; ci < width; ++ci)
1600 {
1601 const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f;
1602 const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f;
1603 const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f;
1604 const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f;
1605 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1606
1607 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1608
1609 if (min_value < center)
1610 current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value;
1611 }
1612 previous_row = current_row;
1613 current_row += width;
1614 }
1615
1616 // second pass
1617 float * next_row = distance_map + width * (height - 1);
1618 current_row = next_row - width;
1619 for (int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1620 {
1621 for (int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1622 {
1623 const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f;
1624 const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f;
1625 const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f;
1626 const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f;
1627 const float center = current_row [ci]; //distance_map[ri*input_->width + ci];
1628
1629 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1630
1631 if (min_value < center)
1632 current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value;
1633 }
1634 next_row = current_row;
1635 current_row -= width;
1636 }
1637}
Represents a distance map obtained from a distance transformation.
void resize(const std::size_t width, const std::size_t height)
Resizes the map to the specified size.
float * getData()
Returns a pointer to the beginning of map.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:194
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
std::size_t getWidth() const
Definition mask_map.h:57
void resize(std::size_t width, std::size_t height)
std::size_t getHeight() const
Definition mask_map.h:63
unsigned char * getData()
Definition mask_map.h:69
PCL base class.
Definition pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
shared_ptr< const PointCloud< PointInT > > ConstPtr
Interface for a quantizable modality.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
Modality based on surface normals.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
~SurfaceNormalModality() override
Destructor.
QuantizedMap & getQuantizedMap() override
Returns a reference to the internal quantized map.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
void quantizeSurfaceNormals()
Quantizes the surface normals.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void extractAllFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const override
Extracts all possible features from the modality within the specified mask.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
QuantizedMap & getSpreadedQuantizedMap() override
Returns a reference to the internal spread quantized map.
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
void extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const override
Extracts features from this modality within the specified mask.
Defines all the PCL implemented PointT point type structures.
#define M_PI
Definition pcl_macros.h:201
Map that stores orientations.
std::size_t getWidth() const
Returns the width of the modality data map.
std::size_t getHeight() const
Returns the height of the modality data map.
~LINEMOD_OrientationMap()=default
Destructor.
void resize(const std::size_t width, const std::size_t height, const float value)
Resizes the map to the specific width and height and initializes all new elements with the specified ...
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
Feature that defines a position and quantized value in a specific modality.
std::size_t modality_index
the index of the corresponding modality.
unsigned char quantized_value
the quantized value attached to the feature.
Look-up-table for fast surface normal quantization.
int size_y
The size of the LUT in y-direction.
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
int size_x
The size of the LUT in x-direction.
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
int range_y
The range of the LUT in y-direction.
int offset_x
The offset in x-direction.
int offset_z
The offset in z-direction.
int range_z
The range of the LUT in z-direction.
int size_z
The size of the LUT in z-direction.
int range_x
The range of the LUT in x-direction.
int offset_y
The offset in y-direction.
Candidate for a feature (used in feature extraction methods).
float distance
Distance to the next different quantized value.
std::size_t x
x-position of the feature.
std::size_t y
y-position of the feature.
bool operator<(const Candidate &rhs) const
Compares two candidates based on their distance to the next different quantized value.