Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ 00040 #define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ 00041 00042 #include <pcl/features/integral_image_normal.h> 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointInT, typename PointOutT> 00046 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::~IntegralImageNormalEstimation () 00047 { 00048 if (diff_x_ != NULL) delete[] diff_x_; 00049 if (diff_y_ != NULL) delete[] diff_y_; 00050 if (depth_data_ != NULL) delete[] depth_data_; 00051 if (distance_map_ != NULL) delete[] distance_map_; 00052 } 00053 00054 ////////////////////////////////////////////////////////////////////////////////////////// 00055 template <typename PointInT, typename PointOutT> void 00056 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initData () 00057 { 00058 if (border_policy_ != BORDER_POLICY_IGNORE && 00059 border_policy_ != BORDER_POLICY_MIRROR) 00060 PCL_THROW_EXCEPTION (InitFailedException, 00061 "[pcl::IntegralImageNormalEstimation::initData] unknown border policy."); 00062 00063 if (normal_estimation_method_ != COVARIANCE_MATRIX && 00064 normal_estimation_method_ != AVERAGE_3D_GRADIENT && 00065 normal_estimation_method_ != AVERAGE_DEPTH_CHANGE && 00066 normal_estimation_method_ != SIMPLE_3D_GRADIENT) 00067 PCL_THROW_EXCEPTION (InitFailedException, 00068 "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method."); 00069 00070 // compute derivatives 00071 if (diff_x_ != NULL) delete[] diff_x_; 00072 if (diff_y_ != NULL) delete[] diff_y_; 00073 if (depth_data_ != NULL) delete[] depth_data_; 00074 if (distance_map_ != NULL) delete[] distance_map_; 00075 diff_x_ = NULL; 00076 diff_y_ = NULL; 00077 depth_data_ = NULL; 00078 distance_map_ = NULL; 00079 00080 if (normal_estimation_method_ == COVARIANCE_MATRIX) 00081 initCovarianceMatrixMethod (); 00082 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 00083 initAverage3DGradientMethod (); 00084 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 00085 initAverageDepthChangeMethod (); 00086 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) 00087 initSimple3DGradientMethod (); 00088 } 00089 00090 00091 ////////////////////////////////////////////////////////////////////////////////////////// 00092 template <typename PointInT, typename PointOutT> void 00093 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::setRectSize (const int width, const int height) 00094 { 00095 rect_width_ = width; 00096 rect_width_2_ = width/2; 00097 rect_width_4_ = width/4; 00098 rect_height_ = height; 00099 rect_height_2_ = height/2; 00100 rect_height_4_ = height/4; 00101 } 00102 00103 ////////////////////////////////////////////////////////////////////////////////////////// 00104 template <typename PointInT, typename PointOutT> void 00105 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initSimple3DGradientMethod () 00106 { 00107 // number of DataType entries per element (equal or bigger than dimensions) 00108 int element_stride = sizeof (PointInT) / sizeof (float); 00109 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00110 int row_stride = element_stride * input_->width; 00111 00112 const float *data_ = reinterpret_cast<const float*> (&input_->points[0]); 00113 00114 integral_image_XYZ_.setSecondOrderComputation (false); 00115 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride); 00116 00117 init_simple_3d_gradient_ = true; 00118 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false; 00119 } 00120 00121 ////////////////////////////////////////////////////////////////////////////////////////// 00122 template <typename PointInT, typename PointOutT> void 00123 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCovarianceMatrixMethod () 00124 { 00125 // number of DataType entries per element (equal or bigger than dimensions) 00126 int element_stride = sizeof (PointInT) / sizeof (float); 00127 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00128 int row_stride = element_stride * input_->width; 00129 00130 const float *data_ = reinterpret_cast<const float*> (&input_->points[0]); 00131 00132 integral_image_XYZ_.setSecondOrderComputation (true); 00133 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride); 00134 00135 init_covariance_matrix_ = true; 00136 init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false; 00137 } 00138 00139 ////////////////////////////////////////////////////////////////////////////////////////// 00140 template <typename PointInT, typename PointOutT> void 00141 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverage3DGradientMethod () 00142 { 00143 size_t data_size = (input_->points.size () << 2); 00144 diff_x_ = new float[data_size]; 00145 diff_y_ = new float[data_size]; 00146 00147 memset (diff_x_, 0, sizeof(float) * data_size); 00148 memset (diff_y_, 0, sizeof(float) * data_size); 00149 00150 // x u x 00151 // l x r 00152 // x d x 00153 const PointInT* point_up = &(input_->points [1]); 00154 const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]); 00155 const PointInT* point_lf = &(input_->points [input_->width]); 00156 const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]); 00157 float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2); 00158 float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2); 00159 unsigned diff_skip = 8; // skip last element in row and the first in the next row 00160 00161 for (size_t ri = 1; ri < input_->height - 1; ++ri 00162 , point_up += input_->width 00163 , point_dn += input_->width 00164 , point_lf += input_->width 00165 , point_rg += input_->width 00166 , diff_x_ptr += diff_skip 00167 , diff_y_ptr += diff_skip) 00168 { 00169 for (size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4) 00170 { 00171 diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x; 00172 diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y; 00173 diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z; 00174 00175 diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x; 00176 diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y; 00177 diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z; 00178 } 00179 } 00180 00181 // Compute integral images 00182 integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2); 00183 integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2); 00184 init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false; 00185 init_average_3d_gradient_ = true; 00186 } 00187 00188 ////////////////////////////////////////////////////////////////////////////////////////// 00189 template <typename PointInT, typename PointOutT> void 00190 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverageDepthChangeMethod () 00191 { 00192 // number of DataType entries per element (equal or bigger than dimensions) 00193 int element_stride = sizeof (PointInT) / sizeof (float); 00194 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00195 int row_stride = element_stride * input_->width; 00196 00197 const float *data_ = reinterpret_cast<const float*> (&input_->points[0]); 00198 00199 // integral image over the z - value 00200 integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride); 00201 init_depth_change_ = true; 00202 init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false; 00203 } 00204 00205 ////////////////////////////////////////////////////////////////////////////////////////// 00206 template <typename PointInT, typename PointOutT> void 00207 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal ( 00208 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) 00209 { 00210 float bad_point = std::numeric_limits<float>::quiet_NaN (); 00211 00212 if (normal_estimation_method_ == COVARIANCE_MATRIX) 00213 { 00214 if (!init_covariance_matrix_) 00215 initCovarianceMatrixMethod (); 00216 00217 unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_); 00218 00219 // no valid points within the rectangular reagion? 00220 if (count == 0) 00221 { 00222 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00223 return; 00224 } 00225 00226 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00227 Eigen::Vector3f center; 00228 typename IntegralImage2D<float, 3>::SecondOrderType so_elements; 00229 center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> (); 00230 so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); 00231 00232 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]); 00233 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]); 00234 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]); 00235 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]); 00236 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]); 00237 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]); 00238 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count); 00239 float eigen_value; 00240 Eigen::Vector3f eigen_vector; 00241 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); 00242 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); 00243 normal.getNormalVector3fMap () = eigen_vector; 00244 00245 // Compute the curvature surface change 00246 if (eigen_value > 0.0) 00247 normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); 00248 else 00249 normal.curvature = 0; 00250 00251 return; 00252 } 00253 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 00254 { 00255 if (!init_average_3d_gradient_) 00256 initAverage3DGradientMethod (); 00257 00258 unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); 00259 unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); 00260 if (count_x == 0 || count_y == 0) 00261 { 00262 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00263 return; 00264 } 00265 Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); 00266 Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); 00267 00268 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); 00269 double normal_length = normal_vector.squaredNorm (); 00270 if (normal_length == 0.0f) 00271 { 00272 normal.getNormalVector3fMap ().setConstant (bad_point); 00273 normal.curvature = bad_point; 00274 return; 00275 } 00276 00277 normal_vector /= sqrt (normal_length); 00278 00279 float nx = static_cast<float> (normal_vector [0]); 00280 float ny = static_cast<float> (normal_vector [1]); 00281 float nz = static_cast<float> (normal_vector [2]); 00282 00283 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); 00284 00285 normal.normal_x = nx; 00286 normal.normal_y = ny; 00287 normal.normal_z = nz; 00288 normal.curvature = bad_point; 00289 return; 00290 } 00291 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 00292 { 00293 if (!init_depth_change_) 00294 initAverageDepthChangeMethod (); 00295 00296 // width and height are at least 3 x 3 00297 unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_); 00298 unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_); 00299 unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_); 00300 unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_); 00301 00302 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) 00303 { 00304 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00305 return; 00306 } 00307 00308 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); 00309 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); 00310 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); 00311 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); 00312 00313 PointInT pointL = input_->points[point_index - rect_width_4_ - 1]; 00314 PointInT pointR = input_->points[point_index + rect_width_4_ + 1]; 00315 PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1]; 00316 PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1]; 00317 00318 const float mean_x_z = mean_R_z - mean_L_z; 00319 const float mean_y_z = mean_D_z - mean_U_z; 00320 00321 const float mean_x_x = pointR.x - pointL.x; 00322 const float mean_x_y = pointR.y - pointL.y; 00323 const float mean_y_x = pointD.x - pointU.x; 00324 const float mean_y_y = pointD.y - pointU.y; 00325 00326 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; 00327 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; 00328 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; 00329 00330 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); 00331 00332 if (normal_length == 0.0f) 00333 { 00334 normal.getNormalVector3fMap ().setConstant (bad_point); 00335 normal.curvature = bad_point; 00336 return; 00337 } 00338 00339 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); 00340 00341 const float scale = 1.0f / sqrtf (normal_length); 00342 00343 normal.normal_x = normal_x * scale; 00344 normal.normal_y = normal_y * scale; 00345 normal.normal_z = normal_z * scale; 00346 normal.curvature = bad_point; 00347 00348 return; 00349 } 00350 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) 00351 { 00352 if (!init_simple_3d_gradient_) 00353 initSimple3DGradientMethod (); 00354 00355 // this method does not work if lots of NaNs are in the neighborhood of the point 00356 Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) - 00357 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_); 00358 00359 Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) - 00360 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1); 00361 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); 00362 double normal_length = normal_vector.squaredNorm (); 00363 if (normal_length == 0.0f) 00364 { 00365 normal.getNormalVector3fMap ().setConstant (bad_point); 00366 normal.curvature = bad_point; 00367 return; 00368 } 00369 00370 normal_vector /= sqrt (normal_length); 00371 00372 float nx = static_cast<float> (normal_vector [0]); 00373 float ny = static_cast<float> (normal_vector [1]); 00374 float nz = static_cast<float> (normal_vector [2]); 00375 00376 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); 00377 00378 normal.normal_x = nx; 00379 normal.normal_y = ny; 00380 normal.normal_z = nz; 00381 normal.curvature = bad_point; 00382 return; 00383 } 00384 00385 normal.getNormalVector3fMap ().setConstant (bad_point); 00386 normal.curvature = bad_point; 00387 return; 00388 } 00389 00390 ////////////////////////////////////////////////////////////////////////////////////////// 00391 template <typename T> 00392 void 00393 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height, 00394 const boost::function<T(unsigned, unsigned, unsigned, unsigned)> &f, 00395 T & result) 00396 { 00397 if (start_x < 0) 00398 { 00399 if (start_y < 0) 00400 { 00401 result += f (0, 0, end_x, end_y); 00402 result += f (0, 0, -start_x, -start_y); 00403 result += f (0, 0, -start_x, end_y); 00404 result += f (0, 0, end_x, -start_y); 00405 } 00406 else if (end_y >= height) 00407 { 00408 result += f (0, start_y, end_x, height-1); 00409 result += f (0, start_y, -start_x, height-1); 00410 result += f (0, height-(end_y-(height-1)), end_x, height-1); 00411 result += f (0, height-(end_y-(height-1)), -start_x, height-1); 00412 } 00413 else 00414 { 00415 result += f (0, start_y, end_x, end_y); 00416 result += f (0, start_y, -start_x, end_y); 00417 } 00418 } 00419 else if (start_y < 0) 00420 { 00421 if (end_x >= width) 00422 { 00423 result += f (start_x, 0, width-1, end_y); 00424 result += f (start_x, 0, width-1, -start_y); 00425 result += f (width-(end_x-(width-1)), 0, width-1, end_y); 00426 result += f (width-(end_x-(width-1)), 0, width-1, -start_y); 00427 } 00428 else 00429 { 00430 result += f (start_x, 0, end_x, end_y); 00431 result += f (start_x, 0, end_x, -start_y); 00432 } 00433 } 00434 else if (end_x >= width) 00435 { 00436 if (end_y >= height) 00437 { 00438 result += f (start_x, start_y, width-1, height-1); 00439 result += f (start_x, height-(end_y-(height-1)), width-1, height-1); 00440 result += f (width-(end_x-(width-1)), start_y, width-1, height-1); 00441 result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1); 00442 } 00443 else 00444 { 00445 result += f (start_x, start_y, width-1, end_y); 00446 result += f (width-(end_x-(width-1)), start_y, width-1, end_y); 00447 } 00448 } 00449 else if (end_y >= height) 00450 { 00451 result += f (start_x, start_y, end_x, height-1); 00452 result += f (start_x, height-(end_y-(height-1)), end_x, height-1); 00453 } 00454 else 00455 { 00456 result += f (start_x, start_y, end_x, end_y); 00457 } 00458 } 00459 00460 ////////////////////////////////////////////////////////////////////////////////////////// 00461 template <typename PointInT, typename PointOutT> void 00462 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror ( 00463 const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) 00464 { 00465 float bad_point = std::numeric_limits<float>::quiet_NaN (); 00466 00467 const int width = input_->width; 00468 const int height = input_->height; 00469 00470 // ============================================================== 00471 if (normal_estimation_method_ == COVARIANCE_MATRIX) 00472 { 00473 if (!init_covariance_matrix_) 00474 initCovarianceMatrixMethod (); 00475 00476 const int start_x = pos_x - rect_width_2_; 00477 const int start_y = pos_y - rect_height_2_; 00478 const int end_x = start_x + rect_width_; 00479 const int end_y = start_y + rect_height_; 00480 00481 unsigned count = 0; 00482 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count); 00483 00484 // no valid points within the rectangular reagion? 00485 if (count == 0) 00486 { 00487 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00488 return; 00489 } 00490 00491 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00492 Eigen::Vector3f center; 00493 typename IntegralImage2D<float, 3>::SecondOrderType so_elements; 00494 typename IntegralImage2D<float, 3>::ElementType tmp_center; 00495 typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements; 00496 00497 center[0] = 0; 00498 center[1] = 0; 00499 center[2] = 0; 00500 tmp_center[0] = 0; 00501 tmp_center[1] = 0; 00502 tmp_center[2] = 0; 00503 so_elements[0] = 0; 00504 so_elements[1] = 0; 00505 so_elements[2] = 0; 00506 so_elements[3] = 0; 00507 so_elements[4] = 0; 00508 so_elements[5] = 0; 00509 00510 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center); 00511 sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements); 00512 00513 center[0] = float (tmp_center[0]); 00514 center[1] = float (tmp_center[1]); 00515 center[2] = float (tmp_center[2]); 00516 00517 covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]); 00518 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]); 00519 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]); 00520 covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]); 00521 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]); 00522 covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]); 00523 covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count); 00524 float eigen_value; 00525 Eigen::Vector3f eigen_vector; 00526 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); 00527 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); 00528 normal.getNormalVector3fMap () = eigen_vector; 00529 00530 // Compute the curvature surface change 00531 if (eigen_value > 0.0) 00532 normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); 00533 else 00534 normal.curvature = 0; 00535 00536 return; 00537 } 00538 // ======================================================= 00539 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 00540 { 00541 if (!init_average_3d_gradient_) 00542 initAverage3DGradientMethod (); 00543 00544 const int start_x = pos_x - rect_width_2_; 00545 const int start_y = pos_y - rect_height_2_; 00546 const int end_x = start_x + rect_width_; 00547 const int end_y = start_y + rect_height_; 00548 00549 unsigned count_x = 0; 00550 unsigned count_y = 0; 00551 00552 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x); 00553 sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y); 00554 00555 00556 if (count_x == 0 || count_y == 0) 00557 { 00558 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00559 return; 00560 } 00561 Eigen::Vector3d gradient_x (0, 0, 0); 00562 Eigen::Vector3d gradient_y (0, 0, 0); 00563 00564 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x); 00565 sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y); 00566 00567 00568 Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); 00569 double normal_length = normal_vector.squaredNorm (); 00570 if (normal_length == 0.0f) 00571 { 00572 normal.getNormalVector3fMap ().setConstant (bad_point); 00573 normal.curvature = bad_point; 00574 return; 00575 } 00576 00577 normal_vector /= sqrt (normal_length); 00578 00579 float nx = static_cast<float> (normal_vector [0]); 00580 float ny = static_cast<float> (normal_vector [1]); 00581 float nz = static_cast<float> (normal_vector [2]); 00582 00583 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); 00584 00585 normal.normal_x = nx; 00586 normal.normal_y = ny; 00587 normal.normal_z = nz; 00588 normal.curvature = bad_point; 00589 return; 00590 } 00591 // ====================================================== 00592 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 00593 { 00594 if (!init_depth_change_) 00595 initAverageDepthChangeMethod (); 00596 00597 int point_index_L_x = pos_x - rect_width_4_ - 1; 00598 int point_index_L_y = pos_y; 00599 int point_index_R_x = pos_x + rect_width_4_ + 1; 00600 int point_index_R_y = pos_y; 00601 int point_index_U_x = pos_x - 1; 00602 int point_index_U_y = pos_y - rect_height_4_; 00603 int point_index_D_x = pos_x + 1; 00604 int point_index_D_y = pos_y + rect_height_4_; 00605 00606 if (point_index_L_x < 0) 00607 point_index_L_x = -point_index_L_x; 00608 if (point_index_U_x < 0) 00609 point_index_U_x = -point_index_U_x; 00610 if (point_index_U_y < 0) 00611 point_index_U_y = -point_index_U_y; 00612 00613 if (point_index_R_x >= width) 00614 point_index_R_x = width-(point_index_R_x-(width-1)); 00615 if (point_index_D_x >= width) 00616 point_index_D_x = width-(point_index_D_x-(width-1)); 00617 if (point_index_D_y >= height) 00618 point_index_D_y = height-(point_index_D_y-(height-1)); 00619 00620 const int start_x_L = pos_x - rect_width_2_; 00621 const int start_y_L = pos_y - rect_height_4_; 00622 const int end_x_L = start_x_L + rect_width_2_; 00623 const int end_y_L = start_y_L + rect_height_2_; 00624 00625 const int start_x_R = pos_x + 1; 00626 const int start_y_R = pos_y - rect_height_4_; 00627 const int end_x_R = start_x_R + rect_width_2_; 00628 const int end_y_R = start_y_R + rect_height_2_; 00629 00630 const int start_x_U = pos_x - rect_width_4_; 00631 const int start_y_U = pos_y - rect_height_2_; 00632 const int end_x_U = start_x_U + rect_width_2_; 00633 const int end_y_U = start_y_U + rect_height_2_; 00634 00635 const int start_x_D = pos_x - rect_width_4_; 00636 const int start_y_D = pos_y + 1; 00637 const int end_x_D = start_x_D + rect_width_2_; 00638 const int end_y_D = start_y_D + rect_height_2_; 00639 00640 unsigned count_L_z = 0; 00641 unsigned count_R_z = 0; 00642 unsigned count_U_z = 0; 00643 unsigned count_D_z = 0; 00644 00645 sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z); 00646 sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z); 00647 sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z); 00648 sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z); 00649 00650 if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) 00651 { 00652 normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; 00653 return; 00654 } 00655 00656 float mean_L_z = 0; 00657 float mean_R_z = 0; 00658 float mean_U_z = 0; 00659 float mean_D_z = 0; 00660 00661 sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z); 00662 sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z); 00663 sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z); 00664 sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z); 00665 00666 mean_L_z /= float (count_L_z); 00667 mean_R_z /= float (count_R_z); 00668 mean_U_z /= float (count_U_z); 00669 mean_D_z /= float (count_D_z); 00670 00671 00672 PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x]; 00673 PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x]; 00674 PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x]; 00675 PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x]; 00676 00677 const float mean_x_z = mean_R_z - mean_L_z; 00678 const float mean_y_z = mean_D_z - mean_U_z; 00679 00680 const float mean_x_x = pointR.x - pointL.x; 00681 const float mean_x_y = pointR.y - pointL.y; 00682 const float mean_y_x = pointD.x - pointU.x; 00683 const float mean_y_y = pointD.y - pointU.y; 00684 00685 float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; 00686 float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; 00687 float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; 00688 00689 const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); 00690 00691 if (normal_length == 0.0f) 00692 { 00693 normal.getNormalVector3fMap ().setConstant (bad_point); 00694 normal.curvature = bad_point; 00695 return; 00696 } 00697 00698 flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); 00699 00700 const float scale = 1.0f / sqrtf (normal_length); 00701 00702 normal.normal_x = normal_x * scale; 00703 normal.normal_y = normal_y * scale; 00704 normal.normal_z = normal_z * scale; 00705 normal.curvature = bad_point; 00706 00707 return; 00708 } 00709 // ======================================================== 00710 else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) 00711 { 00712 PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT"); 00713 } 00714 00715 normal.getNormalVector3fMap ().setConstant (bad_point); 00716 normal.curvature = bad_point; 00717 return; 00718 } 00719 00720 ////////////////////////////////////////////////////////////////////////////////////////// 00721 template <typename PointInT, typename PointOutT> void 00722 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00723 { 00724 output.sensor_origin_ = input_->sensor_origin_; 00725 output.sensor_orientation_ = input_->sensor_orientation_; 00726 00727 float bad_point = std::numeric_limits<float>::quiet_NaN (); 00728 00729 // compute depth-change map 00730 unsigned char * depthChangeMap = new unsigned char[input_->points.size ()]; 00731 memset (depthChangeMap, 255, input_->points.size ()); 00732 00733 unsigned index = 0; 00734 for (unsigned int ri = 0; ri < input_->height-1; ++ri) 00735 { 00736 for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index) 00737 { 00738 index = ri * input_->width + ci; 00739 00740 const float depth = input_->points [index].z; 00741 const float depthR = input_->points [index + 1].z; 00742 const float depthD = input_->points [index + input_->width].z; 00743 00744 //const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f); 00745 const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f); 00746 00747 if (fabs (depth - depthR) > depthDependendDepthChange 00748 || !pcl_isfinite (depth) || !pcl_isfinite (depthR)) 00749 { 00750 depthChangeMap[index] = 0; 00751 depthChangeMap[index+1] = 0; 00752 } 00753 if (fabs (depth - depthD) > depthDependendDepthChange 00754 || !pcl_isfinite (depth) || !pcl_isfinite (depthD)) 00755 { 00756 depthChangeMap[index] = 0; 00757 depthChangeMap[index + input_->width] = 0; 00758 } 00759 } 00760 } 00761 00762 // compute distance map 00763 //float *distanceMap = new float[input_->points.size ()]; 00764 if (distance_map_ != NULL) delete[] distance_map_; 00765 distance_map_ = new float[input_->points.size ()]; 00766 float *distanceMap = distance_map_; 00767 for (size_t index = 0; index < input_->points.size (); ++index) 00768 { 00769 if (depthChangeMap[index] == 0) 00770 distanceMap[index] = 0.0f; 00771 else 00772 distanceMap[index] = static_cast<float> (input_->width + input_->height); 00773 } 00774 00775 // first pass 00776 float* previous_row = distanceMap; 00777 float* current_row = previous_row + input_->width; 00778 for (size_t ri = 1; ri < input_->height; ++ri) 00779 { 00780 for (size_t ci = 1; ci < input_->width; ++ci) 00781 { 00782 const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f; 00783 const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f; 00784 const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f; 00785 const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f; 00786 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; 00787 00788 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight)); 00789 00790 if (minValue < center) 00791 current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue; 00792 } 00793 previous_row = current_row; 00794 current_row += input_->width; 00795 } 00796 00797 float* next_row = distanceMap + input_->width * (input_->height - 1); 00798 current_row = next_row - input_->width; 00799 // second pass 00800 for (int ri = input_->height-2; ri >= 0; --ri) 00801 { 00802 for (int ci = input_->width-2; ci >= 0; --ci) 00803 { 00804 const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f; 00805 const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f; 00806 const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f; 00807 const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f; 00808 const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; 00809 00810 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight)); 00811 00812 if (minValue < center) 00813 current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue; 00814 } 00815 next_row = current_row; 00816 current_row -= input_->width; 00817 } 00818 00819 if (border_policy_ == BORDER_POLICY_IGNORE) 00820 { 00821 // Set all normals that we do not touch to NaN 00822 // top and bottom borders 00823 // That sets the output density to false! 00824 output.is_dense = false; 00825 unsigned border = int(normal_smoothing_size_); 00826 PointOutT* vec1 = &output [0]; 00827 PointOutT* vec2 = vec1 + input_->width * (input_->height - border); 00828 00829 size_t count = border * input_->width; 00830 for (size_t idx = 0; idx < count; ++idx) 00831 { 00832 vec1 [idx].getNormalVector3fMap ().setConstant (bad_point); 00833 vec1 [idx].curvature = bad_point; 00834 vec2 [idx].getNormalVector3fMap ().setConstant (bad_point); 00835 vec2 [idx].curvature = bad_point; 00836 } 00837 00838 // left and right borders actually columns 00839 vec1 = &output [border * input_->width]; 00840 vec2 = vec1 + input_->width - border; 00841 for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width) 00842 { 00843 for (size_t ci = 0; ci < border; ++ci) 00844 { 00845 vec1 [ci].getNormalVector3fMap ().setConstant (bad_point); 00846 vec1 [ci].curvature = bad_point; 00847 vec2 [ci].getNormalVector3fMap ().setConstant (bad_point); 00848 vec2 [ci].curvature = bad_point; 00849 } 00850 } 00851 00852 if (use_depth_dependent_smoothing_) 00853 { 00854 index = border + input_->width * border; 00855 unsigned skip = (border << 1); 00856 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) 00857 { 00858 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) 00859 { 00860 index = ri * input_->width + ci; 00861 00862 const float depth = input_->points[index].z; 00863 if (!pcl_isfinite (depth)) 00864 { 00865 output[index].getNormalVector3fMap ().setConstant (bad_point); 00866 output[index].curvature = bad_point; 00867 continue; 00868 } 00869 00870 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); 00871 00872 if (smoothing > 2.0f) 00873 { 00874 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); 00875 computePointNormal (ci, ri, index, output [index]); 00876 } 00877 else 00878 { 00879 output[index].getNormalVector3fMap ().setConstant (bad_point); 00880 output[index].curvature = bad_point; 00881 } 00882 } 00883 } 00884 } 00885 else 00886 { 00887 float smoothing_constant = normal_smoothing_size_; 00888 00889 index = border + input_->width * border; 00890 unsigned skip = (border << 1); 00891 for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) 00892 { 00893 for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) 00894 { 00895 index = ri * input_->width + ci; 00896 00897 if (!pcl_isfinite (input_->points[index].z)) 00898 { 00899 output [index].getNormalVector3fMap ().setConstant (bad_point); 00900 output [index].curvature = bad_point; 00901 continue; 00902 } 00903 00904 float smoothing = (std::min)(distanceMap[index], smoothing_constant); 00905 00906 if (smoothing > 2.0f) 00907 { 00908 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); 00909 computePointNormal (ci, ri, index, output [index]); 00910 } 00911 else 00912 { 00913 output [index].getNormalVector3fMap ().setConstant (bad_point); 00914 output [index].curvature = bad_point; 00915 } 00916 } 00917 } 00918 } 00919 } 00920 else if (border_policy_ == BORDER_POLICY_MIRROR) 00921 { 00922 output.is_dense = false; 00923 00924 if (use_depth_dependent_smoothing_) 00925 { 00926 //index = 0; 00927 //unsigned skip = 0; 00928 //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip) 00929 for (unsigned ri = 0; ri < input_->height; ++ri) 00930 { 00931 //for (unsigned ci = 0; ci < input_->width; ++ci, ++index) 00932 for (unsigned ci = 0; ci < input_->width; ++ci) 00933 { 00934 index = ri * input_->width + ci; 00935 00936 const float depth = input_->points[index].z; 00937 if (!pcl_isfinite (depth)) 00938 { 00939 output[index].getNormalVector3fMap ().setConstant (bad_point); 00940 output[index].curvature = bad_point; 00941 continue; 00942 } 00943 00944 float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); 00945 00946 if (smoothing > 2.0f) 00947 { 00948 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); 00949 computePointNormalMirror (ci, ri, index, output [index]); 00950 } 00951 else 00952 { 00953 output[index].getNormalVector3fMap ().setConstant (bad_point); 00954 output[index].curvature = bad_point; 00955 } 00956 } 00957 } 00958 } 00959 else 00960 { 00961 float smoothing_constant = normal_smoothing_size_; 00962 00963 //index = border + input_->width * border; 00964 //unsigned skip = (border << 1); 00965 //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) 00966 for (unsigned ri = 0; ri < input_->height; ++ri) 00967 { 00968 //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) 00969 for (unsigned ci = 0; ci < input_->width; ++ci) 00970 { 00971 index = ri * input_->width + ci; 00972 00973 if (!pcl_isfinite (input_->points[index].z)) 00974 { 00975 output [index].getNormalVector3fMap ().setConstant (bad_point); 00976 output [index].curvature = bad_point; 00977 continue; 00978 } 00979 00980 float smoothing = (std::min)(distanceMap[index], smoothing_constant); 00981 00982 if (smoothing > 2.0f) 00983 { 00984 setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing)); 00985 computePointNormalMirror (ci, ri, index, output [index]); 00986 } 00987 else 00988 { 00989 output [index].getNormalVector3fMap ().setConstant (bad_point); 00990 output [index].curvature = bad_point; 00991 } 00992 } 00993 } 00994 } 00995 } 00996 00997 delete[] depthChangeMap; 00998 //delete[] distanceMap; 00999 } 01000 01001 ////////////////////////////////////////////////////////////////////////////////////////// 01002 template <typename PointInT, typename PointOutT> bool 01003 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCompute () 01004 { 01005 if (!input_->isOrganized ()) 01006 { 01007 PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n"); 01008 return (false); 01009 } 01010 return (Feature<PointInT, PointOutT>::initCompute ()); 01011 } 01012 01013 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>; 01014 01015 #endif 01016