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