Point Cloud Library (PCL) 1.12.1
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
39#ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
40#define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
41
42#include <pcl/features/integral_image_normal.h>
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;
129
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 std::size_t data_size = (input_->size () << 2);
144 diff_x_ = new float[data_size];
145 diff_y_ = new float[data_size];
147 memset (diff_x_, 0, sizeof(float) * data_size);
148 memset (diff_y_, 0, sizeof(float) * data_size);
149
150 // x u x
151 // l x r
152 // x d x
153 const PointInT* point_up = &(input_->points [1]);
154 const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
155 const PointInT* point_lf = &(input_->points [input_->width]);
156 const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
157 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
158 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
159 unsigned diff_skip = 8; // skip last element in row and the first in the next row
160
161 for (std::size_t ri = 1; ri < input_->height - 1; ++ri
162 , point_up += input_->width
163 , point_dn += input_->width
164 , point_lf += input_->width
165 , point_rg += input_->width
166 , diff_x_ptr += diff_skip
167 , diff_y_ptr += diff_skip)
168 {
169 for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
170 {
171 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
172 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
173 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
174
175 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
176 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
177 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
178 }
179 }
180
181 // Compute integral images
182 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
183 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
184 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
185 init_average_3d_gradient_ = true;
186}
187
188//////////////////////////////////////////////////////////////////////////////////////////
189template <typename PointInT, typename PointOutT> void
191{
192 // number of DataType entries per element (equal or bigger than dimensions)
193 int element_stride = sizeof (PointInT) / sizeof (float);
194 // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
195 int row_stride = element_stride * input_->width;
196
197 const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
198
199 // integral image over the z - value
200 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
201 init_depth_change_ = true;
202 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
203}
204
205//////////////////////////////////////////////////////////////////////////////////////////
206template <typename PointInT, typename PointOutT> void
208 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
209{
210 float bad_point = std::numeric_limits<float>::quiet_NaN ();
211
212 if (normal_estimation_method_ == COVARIANCE_MATRIX)
213 {
214 if (!init_covariance_matrix_)
215 initCovarianceMatrixMethod ();
216
217 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
218
219 // no valid points within the rectangular region?
220 if (count == 0)
221 {
222 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
223 return;
224 }
225
226 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
227 Eigen::Vector3f center;
229 center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
230 so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
231
232 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
233 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
234 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
235 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
236 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
237 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
238 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
239 float eigen_value;
240 Eigen::Vector3f eigen_vector;
241 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
242 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
243 normal.getNormalVector3fMap () = eigen_vector;
244
245 // Compute the curvature surface change
246 if (eigen_value > 0.0)
247 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
248 else
249 normal.curvature = 0;
250
251 return;
252 }
253 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
254 {
255 if (!init_average_3d_gradient_)
256 initAverage3DGradientMethod ();
257
258 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
259 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
260 if (count_x == 0 || count_y == 0)
261 {
262 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
263 return;
264 }
265 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
266 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
267
268 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
269 double normal_length = normal_vector.squaredNorm ();
270 if (normal_length == 0.0f)
271 {
272 normal.getNormalVector3fMap ().setConstant (bad_point);
273 normal.curvature = bad_point;
274 return;
275 }
276
277 normal_vector /= sqrt (normal_length);
278
279 float nx = static_cast<float> (normal_vector [0]);
280 float ny = static_cast<float> (normal_vector [1]);
281 float nz = static_cast<float> (normal_vector [2]);
282
283 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
284
285 normal.normal_x = nx;
286 normal.normal_y = ny;
287 normal.normal_z = nz;
288 normal.curvature = bad_point;
289 return;
290 }
291 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
292 {
293 if (!init_depth_change_)
294 initAverageDepthChangeMethod ();
295
296 // width and height are at least 3 x 3
297 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
298 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
299 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
300 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
301
302 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
303 {
304 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
305 return;
306 }
307
308 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);
309 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);
310 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);
311 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
313 PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
314 PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
315 PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
316 PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
317
318 const float mean_x_z = mean_R_z - mean_L_z;
319 const float mean_y_z = mean_D_z - mean_U_z;
321 const float mean_x_x = pointR.x - pointL.x;
322 const float mean_x_y = pointR.y - pointL.y;
323 const float mean_y_x = pointD.x - pointU.x;
324 const float mean_y_y = pointD.y - pointU.y;
325
326 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
327 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
328 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
329
330 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
331
332 if (normal_length == 0.0f)
333 {
334 normal.getNormalVector3fMap ().setConstant (bad_point);
335 normal.curvature = bad_point;
336 return;
337 }
338
339 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
341 const float scale = 1.0f / std::sqrt (normal_length);
342
343 normal.normal_x = normal_x * scale;
344 normal.normal_y = normal_y * scale;
345 normal.normal_z = normal_z * scale;
346 normal.curvature = bad_point;
347
348 return;
349 }
350 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
351 {
352 if (!init_simple_3d_gradient_)
353 initSimple3DGradientMethod ();
354
355 // this method does not work if lots of NaNs are in the neighborhood of the point
356 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
357 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
358
359 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
360 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
361 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
362 double normal_length = normal_vector.squaredNorm ();
363 if (normal_length == 0.0f)
364 {
365 normal.getNormalVector3fMap ().setConstant (bad_point);
366 normal.curvature = bad_point;
367 return;
368 }
369
370 normal_vector /= sqrt (normal_length);
371
372 float nx = static_cast<float> (normal_vector [0]);
373 float ny = static_cast<float> (normal_vector [1]);
374 float nz = static_cast<float> (normal_vector [2]);
375
376 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
377
378 normal.normal_x = nx;
379 normal.normal_y = ny;
380 normal.normal_z = nz;
381 normal.curvature = bad_point;
382 return;
383 }
384
385 normal.getNormalVector3fMap ().setConstant (bad_point);
386 normal.curvature = bad_point;
387 return;
388}
389
390//////////////////////////////////////////////////////////////////////////////////////////
391template <typename T>
392void
393sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
394 const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
395 T & result)
396{
397 if (start_x < 0)
398 {
399 if (start_y < 0)
400 {
401 result += f (0, 0, end_x, end_y);
402 result += f (0, 0, -start_x, -start_y);
403 result += f (0, 0, -start_x, end_y);
404 result += f (0, 0, end_x, -start_y);
405 }
406 else if (end_y >= height)
407 {
408 result += f (0, start_y, end_x, height-1);
409 result += f (0, start_y, -start_x, height-1);
410 result += f (0, height-(end_y-(height-1)), end_x, height-1);
411 result += f (0, height-(end_y-(height-1)), -start_x, height-1);
412 }
413 else
414 {
415 result += f (0, start_y, end_x, end_y);
416 result += f (0, start_y, -start_x, end_y);
417 }
418 }
419 else if (start_y < 0)
420 {
421 if (end_x >= width)
422 {
423 result += f (start_x, 0, width-1, end_y);
424 result += f (start_x, 0, width-1, -start_y);
425 result += f (width-(end_x-(width-1)), 0, width-1, end_y);
426 result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
427 }
428 else
429 {
430 result += f (start_x, 0, end_x, end_y);
431 result += f (start_x, 0, end_x, -start_y);
432 }
433 }
434 else if (end_x >= width)
435 {
436 if (end_y >= height)
437 {
438 result += f (start_x, start_y, width-1, height-1);
439 result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
440 result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
441 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
442 }
443 else
444 {
445 result += f (start_x, start_y, width-1, end_y);
446 result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
447 }
448 }
449 else if (end_y >= height)
450 {
451 result += f (start_x, start_y, end_x, height-1);
452 result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
453 }
454 else
455 {
456 result += f (start_x, start_y, end_x, end_y);
457 }
458}
459
460//////////////////////////////////////////////////////////////////////////////////////////
461template <typename PointInT, typename PointOutT> void
463 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
464{
465 float bad_point = std::numeric_limits<float>::quiet_NaN ();
466
467 const int width = input_->width;
468 const int height = input_->height;
469
470 // ==============================================================
471 if (normal_estimation_method_ == COVARIANCE_MATRIX)
472 {
473 if (!init_covariance_matrix_)
474 initCovarianceMatrixMethod ();
475
476 const int start_x = pos_x - rect_width_2_;
477 const int start_y = pos_y - rect_height_2_;
478 const int end_x = start_x + rect_width_;
479 const int end_y = start_y + rect_height_;
480
481 unsigned count = 0;
482 auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
483 sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
484
485 // no valid points within the rectangular region?
486 if (count == 0)
487 {
488 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
489 return;
490 }
491
492 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
493 Eigen::Vector3f center;
495 typename IntegralImage2D<float, 3>::ElementType tmp_center;
496 typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
497
498 center[0] = 0;
499 center[1] = 0;
500 center[2] = 0;
501 tmp_center[0] = 0;
502 tmp_center[1] = 0;
503 tmp_center[2] = 0;
504 so_elements[0] = 0;
505 so_elements[1] = 0;
506 so_elements[2] = 0;
507 so_elements[3] = 0;
508 so_elements[4] = 0;
509 so_elements[5] = 0;
510
511 auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
512 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
513 auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
514 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
515
516 center[0] = float (tmp_center[0]);
517 center[1] = float (tmp_center[1]);
518 center[2] = float (tmp_center[2]);
519
520 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
521 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
522 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
523 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
524 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
525 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
526 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
527 float eigen_value;
528 Eigen::Vector3f eigen_vector;
529 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
530 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
531 normal.getNormalVector3fMap () = eigen_vector;
532
533 // Compute the curvature surface change
534 if (eigen_value > 0.0)
535 normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
536 else
537 normal.curvature = 0;
538
539 return;
540 }
541 // =======================================================
542 if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
543 {
544 if (!init_average_3d_gradient_)
545 initAverage3DGradientMethod ();
546
547 const int start_x = pos_x - rect_width_2_;
548 const int start_y = pos_y - rect_height_2_;
549 const int end_x = start_x + rect_width_;
550 const int end_y = start_y + rect_height_;
551
552 unsigned count_x = 0;
553 unsigned count_y = 0;
554
555 auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
557 auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
558 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
559
560
561 if (count_x == 0 || count_y == 0)
562 {
563 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
564 return;
565 }
566 Eigen::Vector3d gradient_x (0, 0, 0);
567 Eigen::Vector3d gradient_y (0, 0, 0);
568
569 auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
570 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
571 auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
572 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
573
574
575 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
576 double normal_length = normal_vector.squaredNorm ();
577 if (normal_length == 0.0f)
578 {
579 normal.getNormalVector3fMap ().setConstant (bad_point);
580 normal.curvature = bad_point;
581 return;
582 }
583
584 normal_vector /= sqrt (normal_length);
585
586 float nx = static_cast<float> (normal_vector [0]);
587 float ny = static_cast<float> (normal_vector [1]);
588 float nz = static_cast<float> (normal_vector [2]);
589
590 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
591
592 normal.normal_x = nx;
593 normal.normal_y = ny;
594 normal.normal_z = nz;
595 normal.curvature = bad_point;
596 return;
597 }
598 // ======================================================
599 if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
600 {
601 if (!init_depth_change_)
602 initAverageDepthChangeMethod ();
603
604 int point_index_L_x = pos_x - rect_width_4_ - 1;
605 int point_index_L_y = pos_y;
606 int point_index_R_x = pos_x + rect_width_4_ + 1;
607 int point_index_R_y = pos_y;
608 int point_index_U_x = pos_x - 1;
609 int point_index_U_y = pos_y - rect_height_4_;
610 int point_index_D_x = pos_x + 1;
611 int point_index_D_y = pos_y + rect_height_4_;
612
613 if (point_index_L_x < 0)
614 point_index_L_x = -point_index_L_x;
615 if (point_index_U_x < 0)
616 point_index_U_x = -point_index_U_x;
617 if (point_index_U_y < 0)
618 point_index_U_y = -point_index_U_y;
619
620 if (point_index_R_x >= width)
621 point_index_R_x = width-(point_index_R_x-(width-1));
622 if (point_index_D_x >= width)
623 point_index_D_x = width-(point_index_D_x-(width-1));
624 if (point_index_D_y >= height)
625 point_index_D_y = height-(point_index_D_y-(height-1));
626
627 const int start_x_L = pos_x - rect_width_2_;
628 const int start_y_L = pos_y - rect_height_4_;
629 const int end_x_L = start_x_L + rect_width_2_;
630 const int end_y_L = start_y_L + rect_height_2_;
631
632 const int start_x_R = pos_x + 1;
633 const int start_y_R = pos_y - rect_height_4_;
634 const int end_x_R = start_x_R + rect_width_2_;
635 const int end_y_R = start_y_R + rect_height_2_;
636
637 const int start_x_U = pos_x - rect_width_4_;
638 const int start_y_U = pos_y - rect_height_2_;
639 const int end_x_U = start_x_U + rect_width_2_;
640 const int end_y_U = start_y_U + rect_height_2_;
641
642 const int start_x_D = pos_x - rect_width_4_;
643 const int start_y_D = pos_y + 1;
644 const int end_x_D = start_x_D + rect_width_2_;
645 const int end_y_D = start_y_D + rect_height_2_;
646
647 unsigned count_L_z = 0;
648 unsigned count_R_z = 0;
649 unsigned count_U_z = 0;
650 unsigned count_D_z = 0;
651
652 auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
653 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
654 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
655 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
656 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
657
658 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
659 {
660 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
661 return;
662 }
663
664 float mean_L_z = 0;
665 float mean_R_z = 0;
666 float mean_U_z = 0;
667 float mean_D_z = 0;
668
669 auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
670 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
671 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
672 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
673 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
674
675 mean_L_z /= float (count_L_z);
676 mean_R_z /= float (count_R_z);
677 mean_U_z /= float (count_U_z);
678 mean_D_z /= float (count_D_z);
679
680
681 PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
682 PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
683 PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
684 PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
685
686 const float mean_x_z = mean_R_z - mean_L_z;
687 const float mean_y_z = mean_D_z - mean_U_z;
688
689 const float mean_x_x = pointR.x - pointL.x;
690 const float mean_x_y = pointR.y - pointL.y;
691 const float mean_y_x = pointD.x - pointU.x;
692 const float mean_y_y = pointD.y - pointU.y;
693
694 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
695 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
696 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
697
698 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
699
700 if (normal_length == 0.0f)
701 {
702 normal.getNormalVector3fMap ().setConstant (bad_point);
703 normal.curvature = bad_point;
704 return;
705 }
706
707 flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
708
709 const float scale = 1.0f / std::sqrt (normal_length);
710
711 normal.normal_x = normal_x * scale;
712 normal.normal_y = normal_y * scale;
713 normal.normal_z = normal_z * scale;
714 normal.curvature = bad_point;
715
716 return;
717 }
718 // ========================================================
719 if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
720 {
721 PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
722 }
723
724 normal.getNormalVector3fMap ().setConstant (bad_point);
725 normal.curvature = bad_point;
726 return;
727}
728
729//////////////////////////////////////////////////////////////////////////////////////////
730template <typename PointInT, typename PointOutT> void
732{
733 output.sensor_origin_ = input_->sensor_origin_;
734 output.sensor_orientation_ = input_->sensor_orientation_;
735
736 float bad_point = std::numeric_limits<float>::quiet_NaN ();
737
738 // compute depth-change map
739 unsigned char * depthChangeMap = new unsigned char[input_->size ()];
740 memset (depthChangeMap, 255, input_->size ());
741
742 unsigned index = 0;
743 for (unsigned int ri = 0; ri < input_->height-1; ++ri)
744 {
745 for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
746 {
747 index = ri * input_->width + ci;
748
749 const float depth = input_->points [index].z;
750 const float depthR = input_->points [index + 1].z;
751 const float depthD = input_->points [index + input_->width].z;
752
753 //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f);
754 const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
755
756 if (std::fabs (depth - depthR) > depthDependendDepthChange
757 || !std::isfinite (depth) || !std::isfinite (depthR))
758 {
759 depthChangeMap[index] = 0;
760 depthChangeMap[index+1] = 0;
761 }
762 if (std::fabs (depth - depthD) > depthDependendDepthChange
763 || !std::isfinite (depth) || !std::isfinite (depthD))
764 {
765 depthChangeMap[index] = 0;
766 depthChangeMap[index + input_->width] = 0;
767 }
768 }
769 }
770
771 // compute distance map
772 //float *distanceMap = new float[input_->size ()];
773 delete[] distance_map_;
774 distance_map_ = new float[input_->size ()];
775 float *distanceMap = distance_map_;
776 for (std::size_t index = 0; index < input_->size (); ++index)
777 {
778 if (depthChangeMap[index] == 0)
779 distanceMap[index] = 0.0f;
780 else
781 distanceMap[index] = static_cast<float> (input_->width + input_->height);
782 }
783
784 // first pass
785 float* previous_row = distanceMap;
786 float* current_row = previous_row + input_->width;
787 for (std::size_t ri = 1; ri < input_->height; ++ri)
788 {
789 for (std::size_t ci = 1; ci < input_->width; ++ci)
790 {
791 const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
792 const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
793 const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
794 const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
795 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
796
797 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
798
799 if (minValue < center)
800 current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
801 }
802 previous_row = current_row;
803 current_row += input_->width;
804 }
805
806 float* next_row = distanceMap + input_->width * (input_->height - 1);
807 current_row = next_row - input_->width;
808 // second pass
809 for (int ri = input_->height-2; ri >= 0; --ri)
810 {
811 for (int ci = input_->width-2; ci >= 0; --ci)
812 {
813 const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
814 const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
815 const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
816 const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
817 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
818
819 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
820
821 if (minValue < center)
822 current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
823 }
824 next_row = current_row;
825 current_row -= input_->width;
826 }
827
828 if (indices_->size () < input_->size ())
829 computeFeaturePart (distanceMap, bad_point, output);
830 else
831 computeFeatureFull (distanceMap, bad_point, output);
832
833 delete[] depthChangeMap;
834}
835
836//////////////////////////////////////////////////////////////////////////////////////////
837template <typename PointInT, typename PointOutT> void
839 const float &bad_point,
840 PointCloudOut &output)
841{
842 unsigned index = 0;
843
844 if (border_policy_ == BORDER_POLICY_IGNORE)
845 {
846 // Set all normals that we do not touch to NaN
847 // top and bottom borders
848 // That sets the output density to false!
849 output.is_dense = false;
850 unsigned border = int(normal_smoothing_size_);
851 PointOutT* vec1 = &output [0];
852 PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
853
854 std::size_t count = border * input_->width;
855 for (std::size_t idx = 0; idx < count; ++idx)
856 {
857 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
858 vec1 [idx].curvature = bad_point;
859 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
860 vec2 [idx].curvature = bad_point;
861 }
862
863 // left and right borders actually columns
864 vec1 = &output [border * input_->width];
865 vec2 = vec1 + input_->width - border;
866 for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
867 {
868 for (std::size_t ci = 0; ci < border; ++ci)
869 {
870 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
871 vec1 [ci].curvature = bad_point;
872 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
873 vec2 [ci].curvature = bad_point;
874 }
875 }
876
877 if (use_depth_dependent_smoothing_)
878 {
879 index = border + input_->width * border;
880 unsigned skip = (border << 1);
881 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
882 {
883 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
884 {
885 index = ri * input_->width + ci;
886
887 const float depth = (*input_)[index].z;
888 if (!std::isfinite (depth))
889 {
890 output[index].getNormalVector3fMap ().setConstant (bad_point);
891 output[index].curvature = bad_point;
892 continue;
893 }
894
895 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
896
897 if (smoothing > 2.0f)
898 {
899 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
900 computePointNormal (ci, ri, index, output [index]);
901 }
902 else
903 {
904 output[index].getNormalVector3fMap ().setConstant (bad_point);
905 output[index].curvature = bad_point;
906 }
907 }
908 }
909 }
910 else
911 {
912 float smoothing_constant = normal_smoothing_size_;
913
914 index = border + input_->width * border;
915 unsigned skip = (border << 1);
916 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
917 {
918 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
919 {
920 index = ri * input_->width + ci;
921
922 if (!std::isfinite ((*input_)[index].z))
923 {
924 output [index].getNormalVector3fMap ().setConstant (bad_point);
925 output [index].curvature = bad_point;
926 continue;
927 }
928
929 float smoothing = (std::min)(distanceMap[index], smoothing_constant);
930
931 if (smoothing > 2.0f)
932 {
933 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
934 computePointNormal (ci, ri, index, output [index]);
935 }
936 else
937 {
938 output [index].getNormalVector3fMap ().setConstant (bad_point);
939 output [index].curvature = bad_point;
940 }
941 }
942 }
943 }
944 }
945 else if (border_policy_ == BORDER_POLICY_MIRROR)
946 {
947 output.is_dense = false;
948
949 if (use_depth_dependent_smoothing_)
950 {
951 //index = 0;
952 //unsigned skip = 0;
953 //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
954 for (unsigned ri = 0; ri < input_->height; ++ri)
955 {
956 //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
957 for (unsigned ci = 0; ci < input_->width; ++ci)
958 {
959 index = ri * input_->width + ci;
960
961 const float depth = (*input_)[index].z;
962 if (!std::isfinite (depth))
963 {
964 output[index].getNormalVector3fMap ().setConstant (bad_point);
965 output[index].curvature = bad_point;
966 continue;
967 }
968
969 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
970
971 if (smoothing > 2.0f)
972 {
973 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
974 computePointNormalMirror (ci, ri, index, output [index]);
975 }
976 else
977 {
978 output[index].getNormalVector3fMap ().setConstant (bad_point);
979 output[index].curvature = bad_point;
980 }
981 }
982 }
983 }
984 else
985 {
986 float smoothing_constant = normal_smoothing_size_;
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], smoothing_constant);
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 unsigned border = int(normal_smoothing_size_);
1033 unsigned bottom = input_->height > border ? input_->height - border : 0;
1034 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 float smoothing_constant = normal_smoothing_size_;
1081 // Iterating over the entire index vector
1082 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1083 {
1084 unsigned pt_index = (*indices_)[idx];
1085 unsigned u = pt_index % input_->width;
1086 unsigned v = pt_index / input_->width;
1087 if (v < border || v > bottom)
1088 {
1089 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1090 output[idx].curvature = bad_point;
1091 continue;
1092 }
1093
1094 if (u < border || u > right)
1095 {
1096 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1097 output[idx].curvature = bad_point;
1098 continue;
1099 }
1100
1101 if (!std::isfinite ((*input_)[pt_index].z))
1102 {
1103 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1104 output [idx].curvature = bad_point;
1105 continue;
1106 }
1107
1108 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1109
1110 if (smoothing > 2.0f)
1111 {
1112 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1113 computePointNormal (u, v, pt_index, output [idx]);
1114 }
1115 else
1116 {
1117 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1118 output [idx].curvature = bad_point;
1119 }
1120 }
1121 }
1122 }// border_policy_ == BORDER_POLICY_IGNORE
1123 else if (border_policy_ == BORDER_POLICY_MIRROR)
1124 {
1125 output.is_dense = false;
1126
1127 if (use_depth_dependent_smoothing_)
1128 {
1129 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1130 {
1131 unsigned pt_index = (*indices_)[idx];
1132 unsigned u = pt_index % input_->width;
1133 unsigned v = pt_index / input_->width;
1134
1135 const float depth = (*input_)[pt_index].z;
1136 if (!std::isfinite (depth))
1137 {
1138 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1139 output[idx].curvature = bad_point;
1140 continue;
1141 }
1142
1143 float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1144
1145 if (smoothing > 2.0f)
1146 {
1147 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1148 computePointNormalMirror (u, v, pt_index, output [idx]);
1149 }
1150 else
1151 {
1152 output[idx].getNormalVector3fMap ().setConstant (bad_point);
1153 output[idx].curvature = bad_point;
1154 }
1155 }
1156 }
1157 else
1158 {
1159 float smoothing_constant = normal_smoothing_size_;
1160 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1161 {
1162 unsigned pt_index = (*indices_)[idx];
1163 unsigned u = pt_index % input_->width;
1164 unsigned v = pt_index / input_->width;
1165
1166 if (!std::isfinite ((*input_)[pt_index].z))
1167 {
1168 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1169 output [idx].curvature = bad_point;
1170 continue;
1171 }
1172
1173 float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1174
1175 if (smoothing > 2.0f)
1176 {
1177 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1178 computePointNormalMirror (u, v, pt_index, output [idx]);
1179 }
1180 else
1181 {
1182 output [idx].getNormalVector3fMap ().setConstant (bad_point);
1183 output [idx].curvature = bad_point;
1184 }
1185 }
1186 }
1187 } // border_policy_ == BORDER_POLICY_MIRROR
1188}
1189
1190//////////////////////////////////////////////////////////////////////////////////////////
1191template <typename PointInT, typename PointOutT> bool
1193{
1194 if (!input_->isOrganized ())
1195 {
1196 PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1197 return (false);
1198 }
1199 return (Feature<PointInT, PointOutT>::initCompute ());
1200}
1201
1202#define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1203
1204#endif
1205
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
Determines an integral image representation for a given organized data array.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
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.
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
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
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:296
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