Point Cloud Library (PCL) 1.15.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/common/eigen.h> // for eigen33
40#include <pcl/features/integral_image_normal.h>
41
42#include <algorithm>
43
44//////////////////////////////////////////////////////////////////////////////////////////
45template <typename PointInT, typename PointOutT>
47{
48 delete[] diff_x_;
49 delete[] diff_y_;
50 delete[] depth_data_;
51 delete[] distance_map_;
52}
53
54//////////////////////////////////////////////////////////////////////////////////////////
55template <typename PointInT, typename PointOutT> void
57{
58 if (border_policy_ != BORDER_POLICY_IGNORE &&
59 border_policy_ != BORDER_POLICY_MIRROR)
60 PCL_THROW_EXCEPTION (InitFailedException,
61 "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
62
63 if (normal_estimation_method_ != COVARIANCE_MATRIX &&
64 normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
65 normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
66 normal_estimation_method_ != SIMPLE_3D_GRADIENT)
67 PCL_THROW_EXCEPTION (InitFailedException,
68 "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
69
70 // compute derivatives
71 delete[] diff_x_;
72 delete[] diff_y_;
73 delete[] depth_data_;
74 delete[] distance_map_;
75 diff_x_ = nullptr;
76 diff_y_ = nullptr;
77 depth_data_ = nullptr;
78 distance_map_ = nullptr;
79
80 if (normal_estimation_method_ == COVARIANCE_MATRIX)
81 initCovarianceMatrixMethod ();
82 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
83 initAverage3DGradientMethod ();
84 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
85 initAverageDepthChangeMethod ();
86 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
87 initSimple3DGradientMethod ();
88}
89
90
91//////////////////////////////////////////////////////////////////////////////////////////
92template <typename PointInT, typename PointOutT> void
94{
95 rect_width_ = width;
96 rect_width_2_ = width/2;
97 rect_width_4_ = width/4;
98 rect_height_ = height;
99 rect_height_2_ = height/2;
100 rect_height_4_ = height/4;
101}
102
103//////////////////////////////////////////////////////////////////////////////////////////
104template <typename PointInT, typename PointOutT> void
106{
107 // number of DataType entries per element (equal or bigger than dimensions)
108 int element_stride = sizeof (PointInT) / sizeof (float);
109 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
110 int row_stride = element_stride * input_->width;
111
112 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
113
114 integral_image_XYZ_.setSecondOrderComputation (false);
115 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
116
117 init_simple_3d_gradient_ = true;
118 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
119}
120
121//////////////////////////////////////////////////////////////////////////////////////////
122template <typename PointInT, typename PointOutT> void
124{
125 // number of DataType entries per element (equal or bigger than dimensions)
126 int element_stride = sizeof (PointInT) / sizeof (float);
127 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
128 int row_stride = element_stride * input_->width;
130 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
131
132 integral_image_XYZ_.setSecondOrderComputation (true);
133 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
134
135 init_covariance_matrix_ = true;
136 init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
137}
138
139//////////////////////////////////////////////////////////////////////////////////////////
140template <typename PointInT, typename PointOutT> void
142{
143 delete[] diff_x_;
144 delete[] diff_y_;
145 std::size_t data_size = (input_->size () << 2);
146 diff_x_ = new float[data_size]{};
147 diff_y_ = new float[data_size]{};
148
149 // x u x
150 // l x r
151 // x d x
152 const PointInT* point_up = &(input_->points [1]);
153 const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
154 const PointInT* point_lf = &(input_->points [input_->width]);
155 const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
156 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
157 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
158 unsigned diff_skip = 8; // skip last element in row and the first in the next row
159
160 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
161 , point_up += input_->width
162 , point_dn += input_->width
163 , point_lf += input_->width
164 , point_rg += input_->width
165 , diff_x_ptr += diff_skip
166 , diff_y_ptr += diff_skip)
167 {
168 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
169 {
170 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
171 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
172 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
173
174 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
175 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
176 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
177 }
178 }
179
180 // Compute integral images
181 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
182 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
183 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
184 init_average_3d_gradient_ = true;
185}
186
187//////////////////////////////////////////////////////////////////////////////////////////
188template <typename PointInT, typename PointOutT> void
190{
191 // number of DataType entries per element (equal or bigger than dimensions)
192 int element_stride = sizeof (PointInT) / sizeof (float);
193 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
194 int row_stride = element_stride * input_->width;
195
196 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
197
198 // integral image over the z - value
199 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
200 init_depth_change_ = true;
201 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
202}
203
204//////////////////////////////////////////////////////////////////////////////////////////
205template <typename PointInT, typename PointOutT> void
207 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
208{
209 float bad_point = std::numeric_limits<float>::quiet_NaN ();
210
211 if (normal_estimation_method_ == COVARIANCE_MATRIX)
212 {
213 if (!init_covariance_matrix_)
214 initCovarianceMatrixMethod ();
215
216 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
217
218 // no valid points within the rectangular region?
219 if (count == 0)
220 {
221 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
222 return;
223 }
224
225 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
226 Eigen::Vector3f center;
228 center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
229 so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
230
231 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
232 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
233 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
234 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
235 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
236 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
237 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
238 float eigen_value;
239 Eigen::Vector3f eigen_vector;
240 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
241 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
242 normal.getNormalVector3fMap () = eigen_vector;
243
244 // Compute the curvature surface change
245 if (eigen_value > 0.0)
246 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
247 else
248 normal.curvature = 0;
249
250 return;
251 }
252 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
253 {
254 if (!init_average_3d_gradient_)
255 initAverage3DGradientMethod ();
256
257 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
258 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
259 if (count_x == 0 || count_y == 0)
260 {
261 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
262 return;
263 }
264 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
265 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
266
267 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
268 double normal_length = normal_vector.squaredNorm ();
269 if (normal_length == 0.0f)
270 {
271 normal.getNormalVector3fMap ().setConstant (bad_point);
272 normal.curvature = bad_point;
273 return;
274 }
275
276 normal_vector /= sqrt (normal_length);
277
278 float nx = static_cast<float> (normal_vector [0]);
279 float ny = static_cast<float> (normal_vector [1]);
280 float nz = static_cast<float> (normal_vector [2]);
281
282 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
283
284 normal.normal_x = nx;
285 normal.normal_y = ny;
286 normal.normal_z = nz;
287 normal.curvature = bad_point;
288 return;
289 }
290 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
291 {
292 if (!init_depth_change_)
293 initAverageDepthChangeMethod ();
294
295 // width and height are at least 3 x 3
296 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
297 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
298 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
299 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
300
301 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
302 {
303 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
304 return;
305 }
306
307 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);
308 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);
309 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);
310 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);
312 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
313 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
314 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
315 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
316
317 const float mean_x_z = mean_R_z - mean_L_z;
318 const float mean_y_z = mean_D_z - mean_U_z;
320 const float mean_x_x = pointR.x - pointL.x;
321 const float mean_x_y = pointR.y - pointL.y;
322 const float mean_y_x = pointD.x - pointU.x;
323 const float mean_y_y = pointD.y - pointU.y;
324
325 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
326 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
327 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
328
329 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
330
331 if (normal_length == 0.0f)
332 {
333 normal.getNormalVector3fMap ().setConstant (bad_point);
334 normal.curvature = bad_point;
335 return;
336 }
337
338 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
339
340 const float scale = 1.0f / std::sqrt (normal_length);
341
342 normal.normal_x = normal_x * scale;
343 normal.normal_y = normal_y * scale;
344 normal.normal_z = normal_z * scale;
345 normal.curvature = bad_point;
346
347 return;
348 }
349 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
350 {
351 if (!init_simple_3d_gradient_)
352 initSimple3DGradientMethod ();
353
354 // this method does not work if lots of NaNs are in the neighborhood of the point
355 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
356 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
357
358 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
359 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
360 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
361 double normal_length = normal_vector.squaredNorm ();
362 if (normal_length == 0.0f)
363 {
364 normal.getNormalVector3fMap ().setConstant (bad_point);
365 normal.curvature = bad_point;
366 return;
367 }
368
369 normal_vector /= sqrt (normal_length);
370
371 float nx = static_cast<float> (normal_vector [0]);
372 float ny = static_cast<float> (normal_vector [1]);
373 float nz = static_cast<float> (normal_vector [2]);
374
375 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
376
377 normal.normal_x = nx;
378 normal.normal_y = ny;
379 normal.normal_z = nz;
380 normal.curvature = bad_point;
381 return;
382 }
383
384 normal.getNormalVector3fMap ().setConstant (bad_point);
385 normal.curvature = bad_point;
386 return;
387}
388
389//////////////////////////////////////////////////////////////////////////////////////////
390template <typename T>
391void
392sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
393 const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
394 T & result)
395{
396 if (start_x < 0)
397 {
398 if (start_y < 0)
399 {
400 result += f (0, 0, end_x, end_y);
401 result += f (0, 0, -start_x, -start_y);
402 result += f (0, 0, -start_x, end_y);
403 result += f (0, 0, end_x, -start_y);
404 }
405 else if (end_y >= height)
406 {
407 result += f (0, start_y, end_x, height-1);
408 result += f (0, start_y, -start_x, height-1);
409 result += f (0, height-(end_y-(height-1)), end_x, height-1);
410 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
411 }
412 else
413 {
414 result += f (0, start_y, end_x, end_y);
415 result += f (0, start_y, -start_x, end_y);
416 }
417 }
418 else if (start_y < 0)
419 {
420 if (end_x >= width)
421 {
422 result += f (start_x, 0, width-1, end_y);
423 result += f (start_x, 0, width-1, -start_y);
424 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
425 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
426 }
427 else
428 {
429 result += f (start_x, 0, end_x, end_y);
430 result += f (start_x, 0, end_x, -start_y);
431 }
432 }
433 else if (end_x >= width)
434 {
435 if (end_y >= height)
436 {
437 result += f (start_x, start_y, width-1, height-1);
438 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
439 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
440 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
441 }
442 else
443 {
444 result += f (start_x, start_y, width-1, end_y);
445 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
446 }
447 }
448 else if (end_y >= height)
449 {
450 result += f (start_x, start_y, end_x, height-1);
451 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
452 }
453 else
454 {
455 result += f (start_x, start_y, end_x, end_y);
456 }
457}
458
459//////////////////////////////////////////////////////////////////////////////////////////
460template <typename PointInT, typename PointOutT> void
462 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
463{
464 float bad_point = std::numeric_limits<float>::quiet_NaN ();
465
466 const int width = input_->width;
467 const int height = input_->height;
468
469 // ==============================================================
470 if (normal_estimation_method_ == COVARIANCE_MATRIX)
471 {
472 if (!init_covariance_matrix_)
473 initCovarianceMatrixMethod ();
474
475 const int start_x = pos_x - rect_width_2_;
476 const int start_y = pos_y - rect_height_2_;
477 const int end_x = start_x + rect_width_;
478 const int end_y = start_y + rect_height_;
479
480 unsigned count = 0;
481 auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
482 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
483
484 // no valid points within the rectangular region?
485 if (count == 0)
486 {
487 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
488 return;
489 }
490
491 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
492 Eigen::Vector3f center;
494 typename IntegralImage2D<float, 3>::ElementType tmp_center;
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:197
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:67
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