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