Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 */ 00035 00036 #ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ 00037 #define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ 00038 #define EIGEN_II_METHOD 1 00039 00040 #include "pcl/features/integral_image_normal.h" 00041 #include "pcl/common/time.h" 00043 template <typename PointInT, typename PointOutT> 00044 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::~IntegralImageNormalEstimation () 00045 { 00046 if (integral_image_x_ != NULL) delete integral_image_x_; 00047 if (integral_image_y_ != NULL) delete integral_image_y_; 00048 if (integral_image_xyz_ != NULL) delete integral_image_xyz_; 00049 if (integral_image_ != NULL) delete integral_image_; 00050 if (diff_x_ != NULL) delete diff_x_; 00051 if (diff_y_ != NULL) delete diff_y_; 00052 if (depth_data_ != NULL) delete depth_data_; 00053 } 00054 00056 template <typename PointInT, typename PointOutT> void 00057 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initData () 00058 { 00059 // compute derivatives 00060 if (integral_image_x_ != NULL) delete integral_image_x_; 00061 if (integral_image_y_ != NULL) delete integral_image_y_; 00062 if (integral_image_xyz_ != NULL) delete integral_image_xyz_; 00063 if (integral_image_ != NULL) delete integral_image_; 00064 if (diff_x_ != NULL) delete diff_x_; 00065 if (diff_y_ != NULL) delete diff_y_; 00066 if (depth_data_ != NULL) delete depth_data_; 00067 00068 if (normal_estimation_method_ == COVARIANCE_MATRIX) 00069 initCovarianceMatrixMethod (); 00070 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 00071 initAverage3DGradientMethod (); 00072 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 00073 initAverageDepthChangeMethod (); 00074 } 00075 00076 00078 template <typename PointInT, typename PointOutT> void 00079 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::setRectSize (const int width, const int height) 00080 { 00081 rect_width_ = width; 00082 rect_height_ = height; 00083 } 00084 00086 template <typename PointInT, typename PointOutT> void 00087 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCovarianceMatrixMethod () 00088 { 00089 // number of DataType entries per element (equal or bigger than dimensions) 00090 int element_stride = sizeof (input_->points[0]) / sizeof (float); 00091 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00092 int row_stride = element_stride * input_->width; 00093 00094 float *data_ = reinterpret_cast<float*>((PointInT*)(&(input_->points[0]))); 00095 00096 // compute integral images 00097 #if EIGEN_II_METHOD 00098 integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride); 00099 #else 00100 integral_image_xyz_ = new pcl::IntegralImage2D<float, double>(data_, input_->width, input_->height, 3, true, element_stride, row_stride); 00101 #endif 00102 init_covariance_matrix_ = true; 00103 init_average_3d_gradient_ = init_depth_change_ = false; 00104 } 00105 00107 template <typename PointInT, typename PointOutT> void 00108 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverage3DGradientMethod () 00109 { 00110 float *data_ = reinterpret_cast<float*>((PointInT*)(&(input_->points[0]))); 00111 size_t nr_points = 4 * input_->points.size (); 00112 diff_x_ = new float[nr_points]; 00113 diff_y_ = new float[nr_points]; 00114 00115 // number of DataType entries per element (equal or bigger than dimensions) 00116 int element_stride = sizeof (input_->points[0]) / sizeof (float); 00117 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00118 int row_stride = element_stride * input_->width; 00119 00120 memset (diff_x_, 0, sizeof(float) * nr_points); 00121 memset (diff_y_, 0, sizeof(float) * nr_points); 00122 00123 for (size_t ri = 1; ri < input_->height - 1; ++ri) 00124 { 00125 float *data_pointer_y_up = data_ + (ri-1)*row_stride + element_stride; 00126 float *data_pointer_y_down = data_ + (ri+1)*row_stride + element_stride; 00127 float *data_pointer_x_left = data_ + ri*row_stride; 00128 float *data_pointer_x_right = data_ + ri*row_stride + 2*element_stride; 00129 00130 float * diff_x_pointer = diff_x_ + ri * 4 * input_->width + 4; 00131 float * diff_y_pointer = diff_y_ + ri * 4 * input_->width + 4; 00132 00133 for (size_t ci = 1; ci < input_->width - 1; ++ci) 00134 { 00135 diff_x_pointer[0] = data_pointer_x_right[0] - data_pointer_x_left[0]; 00136 diff_x_pointer[1] = data_pointer_x_right[1] - data_pointer_x_left[1]; 00137 diff_x_pointer[2] = data_pointer_x_right[2] - data_pointer_x_left[2]; 00138 00139 diff_y_pointer[0] = data_pointer_y_down[0] - data_pointer_y_up[0]; 00140 diff_y_pointer[1] = data_pointer_y_down[1] - data_pointer_y_up[1]; 00141 diff_y_pointer[2] = data_pointer_y_down[2] - data_pointer_y_up[2]; 00142 00143 diff_x_pointer += 4; 00144 diff_y_pointer += 4; 00145 00146 data_pointer_y_up += element_stride; 00147 data_pointer_y_down += element_stride; 00148 data_pointer_x_left += element_stride; 00149 data_pointer_x_right += element_stride; 00150 } 00151 } 00152 00153 // Compute integral images 00154 integral_image_x_ = new pcl::IntegralImage2D<float, double>(diff_x_, input_->width, input_->height, 3, false, 4, 4 * input_->width); 00155 integral_image_y_ = new pcl::IntegralImage2D<float, double>(diff_y_, input_->width, input_->height, 3, false, 4, 4 * input_->width); 00156 00157 init_covariance_matrix_ = init_depth_change_ = false; 00158 init_average_3d_gradient_ = true; 00159 } 00160 00162 template <typename PointInT, typename PointOutT> void 00163 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverageDepthChangeMethod () 00164 { 00165 // number of DataType entries per element (equal or bigger than dimensions) 00166 int element_stride = sizeof (input_->points[0]) / sizeof (float); 00167 // number of DataType entries per row (equal or bigger than element_stride number of elements per row) 00168 int row_stride = element_stride * input_->width; 00169 00170 float *data_ = reinterpret_cast<float*>((PointInT*)(&(input_->points[0]))); 00171 // compute integral image 00172 integral_image_ = new pcl::IntegralImage2D<float, double>( 00173 &(data_[2]), input_->width, input_->height, 1, false, element_stride, row_stride); 00174 00175 init_depth_change_ = true; 00176 init_covariance_matrix_ = init_average_3d_gradient_ = false; 00177 } 00178 00180 template <typename PointInT, typename PointOutT> void 00181 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal ( 00182 const int pos_x, const int pos_y, PointOutT &normal) 00183 { 00184 float bad_point = std::numeric_limits<float>::quiet_NaN (); 00185 00186 if (normal_estimation_method_ == COVARIANCE_MATRIX) 00187 { 00188 if (!init_covariance_matrix_) 00189 initCovarianceMatrixMethod (); 00190 00191 #if EIGEN_II_METHOD 00192 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00193 Eigen::Vector3f center; 00194 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00195 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00196 typename IntegralImage2Dim<float, 3>::SecondOrderType so_elements; 00197 00198 center = integral_image_XYZ_.getFirstOrderSum(pos_x - (rect_width_ >> 1), pos_y - (rect_height_ >> 1), rect_width_, rect_height_).cast<float> (); 00199 so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - (rect_width_ >> 1), pos_y - (rect_height_ >> 1), rect_width_, rect_height_); 00200 covariance_matrix (0, 0) = so_elements [0]; 00201 covariance_matrix (0, 1) = covariance_matrix (1, 0) = so_elements [1]; 00202 covariance_matrix (0, 2) = covariance_matrix (2, 0) = so_elements [2]; 00203 covariance_matrix (1, 1) = so_elements [3]; 00204 covariance_matrix (1, 2) = covariance_matrix (2, 1) = so_elements [4]; 00205 covariance_matrix (2, 2) = so_elements [5]; 00206 covariance_matrix -= (center * center.transpose ()) / (rect_width_ * rect_height_); 00207 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00208 if (eigen_vectors (2, 0) < 0.0f) 00209 normal.getNormalVector4fMap () = Eigen::Vector4f (eigen_vectors (0, 0), eigen_vectors (1, 0), eigen_vectors (2, 0), 0); 00210 else 00211 normal.getNormalVector4fMap () = Eigen::Vector4f (-eigen_vectors (0, 0), -eigen_vectors (1, 0), -eigen_vectors (2, 0), 0); 00212 00213 // Compute the curvature surface change 00214 if (eigen_values [2] > 0.0) 00215 normal.curvature = fabs ( eigen_values[0] / eigen_values.sum () ); 00216 else 00217 normal.curvature = 0; 00218 #else 00219 const float mean_x = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0); 00220 const float mean_y = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 1); 00221 const float mean_z = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 2); 00222 00223 const float mean_xx = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0, 0); 00224 const float mean_xy = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0, 1); 00225 const float mean_xz = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0, 2); 00226 const float mean_yx = mean_xy; 00227 const float mean_yy = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 1, 1); 00228 const float mean_yz = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 1, 2); 00229 const float mean_zx = mean_xz; 00230 const float mean_zy = mean_yz; 00231 const float mean_zz = integral_image_xyz_->getSum(pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 2, 2); 00232 00233 00234 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00235 covariance_matrix (0, 0) = mean_xx; covariance_matrix (0, 1) = mean_xy; covariance_matrix (0, 2) = mean_xz; 00236 covariance_matrix (1, 0) = mean_yx; covariance_matrix (1, 1) = mean_yy; covariance_matrix (1, 2) = mean_yz; 00237 covariance_matrix (2, 0) = mean_zx; covariance_matrix (2, 1) = mean_zy; covariance_matrix (2, 2) = mean_zz; 00238 00239 Eigen::Vector3f center (mean_x, mean_y, mean_z); 00240 00241 covariance_matrix -= (1.0f/(rect_width_*rect_height_)) * (center * center.transpose()); 00242 covariance_matrix *= 1.0f/(rect_width_*rect_height_-1); 00243 00244 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00245 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00246 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); 00247 00248 Eigen::Vector4f n (eigen_vectors (0, 0), eigen_vectors (1, 0), eigen_vectors (2, 0), 0); 00249 00250 if (n[2] > 0.0f) 00251 n *= -1.0f; 00252 00253 normal.getNormalVector4fMap () = n; 00254 00255 // Compute the curvature surface change 00256 float eig_sum = eigen_values.sum (); 00257 if (eig_sum != 0) 00258 normal.curvature = fabs ( eigen_values[0] / eig_sum ); 00259 else 00260 normal.curvature = 0; 00261 #endif 00262 return; 00263 } 00264 else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 00265 { 00266 if (!init_average_3d_gradient_) 00267 initAverage3DGradientMethod (); 00268 const float mean_x_x = integral_image_x_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0); 00269 const float mean_x_y = integral_image_x_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 1); 00270 const float mean_x_z = integral_image_x_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 2); 00271 00272 const float mean_y_x = integral_image_y_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 0); 00273 const float mean_y_y = integral_image_y_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 1); 00274 const float mean_y_z = integral_image_y_->getSum (pos_x-rect_width_/2, pos_y-rect_height_/2, rect_width_, rect_height_, 2); 00275 00276 const float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; 00277 const float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; 00278 const float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; 00279 00280 const float normal_length = sqrt (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); 00281 00282 if (normal_length == 0.0f) 00283 { 00284 normal.getNormalVector4fMap ().setConstant (bad_point); 00285 normal.curvature = bad_point; 00286 return; 00287 } 00288 00289 const float scale = -1.0f / normal_length; 00290 00291 normal.normal_x = normal_x * scale; 00292 normal.normal_y = normal_y * scale; 00293 normal.normal_z = normal_z * scale; 00294 normal.curvature = bad_point; 00295 return; 00296 } 00297 else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 00298 { 00299 if (!init_depth_change_) 00300 initAverageDepthChangeMethod (); 00301 00302 PointInT pointL = input_->points[pos_y * input_->width + pos_x-rect_width_/2]; 00303 PointInT pointR = input_->points[pos_y * input_->width + pos_x+rect_width_/2]; 00304 PointInT pointU = input_->points[(pos_y-rect_height_/2) * input_->width+pos_x]; 00305 PointInT pointD = input_->points[(pos_y+rect_height_/2) * input_->width+pos_x]; 00306 00307 const float mean_L_z = integral_image_->getSum(pos_x-1-rect_width_/2, pos_y-rect_height_/2, rect_width_-1, rect_height_-1, 0)/((rect_width_-1)*(rect_height_-1)); 00308 const float mean_R_z = integral_image_->getSum(pos_x+1-rect_width_/2, pos_y-rect_height_/2, rect_width_-1, rect_height_-1, 0)/((rect_width_-1)*(rect_height_-1)); 00309 const float mean_U_z = integral_image_->getSum(pos_x-rect_width_/2, pos_y-1-rect_height_/2, rect_width_-1, rect_height_-1, 0)/((rect_width_-1)*(rect_height_-1)); 00310 const float mean_D_z = integral_image_->getSum(pos_x-rect_width_/2, pos_y+1-rect_height_/2, rect_width_-1, rect_height_-1, 0)/((rect_width_-1)*(rect_height_-1)); 00311 00312 const float mean_x_z = (mean_R_z - mean_L_z)/2.0f; 00313 const float mean_y_z = (mean_D_z - mean_U_z)/2.0f; 00314 00315 const float mean_x_x = (pointR.x - pointL.x)/(rect_width_); 00316 const float mean_x_y = (pointR.y - pointL.y)/(rect_height_); 00317 const float mean_y_x = (pointD.x - pointU.x)/(rect_width_); 00318 const float mean_y_y = (pointD.y - pointU.y)/(rect_height_); 00319 00320 const float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; 00321 const float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; 00322 const float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; 00323 00324 const float normal_length = sqrt (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); 00325 00326 if (normal_length == 0.0f) 00327 { 00328 normal.getNormalVector4fMap ().setConstant (bad_point); 00329 normal.curvature = bad_point; 00330 return; 00331 } 00332 00333 const float scale = -1.0f / normal_length; 00334 00335 normal.normal_x = normal_x*scale; 00336 normal.normal_y = normal_y*scale; 00337 normal.normal_z = normal_z*scale; 00338 normal.curvature = bad_point; 00339 00340 return; 00341 } 00342 normal.getNormalVector4fMap ().setConstant (bad_point); 00343 normal.curvature = bad_point; 00344 return; 00345 } 00346 00348 template <typename PointInT, typename PointOutT> void 00349 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 00350 { 00351 float bad_point = std::numeric_limits<float>::quiet_NaN (); 00352 00353 // compute depth-change map 00354 unsigned char * depthChangeMap = new unsigned char[input_->points.size ()]; 00355 memset (depthChangeMap, 255, input_->points.size ()); 00356 00357 for (unsigned int ri = 0; ri < input_->height-1; ++ri) 00358 { 00359 for (unsigned int ci = 0; ci < input_->width-1; ++ci) 00360 { 00361 const float depth = (*input_)(ci, ri ).z; 00362 const float depthR = (*input_)(ci + 1, ri ).z; 00363 const float depthD = (*input_)(ci, ri + 1).z; 00364 00365 const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f); 00366 00367 if (abs (depth - depthR) > depthDependendDepthChange 00368 || !pcl_isfinite (depth) || !pcl_isfinite (depthR)) 00369 { 00370 depthChangeMap[ri*input_->width + ci] = 0; 00371 depthChangeMap[ri*input_->width + ci+1] = 0; 00372 } 00373 if (abs (depth - depthD) > depthDependendDepthChange 00374 || !pcl_isfinite (depth) || !pcl_isfinite (depthD)) 00375 { 00376 depthChangeMap[ri*input_->width + ci] = 0; 00377 depthChangeMap[(ri+1)*input_->width + ci] = 0; 00378 } 00379 } 00380 } 00381 00382 00383 // compute distance map 00384 float *distanceMap = new float[input_->points.size ()]; 00385 for (size_t index = 0; index < input_->points.size (); ++index) 00386 { 00387 if (depthChangeMap[index] == 0) 00388 distanceMap[index] = 0.0f; 00389 else 00390 distanceMap[index] = 640.0f; 00391 } 00392 00393 // first pass 00394 for (size_t ri = 1; ri < input_->height; ++ri) 00395 { 00396 for (size_t ci = 1; ci < input_->width; ++ci) 00397 { 00398 const float upLeft = distanceMap[(ri-1)*input_->width + ci-1] + 1.4f; 00399 const float up = distanceMap[(ri-1)*input_->width + ci] + 1.0f; 00400 const float upRight = distanceMap[(ri-1)*input_->width + ci+1] + 1.4f; 00401 const float left = distanceMap[ri*input_->width + ci-1] + 1.0f; 00402 const float center = distanceMap[ri*input_->width + ci]; 00403 00404 const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight)); 00405 00406 if (minValue < center) 00407 distanceMap[ri * input_->width + ci] = minValue; 00408 } 00409 } 00410 00411 // second pass 00412 for (int ri = input_->height-2; ri >= 0; --ri) 00413 { 00414 for (int ci = input_->width-2; ci >= 0; --ci) 00415 { 00416 const float lowerLeft = distanceMap[(ri+1)*input_->width + ci-1] + 1.4f; 00417 const float lower = distanceMap[(ri+1)*input_->width + ci] + 1.0f; 00418 const float lowerRight = distanceMap[(ri+1)*input_->width + ci+1] + 1.4f; 00419 const float right = distanceMap[ri*input_->width + ci+1] + 1.0f; 00420 const float center = distanceMap[ri*input_->width + ci]; 00421 00422 const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight)); 00423 00424 if (minValue < center) 00425 distanceMap[ri*input_->width + ci] = minValue; 00426 } 00427 } 00428 00429 // Set all normals that we do not touch to NaN 00430 for (size_t ri = 0; ri < normal_smoothing_size_; ++ri) 00431 { 00432 for (size_t ci = 0; ci < input_->width; ++ci) 00433 { 00434 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00435 output (ci, ri).curvature = bad_point; 00436 } 00437 } 00438 for (size_t ri = normal_smoothing_size_; ri < input_->height; ++ri) 00439 { 00440 for (size_t ci = 0; ci < normal_smoothing_size_; ++ci) 00441 { 00442 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00443 output (ci, ri).curvature = bad_point; 00444 } 00445 } 00446 00447 for (size_t ri = input_->height - normal_smoothing_size_; ri < input_->height; ++ri) 00448 { 00449 for (size_t ci = normal_smoothing_size_; ci < input_->width; ++ci) 00450 { 00451 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00452 output (ci, ri).curvature = bad_point; 00453 } 00454 } 00455 00456 for (size_t ri = normal_smoothing_size_; ri < input_->height - normal_smoothing_size_; ++ri) 00457 { 00458 for (size_t ci = input_->width - normal_smoothing_size_; ci < input_->width; ++ci) 00459 { 00460 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00461 output (ci, ri).curvature = bad_point; 00462 } 00463 } 00464 00465 if (use_depth_dependent_smoothing_) 00466 { 00467 for (int ri = normal_smoothing_size_; ri < input_->height-normal_smoothing_size_; ++ri) 00468 { 00469 for (int ci = normal_smoothing_size_; ci < input_->width-normal_smoothing_size_; ++ci) 00470 { 00471 const float depth = (*input_)(ci, ri).z; 00472 if (!pcl_isfinite (depth)) 00473 { 00474 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00475 output (ci, ri).curvature = bad_point; 00476 continue; 00477 } 00478 00479 float smoothing = (std::min)(distanceMap[ri*input_->width + ci], normal_smoothing_size_ + static_cast<float>(depth)/10.0f); 00480 00481 if (smoothing > 2.0f) 00482 { 00483 setRectSize (smoothing, smoothing); 00484 computePointNormal (ci, ri, output (ci, ri)); 00485 } 00486 else 00487 { 00488 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00489 output (ci, ri).curvature = bad_point; 00490 } 00491 } 00492 } 00493 } 00494 else 00495 { 00496 float smoothing_constant = normal_smoothing_size_ * static_cast<float>(1.0f) / (500.0f * 0.001f); 00497 for (int ri = normal_smoothing_size_; ri < input_->height-normal_smoothing_size_; ++ri) 00498 { 00499 for (int ci = normal_smoothing_size_; ci < input_->width-normal_smoothing_size_; ++ci) 00500 { 00501 if (!pcl_isfinite ((*input_) (ci, ri).z)) 00502 { 00503 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00504 output (ci, ri).curvature = bad_point; 00505 continue; 00506 } 00507 00508 float smoothing = (std::min)(distanceMap[ri*input_->width + ci], smoothing_constant); 00509 00510 if (smoothing > 2.0f) 00511 { 00512 setRectSize (smoothing, smoothing); 00513 computePointNormal (ci, ri, output (ci, ri)); 00514 } 00515 else 00516 { 00517 output (ci, ri).getNormalVector4fMap ().setConstant (bad_point); 00518 output (ci, ri).curvature = bad_point; 00519 } 00520 } 00521 } 00522 } 00523 00524 delete[] depthChangeMap; 00525 delete[] distanceMap; 00526 } 00527 00528 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>; 00529 00530 #endif 00531