Point Cloud Library (PCL)  1.3.1
integral_image_normal.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines