Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
integral_image_normal.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38#pragma once
39#include <pcl/features/integral_image_normal.h>
40
41#include <algorithm>
42
43//////////////////////////////////////////////////////////////////////////////////////////
44template <typename PointInT, typename PointOutT>
46{
47 delete[] diff_x_;
48 delete[] diff_y_;
49 delete[] depth_data_;
50 delete[] distance_map_;
51}
52
53//////////////////////////////////////////////////////////////////////////////////////////
54template <typename PointInT, typename PointOutT> void
56{
57 if (border_policy_ != BORDER_POLICY_IGNORE &&
58 border_policy_ != BORDER_POLICY_MIRROR)
59 PCL_THROW_EXCEPTION (InitFailedException,
60 "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
61
62 if (normal_estimation_method_ != COVARIANCE_MATRIX &&
63 normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
64 normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
65 normal_estimation_method_ != SIMPLE_3D_GRADIENT)
66 PCL_THROW_EXCEPTION (InitFailedException,
67 "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
68
69 // compute derivatives
70 delete[] diff_x_;
71 delete[] diff_y_;
72 delete[] depth_data_;
73 delete[] distance_map_;
74 diff_x_ = nullptr;
75 diff_y_ = nullptr;
76 depth_data_ = nullptr;
77 distance_map_ = nullptr;
78
79 if (normal_estimation_method_ == COVARIANCE_MATRIX)
80 initCovarianceMatrixMethod ();
81 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
82 initAverage3DGradientMethod ();
83 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
84 initAverageDepthChangeMethod ();
85 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
86 initSimple3DGradientMethod ();
87}
88
89
90//////////////////////////////////////////////////////////////////////////////////////////
91template <typename PointInT, typename PointOutT> void
93{
94 rect_width_ = width;
95 rect_width_2_ = width/2;
96 rect_width_4_ = width/4;
97 rect_height_ = height;
98 rect_height_2_ = height/2;
99 rect_height_4_ = height/4;
100}
101
102//////////////////////////////////////////////////////////////////////////////////////////
103template <typename PointInT, typename PointOutT> void
105{
106 // number of DataType entries per element (equal or bigger than dimensions)
107 int element_stride = sizeof (PointInT) / sizeof (float);
108 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
109 int row_stride = element_stride * input_->width;
110
111 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
112
113 integral_image_XYZ_.setSecondOrderComputation (false);
114 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
115
116 init_simple_3d_gradient_ = true;
117 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////
121template <typename PointInT, typename PointOutT> void
123{
124 // number of DataType entries per element (equal or bigger than dimensions)
125 int element_stride = sizeof (PointInT) / sizeof (float);
126 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
127 int row_stride = element_stride * input_->width;
128
129 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
130
131 integral_image_XYZ_.setSecondOrderComputation (true);
132 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
133
134 init_covariance_matrix_ = true;
135 init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
136}
137
138//////////////////////////////////////////////////////////////////////////////////////////
139template <typename PointInT, typename PointOutT> void
141{
142 delete[] diff_x_;
143 delete[] diff_y_;
144 std::size_t data_size = (input_->size () << 2);
145 diff_x_ = new float[data_size]{};
146 diff_y_ = new float[data_size]{};
148 // x u x
149 // l x r
150 // x d x
151 const PointInT* point_up = &(input_->points [1]);
152 const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
153 const PointInT* point_lf = &(input_->points [input_->width]);
154 const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
155 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
156 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
157 unsigned diff_skip = 8; // skip last element in row and the first in the next row
158
159 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
160 , point_up += input_->width
161 , point_dn += input_->width
162 , point_lf += input_->width
163 , point_rg += input_->width
164 , diff_x_ptr += diff_skip
165 , diff_y_ptr += diff_skip)
166 {
167 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
168 {
169 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
170 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
171 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
172
173 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
174 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
175 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
176 }
177 }
178
179 // Compute integral images
180 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
181 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
182 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
183 init_average_3d_gradient_ = true;
184}
185
186//////////////////////////////////////////////////////////////////////////////////////////
187template <typename PointInT, typename PointOutT> void
189{
190 // number of DataType entries per element (equal or bigger than dimensions)
191 int element_stride = sizeof (PointInT) / sizeof (float);
192 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
193 int row_stride = element_stride * input_->width;
194
195 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
196
197 // integral image over the z - value
198 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
199 init_depth_change_ = true;
200 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
201}
202
203//////////////////////////////////////////////////////////////////////////////////////////
204template <typename PointInT, typename PointOutT> void
206 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
207{
208 float bad_point = std::numeric_limits<float>::quiet_NaN ();
209
210 if (normal_estimation_method_ == COVARIANCE_MATRIX)
211 {
212 if (!init_covariance_matrix_)
213 initCovarianceMatrixMethod ();
214
215 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
216
217 // no valid points within the rectangular region?
218 if (count == 0)
219 {
220 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
221 return;
222 }
223
224 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
225 Eigen::Vector3f center;
227 center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
228 so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
229
230 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
231 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
232 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
233 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
234 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
235 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
236 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
237 float eigen_value;
238 Eigen::Vector3f eigen_vector;
239 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
240 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
241 normal.getNormalVector3fMap () = eigen_vector;
242
243 // Compute the curvature surface change
244 if (eigen_value > 0.0)
245 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
246 else
247 normal.curvature = 0;
248
249 return;
250 }
251 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
252 {
253 if (!init_average_3d_gradient_)
254 initAverage3DGradientMethod ();
255
256 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
257 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
258 if (count_x == 0 || count_y == 0)
259 {
260 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
261 return;
262 }
263 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
264 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
265
266 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
267 double normal_length = normal_vector.squaredNorm ();
268 if (normal_length == 0.0f)
269 {
270 normal.getNormalVector3fMap ().setConstant (bad_point);
271 normal.curvature = bad_point;
272 return;
273 }
274
275 normal_vector /= sqrt (normal_length);
276
277 float nx = static_cast<float> (normal_vector [0]);
278 float ny = static_cast<float> (normal_vector [1]);
279 float nz = static_cast<float> (normal_vector [2]);
280
281 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
282
283 normal.normal_x = nx;
284 normal.normal_y = ny;
285 normal.normal_z = nz;
286 normal.curvature = bad_point;
287 return;
288 }
289 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
290 {
291 if (!init_depth_change_)
292 initAverageDepthChangeMethod ();
293
294 // width and height are at least 3 x 3
295 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
296 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
297 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
298 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
299
300 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
301 {
302 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
303 return;
304 }
305
306 float mean_L_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z);
307 float mean_R_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z);
308 float mean_U_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z);
309 float mean_D_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z);
310
311 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
312 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
313 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
314 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
315
316 const float mean_x_z = mean_R_z - mean_L_z;
317 const float mean_y_z = mean_D_z - mean_U_z;
318
319 const float mean_x_x = pointR.x - pointL.x;
320 const float mean_x_y = pointR.y - pointL.y;
321 const float mean_y_x = pointD.x - pointU.x;
322 const float mean_y_y = pointD.y - pointU.y;
324 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
325 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
326 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
327
328 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
329
330 if (normal_length == 0.0f)
331 {
332 normal.getNormalVector3fMap ().setConstant (bad_point);
333 normal.curvature = bad_point;
334 return;
335 }
336
337 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
338
339 const float scale = 1.0f / std::sqrt (normal_length);
340
341 normal.normal_x = normal_x * scale;
342 normal.normal_y = normal_y * scale;
343 normal.normal_z = normal_z * scale;
344 normal.curvature = bad_point;
345
346 return;
347 }
348 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
349 {
350 if (!init_simple_3d_gradient_)
351 initSimple3DGradientMethod ();
352
353 // this method does not work if lots of NaNs are in the neighborhood of the point
354 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
355 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
356
357 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
358 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
359 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
360 double normal_length = normal_vector.squaredNorm ();
361 if (normal_length == 0.0f)
362 {
363 normal.getNormalVector3fMap ().setConstant (bad_point);
364 normal.curvature = bad_point;
365 return;
366 }
367
368 normal_vector /= sqrt (normal_length);
369
370 float nx = static_cast<float> (normal_vector [0]);
371 float ny = static_cast<float> (normal_vector [1]);
372 float nz = static_cast<float> (normal_vector [2]);
373
374 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
375
376 normal.normal_x = nx;
377 normal.normal_y = ny;
378 normal.normal_z = nz;
379 normal.curvature = bad_point;
380 return;
381 }
382
383 normal.getNormalVector3fMap ().setConstant (bad_point);
384 normal.curvature = bad_point;
385 return;
386}
387
388//////////////////////////////////////////////////////////////////////////////////////////
389template <typename T>
390void
391sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
392 const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
393 T & result)
394{
395 if (start_x < 0)
396 {
397 if (start_y < 0)
398 {
399 result += f (0, 0, end_x, end_y);
400 result += f (0, 0, -start_x, -start_y);
401 result += f (0, 0, -start_x, end_y);
402 result += f (0, 0, end_x, -start_y);
403 }
404 else if (end_y >= height)
405 {
406 result += f (0, start_y, end_x, height-1);
407 result += f (0, start_y, -start_x, height-1);
408 result += f (0, height-(end_y-(height-1)), end_x, height-1);
409 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
410 }
411 else
412 {
413 result += f (0, start_y, end_x, end_y);
414 result += f (0, start_y, -start_x, end_y);
415 }
416 }
417 else if (start_y < 0)
418 {
419 if (end_x >= width)
420 {
421 result += f (start_x, 0, width-1, end_y);
422 result += f (start_x, 0, width-1, -start_y);
423 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
424 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
425 }
426 else
427 {
428 result += f (start_x, 0, end_x, end_y);
429 result += f (start_x, 0, end_x, -start_y);
430 }
431 }
432 else if (end_x >= width)
433 {
434 if (end_y >= height)
435 {
436 result += f (start_x, start_y, width-1, height-1);
437 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
438 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
439 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
440 }
441 else
442 {
443 result += f (start_x, start_y, width-1, end_y);
444 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
445 }
446 }
447 else if (end_y >= height)
448 {
449 result += f (start_x, start_y, end_x, height-1);
450 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
451 }
452 else
453 {
454 result += f (start_x, start_y, end_x, end_y);
455 }
456}
457
458//////////////////////////////////////////////////////////////////////////////////////////
459template <typename PointInT, typename PointOutT> void
461 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
462{
463 float bad_point = std::numeric_limits<float>::quiet_NaN ();
464
465 const int width = input_->width;
466 const int height = input_->height;
467
468 // ==============================================================
469 if (normal_estimation_method_ == COVARIANCE_MATRIX)
470 {
471 if (!init_covariance_matrix_)
472 initCovarianceMatrixMethod ();
473
474 const int start_x = pos_x - rect_width_2_;
475 const int start_y = pos_y - rect_height_2_;
476 const int end_x = start_x + rect_width_;
477 const int end_y = start_y + rect_height_;
478
479 unsigned count = 0;
480 auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
481 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
482
483 // no valid points within the rectangular region?
484 if (count == 0)
485 {
486 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
487 return;
488 }
489
490 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
491 Eigen::Vector3f center;
493 typename IntegralImage2D<float, 3>::ElementType tmp_center;
494 typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
495
496 center[0] = 0;
497 center[1] = 0;
498 center[2] = 0;
499 tmp_center[0] = 0;
500 tmp_center[1] = 0;
501 tmp_center[2] = 0;
502 so_elements[0] = 0;
503 so_elements[1] = 0;
504 so_elements[2] = 0;
505 so_elements[3] = 0;
506 so_elements[4] = 0;
507 so_elements[5] = 0;
508
509 auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
510 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
511 auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
512 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
513
514 center[0] = static_cast<float>(tmp_center[0]);
515 center[1] = static_cast<float>(tmp_center[1]);
516 center[2] = static_cast<float>(tmp_center[2]);
517
518 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
519 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
520 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
521 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
522 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
523 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
524 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
525 float eigen_value;
526 Eigen::Vector3f eigen_vector;
527 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
528 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
529 normal.getNormalVector3fMap () = eigen_vector;
530
531 // Compute the curvature surface change
532 if (eigen_value > 0.0)
533 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
534 else
535 normal.curvature = 0;
536
537 return;
538 }
539 // =======================================================
540 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
541 {
542 if (!init_average_3d_gradient_)
543 initAverage3DGradientMethod ();
544
545 const int start_x = pos_x - rect_width_2_;
546 const int start_y = pos_y - rect_height_2_;
547 const int end_x = start_x + rect_width_;
548 const int end_y = start_y + rect_height_;
549
550 unsigned count_x = 0;
551 unsigned count_y = 0;
552
553 auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
554 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
555 auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
557
558
559 if (count_x == 0 || count_y == 0)
560 {
561 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
562 return;
563 }
564 Eigen::Vector3d gradient_x (0, 0, 0);
565 Eigen::Vector3d gradient_y (0, 0, 0);
566
567 auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
568 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
569 auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
570 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
571
572
573 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
574 double normal_length = normal_vector.squaredNorm ();
575 if (normal_length == 0.0f)
576 {
577 normal.getNormalVector3fMap ().setConstant (bad_point);
578 normal.curvature = bad_point;
579 return;
580 }
581
582 normal_vector /= sqrt (normal_length);
583
584 float nx = static_cast<float> (normal_vector [0]);
585 float ny = static_cast<float> (normal_vector [1]);
586 float nz = static_cast<float> (normal_vector [2]);
587
588 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
589
590 normal.normal_x = nx;
591 normal.normal_y = ny;
592 normal.normal_z = nz;
593 normal.curvature = bad_point;
594 return;
595 }
596 // ======================================================
597 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
598 {
599 if (!init_depth_change_)
600 initAverageDepthChangeMethod ();
601
602 int point_index_L_x = pos_x - rect_width_4_ - 1;
603 int point_index_L_y = pos_y;
604 int point_index_R_x = pos_x + rect_width_4_ + 1;
605 int point_index_R_y = pos_y;
606 int point_index_U_x = pos_x - 1;
607 int point_index_U_y = pos_y - rect_height_4_;
608 int point_index_D_x = pos_x + 1;
609 int point_index_D_y = pos_y + rect_height_4_;
610
611 if (point_index_L_x < 0)
612 point_index_L_x = -point_index_L_x;
613 if (point_index_U_x < 0)
614 point_index_U_x = -point_index_U_x;
615 if (point_index_U_y < 0)
616 point_index_U_y = -point_index_U_y;
617
618 if (point_index_R_x >= width)
619 point_index_R_x = width-(point_index_R_x-(width-1));
620 if (point_index_D_x >= width)
621 point_index_D_x = width-(point_index_D_x-(width-1));
622 if (point_index_D_y >= height)
623 point_index_D_y = height-(point_index_D_y-(height-1));
624
625 const int start_x_L = pos_x - rect_width_2_;
626 const int start_y_L = pos_y - rect_height_4_;
627 const int end_x_L = start_x_L + rect_width_2_;
628 const int end_y_L = start_y_L + rect_height_2_;
629
630 const int start_x_R = pos_x + 1;
631 const int start_y_R = pos_y - rect_height_4_;
632 const int end_x_R = start_x_R + rect_width_2_;
633 const int end_y_R = start_y_R + rect_height_2_;
634
635 const int start_x_U = pos_x - rect_width_4_;
636 const int start_y_U = pos_y - rect_height_2_;
637 const int end_x_U = start_x_U + rect_width_2_;
638 const int end_y_U = start_y_U + rect_height_2_;
639
640 const int start_x_D = pos_x - rect_width_4_;
641 const int start_y_D = pos_y + 1;
642 const int end_x_D = start_x_D + rect_width_2_;
643 const int end_y_D = start_y_D + rect_height_2_;
644
645 unsigned count_L_z = 0;
646 unsigned count_R_z = 0;
647 unsigned count_U_z = 0;
648 unsigned count_D_z = 0;
649
650 auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
651 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
652 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
653 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
654 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
655
656 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
657 {
658 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
659 return;
660 }
661
662 float mean_L_z = 0;
663 float mean_R_z = 0;
664 float mean_U_z = 0;
665 float mean_D_z = 0;
666
667 auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
668 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
669 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
670 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
671 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
672
673 mean_L_z /= static_cast<float>(count_L_z);
674 mean_R_z /= static_cast<float>(count_R_z);
675 mean_U_z /= static_cast<float>(count_U_z);
676 mean_D_z /= static_cast<float>(count_D_z);
677
678
679 PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
680 PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
681 PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
682 PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
683
684 const float mean_x_z = mean_R_z - mean_L_z;
685 const float mean_y_z = mean_D_z - mean_U_z;
686
687 const float mean_x_x = pointR.x - pointL.x;
688 const float mean_x_y = pointR.y - pointL.y;
689 const float mean_y_x = pointD.x - pointU.x;
690 const float mean_y_y = pointD.y - pointU.y;
691
692 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
693 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
694 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
695
696 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
697
698 if (normal_length == 0.0f)
699 {
700 normal.getNormalVector3fMap ().setConstant (bad_point);
701 normal.curvature = bad_point;
702 return;
703 }
704
705 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
706
707 const float scale = 1.0f / std::sqrt (normal_length);
708
709 normal.normal_x = normal_x * scale;
710 normal.normal_y = normal_y * scale;
711 normal.normal_z = normal_z * scale;
712 normal.curvature = bad_point;
713
714 return;
715 }
716 // ========================================================
717 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
718 {
719 PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
720 }
721
722 normal.getNormalVector3fMap ().setConstant (bad_point);
723 normal.curvature = bad_point;
724 return;
725}
726
727//////////////////////////////////////////////////////////////////////////////////////////
728template <typename PointInT, typename PointOutT> void
730{
731 output.sensor_origin_ = input_->sensor_origin_;
732 output.sensor_orientation_ = input_->sensor_orientation_;
733
734 float bad_point = std::numeric_limits<float>::quiet_NaN ();
735
736 // compute depth-change map
737 auto depthChangeMap = new unsigned char[input_->size ()];
738 std::fill_n(depthChangeMap, input_->size(), 255);
739
740 unsigned index = 0;
741 for (unsigned int ri = 0; ri < input_->height-1; ++ri)
742 {
743 for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
744 {
745 index = ri * input_->width + ci;
746
747 const float depth = input_->points [index].z;
748 const float depthR = input_->points [index + 1].z;
749 const float depthD = input_->points [index + input_->width].z;
750
751 //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f);
752 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
753
754 if (std::fabs (depth - depthR) > depthDependendDepthChange
755 || !std::isfinite (depth) || !std::isfinite (depthR))
756 {
757 depthChangeMap[index] = 0;
758 depthChangeMap[index+1] = 0;
759 }
760 if (std::fabs (depth - depthD) > depthDependendDepthChange
761 || !std::isfinite (depth) || !std::isfinite (depthD))
762 {
763 depthChangeMap[index] = 0;
764 depthChangeMap[index + input_->width] = 0;
765 }
766 }
767 }
768
769 // compute distance map
770 //float *distanceMap = new float[input_->size ()];
771 delete[] distance_map_;
772 distance_map_ = new float[input_->size ()];
773 float *distanceMap = distance_map_;
774 for (std::size_t index = 0; index < input_->size (); ++index)
775 {
776 if (depthChangeMap[index] == 0)
777 distanceMap[index] = 0.0f;
778 else
779 distanceMap[index] = static_cast<float> (input_->width + input_->height);
780 }
781
782 // first pass
783 float* previous_row = distanceMap;
784 float* current_row = previous_row + input_->width;
785 for (std::size_t ri = 1; ri < input_->height; ++ri)
786 {
787 for (std::size_t ci = 1; ci < input_->width; ++ci)
788 {
789 const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
790 const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
791 const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
792 const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
793 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
794
795 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
796
797 if (minValue < center)
798 current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
799 }
800 previous_row = current_row;
801 current_row += input_->width;
802 }
803
804 float* next_row = distanceMap + input_->width * (input_->height - 1);
805 current_row = next_row - input_->width;
806 // second pass
807 for (int ri = input_->height-2; ri >= 0; --ri)
808 {
809 for (int ci = input_->width-2; ci >= 0; --ci)
810 {
811 const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
812 const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
813 const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
814 const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
815 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
816
817 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
818
819 if (minValue < center)
820 current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
821 }
822 next_row = current_row;
823 current_row -= input_->width;
824 }
825
826 if (indices_->size () < input_->size ())
827 computeFeaturePart (distanceMap, bad_point, output);
828 else
829 computeFeatureFull (distanceMap, bad_point, output);
830
831 delete[] depthChangeMap;
832}
833
834//////////////////////////////////////////////////////////////////////////////////////////
835template <typename PointInT, typename PointOutT> void
837 const float &bad_point,
838 PointCloudOut &output)
839{
840 unsigned index = 0;
841
842 if (border_policy_ == BORDER_POLICY_IGNORE)
843 {
844 // Set all normals that we do not touch to NaN
845 // top and bottom borders
846 // That sets the output density to false!
847 output.is_dense = false;
848 const auto border = static_cast<unsigned>(normal_smoothing_size_);
849 PointOutT* vec1 = &output [0];
850 PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
851
852 std::size_t count = border * input_->width;
853 for (std::size_t idx = 0; idx < count; ++idx)
854 {
855 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
856 vec1 [idx].curvature = bad_point;
857 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
858 vec2 [idx].curvature = bad_point;
859 }
860
861 // left and right borders actually columns
862 vec1 = &output [border * input_->width];
863 vec2 = vec1 + input_->width - border;
864 for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
865 {
866 for (std::size_t ci = 0; ci < border; ++ci)
867 {
868 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
869 vec1 [ci].curvature = bad_point;
870 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
871 vec2 [ci].curvature = bad_point;
872 }
873 }
874
875 if (use_depth_dependent_smoothing_)
876 {
877 index = border + input_->width * border;
878 unsigned skip = (border << 1);
879 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
880 {
881 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
882 {
883 index = ri * input_->width + ci;
884
885 const float depth = (*input_)[index].z;
886 if (!std::isfinite (depth))
887 {
888 output[index].getNormalVector3fMap ().setConstant (bad_point);
889 output[index].curvature = bad_point;
890 continue;
891 }
892
893 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
894
895 if (smoothing > 2.0f)
896 {
897 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
898 // Since depth can be anything, we have no guarantee that the border is sufficient, so we need to check
899 if(ci>static_cast<unsigned>(rect_width_2_) && ri>static_cast<unsigned>(rect_height_2_) && (ci+rect_width_2_)<input_->width && (ri+rect_height_2_)<input_->height) {
900 computePointNormal (ci, ri, index, output [index]);
901 } else {
902 output[index].getNormalVector3fMap ().setConstant (bad_point);
903 output[index].curvature = bad_point;
904 }
905 }
906 else
907 {
908 output[index].getNormalVector3fMap ().setConstant (bad_point);
909 output[index].curvature = bad_point;
910 }
911 }
912 }
913 }
914 else
915 {
916 index = border + input_->width * border;
917 unsigned skip = (border << 1);
918 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
919 {
920 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
921 {
922 index = ri * input_->width + ci;
923
924 if (!std::isfinite ((*input_)[index].z))
925 {
926 output [index].getNormalVector3fMap ().setConstant (bad_point);
927 output [index].curvature = bad_point;
928 continue;
929 }
930
931 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_);
932
933 if (smoothing > 2.0f)
934 {
935 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
936 computePointNormal (ci, ri, index, output [index]);
937 }
938 else
939 {
940 output [index].getNormalVector3fMap ().setConstant (bad_point);
941 output [index].curvature = bad_point;
942 }
943 }
944 }
945 }
946 }
947 else if (border_policy_ == BORDER_POLICY_MIRROR)
948 {
949 output.is_dense = false;
950
951 if (use_depth_dependent_smoothing_)
952 {
953 //index = 0;
954 //unsigned skip = 0;
955 //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
956 for (unsigned ri = 0; ri < input_->height; ++ri)
957 {
958 //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
959 for (unsigned ci = 0; ci < input_->width; ++ci)
960 {
961 index = ri * input_->width + ci;
962
963 const float depth = (*input_)[index].z;
964 if (!std::isfinite (depth))
965 {
966 output[index].getNormalVector3fMap ().setConstant (bad_point);
967 output[index].curvature = bad_point;
968 continue;
969 }
970
971 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
972
973 if (smoothing > 2.0f)
974 {
975 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
976 computePointNormalMirror (ci, ri, index, output [index]);
977 }
978 else
979 {
980 output[index].getNormalVector3fMap ().setConstant (bad_point);
981 output[index].curvature = bad_point;
982 }
983 }
984 }
985 }
986 else
987 {
988 //index = border + input_->width * border;
989 //unsigned skip = (border << 1);
990 //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
991 for (unsigned ri = 0; ri < input_->height; ++ri)
992 {
993 //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
994 for (unsigned ci = 0; ci < input_->width; ++ci)
995 {
996 index = ri * input_->width + ci;
997
998 if (!std::isfinite ((*input_)[index].z))
999 {
1000 output [index].getNormalVector3fMap ().setConstant (bad_point);
1001 output [index].curvature = bad_point;
1002 continue;
1003 }
1004
1005 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_);
1006
1007 if (smoothing > 2.0f)
1008 {
1009 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1010 computePointNormalMirror (ci, ri, index, output [index]);
1011 }
1012 else
1013 {
1014 output [index].getNormalVector3fMap ().setConstant (bad_point);
1015 output [index].curvature = bad_point;
1016 }
1017 }
1018 }
1019 }
1020 }
1021}
1022
1023///////////////////////////////////////////////////////////////////////////////////////////
1024template <typename PointInT, typename PointOutT> void
1026 const float &bad_point,
1027 PointCloudOut &output)
1028{
1029 if (border_policy_ == BORDER_POLICY_IGNORE)
1030 {
1031 output.is_dense = false;
1032 const auto border = static_cast<unsigned>(normal_smoothing_size_);
1033 const unsigned bottom = input_->height > border ? input_->height - border : 0;
1034 const unsigned right = input_->width > border ? input_->width - border : 0;
1035 if (use_depth_dependent_smoothing_)
1036 {
1037 // Iterating over the entire index vector
1038 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1039 {
1040 unsigned pt_index = (*indices_)[idx];
1041 unsigned u = pt_index % input_->width;
1042 unsigned v = pt_index / input_->width;
1043 if (v < border || v > bottom)
1044 {
1045 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1046 output[idx].curvature = bad_point;
1047 continue;
1048 }
1049
1050 if (u < border || u > right)
1051 {
1052 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1053 output[idx].curvature = bad_point;
1054 continue;
1055 }
1056
1057 const float depth = (*input_)[pt_index].z;
1058 if (!std::isfinite (depth))
1059 {
1060 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1061 output[idx].curvature = bad_point;
1062 continue;
1063 }
1064
1065 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1066 if (smoothing > 2.0f)
1067 {
1068 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1069 computePointNormal (u, v, pt_index, output [idx]);
1070 }
1071 else
1072 {
1073 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1074 output[idx].curvature = bad_point;
1075 }
1076 }
1077 }
1078 else
1079 {
1080 // Iterating over the entire index vector
1081 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1082 {
1083 unsigned pt_index = (*indices_)[idx];
1084 unsigned u = pt_index % input_->width;
1085 unsigned v = pt_index / input_->width;
1086 if (v < border || v > bottom)
1087 {
1088 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1089 output[idx].curvature = bad_point;
1090 continue;
1091 }
1092
1093 if (u < border || u > right)
1094 {
1095 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1096 output[idx].curvature = bad_point;
1097 continue;
1098 }
1099
1100 if (!std::isfinite ((*input_)[pt_index].z))
1101 {
1102 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1103 output [idx].curvature = bad_point;
1104 continue;
1105 }
1106
1107 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_);
1108
1109 if (smoothing > 2.0f)
1110 {
1111 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1112 computePointNormal (u, v, pt_index, output [idx]);
1113 }
1114 else
1115 {
1116 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1117 output [idx].curvature = bad_point;
1118 }
1119 }
1120 }
1121 }// border_policy_ == BORDER_POLICY_IGNORE
1122 else if (border_policy_ == BORDER_POLICY_MIRROR)
1123 {
1124 output.is_dense = false;
1125
1126 if (use_depth_dependent_smoothing_)
1127 {
1128 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1129 {
1130 unsigned pt_index = (*indices_)[idx];
1131 unsigned u = pt_index % input_->width;
1132 unsigned v = pt_index / input_->width;
1133
1134 const float depth = (*input_)[pt_index].z;
1135 if (!std::isfinite (depth))
1136 {
1137 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1138 output[idx].curvature = bad_point;
1139 continue;
1140 }
1141
1142 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1143
1144 if (smoothing > 2.0f)
1145 {
1146 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1147 computePointNormalMirror (u, v, pt_index, output [idx]);
1148 }
1149 else
1150 {
1151 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1152 output[idx].curvature = bad_point;
1153 }
1154 }
1155 }
1156 else
1157 {
1158 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1159 {
1160 unsigned pt_index = (*indices_)[idx];
1161 unsigned u = pt_index % input_->width;
1162 unsigned v = pt_index / input_->width;
1163
1164 if (!std::isfinite ((*input_)[pt_index].z))
1165 {
1166 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1167 output [idx].curvature = bad_point;
1168 continue;
1169 }
1170
1171 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_);
1172
1173 if (smoothing > 2.0f)
1174 {
1175 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1176 computePointNormalMirror (u, v, pt_index, output [idx]);
1177 }
1178 else
1179 {
1180 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1181 output [idx].curvature = bad_point;
1182 }
1183 }
1184 }
1185 } // border_policy_ == BORDER_POLICY_MIRROR
1186}
1187
1188//////////////////////////////////////////////////////////////////////////////////////////
1189template <typename PointInT, typename PointOutT> bool
1191{
1192 if (!input_->isOrganized ())
1193 {
1194 PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1195 return (false);
1196 }
1197 return (Feature<PointInT, PointOutT>::initCompute ());
1198}
1199
1200#define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1201
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition exceptions.h:194
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, Dimension, 1 > ElementType
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, second_order_size, 1 > SecondOrderType
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
~IntegralImageNormalEstimation() override
Destructor.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition eigen.hpp:295
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
Definition normal_3d.h:122
bool computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
Definition normal_3d.h:61