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 
00039 #include <pcl/features/integral_image_normal.h>
00040 #include <pcl/features/normal_3d.h>
00041 
00042 #include <boost/bind.hpp>
00043 
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 
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 
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 
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 
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 
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 
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 
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 = std::numeric_limits<float>::quiet_NaN ();
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_).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     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector);
00243     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
00244     normal.getNormalVector3fMap () = eigen_vector;
00245 
00246     // Compute the curvature surface change
00247     if (eigen_value > 0.0)
00248       normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
00249     else
00250       normal.curvature = 0;
00251 
00252     return;
00253   }
00254   else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
00255   {
00256     if (!init_average_3d_gradient_)
00257       initAverage3DGradientMethod ();
00258 
00259     unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00260     unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00261     if (count_x == 0 || count_y == 0)
00262     {
00263       normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00264       return;
00265     }
00266     Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00267     Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00268 
00269     Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
00270     double normal_length = normal_vector.squaredNorm ();
00271     if (normal_length == 0.0f)
00272     {
00273       normal.getNormalVector4fMap ().setConstant (bad_point);
00274       normal.curvature = bad_point;
00275       return;
00276     }
00277 
00278     normal_vector /= sqrt (normal_length);
00279 
00280     float nx = static_cast<float> (normal_vector [0]);
00281     float ny = static_cast<float> (normal_vector [1]);
00282     float nz = static_cast<float> (normal_vector [2]);
00283 
00284     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
00285     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
00286 
00287     normal.normal_x = nx;
00288     normal.normal_y = ny;
00289     normal.normal_z = nz;
00290     normal.curvature = bad_point;
00291     return;
00292   }
00293   else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
00294   {
00295     if (!init_depth_change_)
00296       initAverageDepthChangeMethod ();
00297 
00298 //    unsigned count = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00299 //    if (count == 0)
00300 //    {
00301 //      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00302 //      return;
00303 //    }
00304 //    const float mean_L_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ - 1, pos_y - rect_height_2_    , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
00305 //    const float mean_R_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ + 1, pos_y - rect_height_2_    , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
00306 //    const float mean_U_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_    , pos_y - rect_height_2_ - 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
00307 //    const float mean_D_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_    , pos_y - rect_height_2_ + 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
00308 
00309     // width and height are at least 3 x 3
00310     unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
00311     unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1            , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
00312     unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
00313     unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1             , rect_width_2_, rect_height_2_);
00314 
00315     if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
00316     {
00317       normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00318       return;
00319     }
00320 
00321     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);
00322     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);
00323     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);
00324     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);
00325 
00326     PointInT pointL = input_->points[point_index - rect_width_4_ - 1];
00327     PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
00328     PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
00329     PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1];
00330 
00331     const float mean_x_z = mean_R_z - mean_L_z;
00332     const float mean_y_z = mean_D_z - mean_U_z;
00333 
00334     const float mean_x_x = pointR.x - pointL.x;
00335     const float mean_x_y = pointR.y - pointL.y;
00336     const float mean_y_x = pointD.x - pointU.x;
00337     const float mean_y_y = pointD.y - pointU.y;
00338 
00339     float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
00340     float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
00341     float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
00342 
00343     const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
00344 
00345     if (normal_length == 0.0f)
00346     {
00347       normal.getNormalVector4fMap ().setConstant (bad_point);
00348       normal.curvature = bad_point;
00349       return;
00350     }
00351 
00352     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
00353     
00354     const float scale = 1.0f / sqrt (normal_length);
00355 
00356     normal.normal_x = normal_x * scale;
00357     normal.normal_y = normal_y * scale;
00358     normal.normal_z = normal_z * scale;
00359     normal.curvature = bad_point;
00360 
00361     return;
00362   }
00363   else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
00364   {
00365     if (!init_simple_3d_gradient_)
00366       initSimple3DGradientMethod ();
00367 
00368     // this method does not work if lots of NaNs are in the neighborhood of the point
00369     Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
00370                                  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
00371 
00372     Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
00373                                  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
00374     Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
00375     double normal_length = normal_vector.squaredNorm ();
00376     if (normal_length == 0.0f)
00377     {
00378       normal.getNormalVector4fMap ().setConstant (bad_point);
00379       normal.curvature = bad_point;
00380       return;
00381     }
00382 
00383     normal_vector /= sqrt (normal_length);
00384 
00385     float nx = static_cast<float> (normal_vector [0]);
00386     float ny = static_cast<float> (normal_vector [1]);
00387     float nz = static_cast<float> (normal_vector [2]);
00388 
00389     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
00390     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
00391     
00392     normal.normal_x = nx;
00393     normal.normal_y = ny;
00394     normal.normal_z = nz;
00395     normal.curvature = bad_point;
00396     return;
00397   }
00398 
00399   normal.getNormalVector4fMap ().setConstant (bad_point);
00400   normal.curvature = bad_point;
00401   return;
00402 }
00403 
00405 template <typename T>
00406 void
00407 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
00408   const boost::function<T(unsigned, unsigned, unsigned, unsigned)> &f, 
00409   T & result)
00410 {
00411   //if (start_x < 0 && end_x < 0)
00412   //{
00413   //  int tmp = -end_x;
00414   //  end_x = -start_x;
00415   //  start_x = tmp;
00416   //}
00417 
00418   //if (start_y < 0 && end_y < 0)
00419   //{
00420   //  int tmp = -end_y;
00421   //  end_y = -start_y;
00422   //  start_y = tmp;
00423   //}
00424 
00425   //if (start_x >= width && end_x >= width)
00426   //{
00427   //  int tmp = width-(end_x-(width-1));
00428   //  end_x = width-(start_x-(width-1));
00429   //  start_x = tmp;
00430   //}
00431 
00432   //if (start_y >= height && end_y >= height)
00433   //{
00434   //  int tmp = height-(end_y-(height-1));
00435   //  end_y = height-(start_y-(height-1));
00436   //  start_y = tmp;
00437   //}
00438 
00439   if (start_x < 0)
00440   {
00441     if (start_y < 0)
00442     {
00443       result += f (0, 0, end_x, end_y);
00444       result += f (0, 0, -start_x, -start_y);
00445       result += f (0, 0, -start_x, end_y);
00446       result += f (0, 0, end_x, -start_y);
00447     }
00448     else if (end_y >= height)
00449     {
00450       result += f (0, start_y, end_x, height-1);
00451       result += f (0, start_y, -start_x, height-1);
00452       result += f (0, height-(end_y-(height-1)), end_x, height-1);
00453       result += f (0, height-(end_y-(height-1)), -start_x, height-1);
00454     }
00455     else
00456     {
00457       result += f (0, start_y, end_x, end_y);
00458       result += f (0, start_y, -start_x, end_y);
00459     }
00460   }
00461   else if (start_y < 0)
00462   {
00463     if (end_x >= width)
00464     {
00465       result += f (start_x, 0, width-1, end_y);
00466       result += f (start_x, 0, width-1, -start_y);
00467       result += f (width-(end_x-(width-1)), 0, width-1, end_y);
00468       result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
00469     }
00470     else
00471     {
00472       result += f (start_x, 0, end_x, end_y);
00473       result += f (start_x, 0, end_x, -start_y);
00474     }
00475   }
00476   else if (end_x >= width)
00477   {
00478     if (end_y >= height)
00479     {
00480       result += f (start_x, start_y, width-1, height-1);
00481       result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
00482       result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
00483       result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
00484     }
00485     else
00486     {
00487       result += f (start_x, start_y, width-1, end_y);
00488       result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
00489     }
00490   }
00491   else if (end_y >= height)
00492   {
00493     result += f (start_x, start_y, end_x, height-1);
00494     result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
00495   }
00496   else
00497   {
00498     result += f (start_x, start_y, end_x, end_y);
00499   }
00500 }
00501 
00503 template <typename PointInT, typename PointOutT> void
00504 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
00505     const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
00506 {
00507   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00508 
00509   const int width = input_->width;
00510   const int height = input_->height;
00511 
00512   if (normal_estimation_method_ == COVARIANCE_MATRIX) // ==============================================================
00513   {
00514     if (!init_covariance_matrix_)
00515       initCovarianceMatrixMethod ();
00516 
00517     const int start_x = pos_x - rect_width_2_;
00518     const int start_y = pos_y - rect_height_2_;
00519     const int end_x = start_x + rect_width_;
00520     const int end_y = start_y + rect_height_;
00521 
00522     unsigned count = 0;
00523     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);
00524     
00525     // no valid points within the rectangular reagion?
00526     if (count == 0)
00527     {
00528       normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00529       return;
00530     }
00531 
00532     EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00533     Eigen::Vector3f center;
00534     typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
00535     typename IntegralImage2D<float, 3>::ElementType tmp_center;
00536     typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
00537 
00538     center[0] = 0;
00539     center[1] = 0;
00540     center[2] = 0;
00541     tmp_center[0] = 0;
00542     tmp_center[1] = 0;
00543     tmp_center[2] = 0;
00544     so_elements[0] = 0;
00545     so_elements[1] = 0;
00546     so_elements[2] = 0;
00547     so_elements[3] = 0;
00548     so_elements[4] = 0;
00549     so_elements[5] = 0;
00550 
00551     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);
00552     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);
00553 
00554     center[0] = tmp_center[0];
00555     center[1] = tmp_center[1];
00556     center[2] = tmp_center[2];
00557 
00558     covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
00559     covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
00560     covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
00561     covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
00562     covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
00563     covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
00564     covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
00565     float eigen_value;
00566     Eigen::Vector3f eigen_vector;
00567     pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
00568     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector);
00569     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
00570     normal.getNormalVector3fMap () = eigen_vector;
00571 
00572     // Compute the curvature surface change
00573     if (eigen_value > 0.0)
00574       normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
00575     else
00576       normal.curvature = 0;
00577 
00578     return;
00579   }
00580   else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) // =======================================================
00581   {
00582     if (!init_average_3d_gradient_)
00583       initAverage3DGradientMethod ();
00584 
00585     const int start_x = pos_x - rect_width_2_;
00586     const int start_y = pos_y - rect_height_2_;
00587     const int end_x = start_x + rect_width_;
00588     const int end_y = start_y + rect_height_;
00589 
00590     unsigned count_x = 0;
00591     unsigned count_y = 0;
00592 
00593     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);
00594     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);
00595 
00596 
00597     if (count_x == 0 || count_y == 0)
00598     {
00599       normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00600       return;
00601     }
00602     //Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00603     //Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
00604 
00605     Eigen::Vector3d gradient_x (0, 0, 0);
00606     Eigen::Vector3d gradient_y (0, 0, 0);
00607 
00608     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);
00609     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);
00610 
00611 
00612     Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
00613     double normal_length = normal_vector.squaredNorm ();
00614     if (normal_length == 0.0f)
00615     {
00616       normal.getNormalVector4fMap ().setConstant (bad_point);
00617       normal.curvature = bad_point;
00618       return;
00619     }
00620 
00621     normal_vector /= sqrt (normal_length);
00622 
00623     float nx = static_cast<float> (normal_vector [0]);
00624     float ny = static_cast<float> (normal_vector [1]);
00625     float nz = static_cast<float> (normal_vector [2]);
00626 
00627     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
00628     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
00629 
00630     normal.normal_x = nx;
00631     normal.normal_y = ny;
00632     normal.normal_z = nz;
00633     normal.curvature = bad_point;
00634     return;
00635   }
00636   else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) // ======================================================
00637   {
00638     if (!init_depth_change_)
00639       initAverageDepthChangeMethod ();
00640 
00641     //const size_t point_index_L = point_index - rect_width_4_ - 1;
00642     //const size_t point_index_R = point_index + rect_width_4_ + 1;
00643     //const size_t point_index_U = point_index - rect_height_4_ * width - 1;
00644     //const size_t point_index_D = point_index + rect_height_4_ * width + 1;
00645 
00646     int point_index_L_x = pos_x - rect_width_4_ - 1;
00647     int point_index_L_y = pos_y;
00648     int point_index_R_x = pos_x + rect_width_4_ + 1;
00649     int point_index_R_y = pos_y;
00650     int point_index_U_x = pos_x - 1;
00651     int point_index_U_y = pos_y - rect_height_4_;
00652     int point_index_D_x = pos_x + 1;
00653     int point_index_D_y = pos_y + rect_height_4_;
00654 
00655     if (point_index_L_x < 0)
00656       point_index_L_x = -point_index_L_x;
00657     if (point_index_U_x < 0)
00658       point_index_U_x = -point_index_U_x;
00659     if (point_index_U_y < 0)
00660       point_index_U_y = -point_index_U_y;
00661 
00662     if (point_index_R_x >= width)
00663       point_index_R_x = width-(point_index_R_x-(width-1));
00664     if (point_index_D_x >= width)
00665       point_index_D_x = width-(point_index_D_x-(width-1));
00666     if (point_index_D_y >= height)
00667       point_index_D_y = height-(point_index_D_y-(height-1));
00668 
00669     //const size_t min_x = pos_x - rect_width_4_ - 1;
00670     //const size_t max_x = pos_x + rect_width_4_ + 1;
00671     //const size_t min_y = pos_y - rect_height_4_ - 1;
00672     //const size_t max_y = pos_y + rect_height_4_ + 1;
00673 
00674     //if (min_x >= width || max_x >= width || min_y >= height || max_y >= height)
00675     //{
00676     //  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00677     //  return;
00678     //}
00679 
00680 
00681     const int start_x_L = pos_x - rect_width_2_;
00682     const int start_y_L = pos_y - rect_height_4_;
00683     const int end_x_L = start_x_L + rect_width_2_;
00684     const int end_y_L = start_y_L + rect_height_2_;
00685 
00686     const int start_x_R = pos_x + 1;
00687     const int start_y_R = pos_y - rect_height_4_;
00688     const int end_x_R = start_x_R + rect_width_2_;
00689     const int end_y_R = start_y_R + rect_height_2_;
00690 
00691     const int start_x_U = pos_x - rect_width_4_;
00692     const int start_y_U = pos_y - rect_height_2_;
00693     const int end_x_U = start_x_U + rect_width_2_;
00694     const int end_y_U = start_y_U + rect_height_2_;
00695 
00696     const int start_x_D = pos_x - rect_width_4_;
00697     const int start_y_D = pos_y + 1;
00698     const int end_x_D = start_x_D + rect_width_2_;
00699     const int end_y_D = start_y_D + rect_height_2_;
00700 
00701     // width and height are at least 3 x 3
00702     //unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
00703     //unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1            , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
00704     //unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
00705     //unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1             , rect_width_2_, rect_height_2_);
00706 
00707     unsigned count_L_z = 0;
00708     unsigned count_R_z = 0;
00709     unsigned count_U_z = 0;
00710     unsigned count_D_z = 0;
00711 
00712     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);
00713     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);
00714     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);
00715     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);
00716 
00717     if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
00718     {
00719       normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
00720       return;
00721     }
00722 
00723     //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);
00724     //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);
00725     //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);
00726     //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);
00727 
00728     float mean_L_z = 0;
00729     float mean_R_z = 0;
00730     float mean_U_z = 0;
00731     float mean_D_z = 0;
00732 
00733     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);
00734     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);
00735     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);
00736     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);
00737 
00738     mean_L_z /= count_L_z;
00739     mean_R_z /= count_R_z;
00740     mean_U_z /= count_U_z;
00741     mean_D_z /= count_D_z;
00742 
00743 
00744     //PointInT pointL = input_->points[point_index - rect_width_4_ - 1];
00745     //PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
00746     //PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
00747     //PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1];
00748     PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
00749     PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
00750     PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
00751     PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];
00752 
00753     const float mean_x_z = mean_R_z - mean_L_z;
00754     const float mean_y_z = mean_D_z - mean_U_z;
00755 
00756     const float mean_x_x = pointR.x - pointL.x;
00757     const float mean_x_y = pointR.y - pointL.y;
00758     const float mean_y_x = pointD.x - pointU.x;
00759     const float mean_y_y = pointD.y - pointU.y;
00760 
00761     float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
00762     float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
00763     float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
00764 
00765     const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
00766 
00767     if (normal_length == 0.0f)
00768     {
00769       normal.getNormalVector4fMap ().setConstant (bad_point);
00770       normal.curvature = bad_point;
00771       return;
00772     }
00773 
00774     pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
00775     
00776     const float scale = 1.0f / sqrt (normal_length);
00777 
00778     normal.normal_x = normal_x * scale;
00779     normal.normal_y = normal_y * scale;
00780     normal.normal_z = normal_z * scale;
00781     normal.curvature = bad_point;
00782 
00783     return;
00784   }
00785   else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) // ========================================================
00786   {
00787     PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
00788 
00789     //if (!init_simple_3d_gradient_)
00790     //  initSimple3DGradientMethod ();
00791 
00795 
00798 
00799 
00800     //const int start_x = pos_x - rect_width_2_;
00801     //const int start_y = pos_y - rect_height_2_;
00802     //const int end_x = start_x + rect_width_;
00803     //const int end_y = start_y + rect_height_;
00804 
00805     //Eigen::Vector3d gradient_x (0, 0, 0);
00806     //Eigen::Vector3d gradient_y (0, 0, 0);
00807 
00808     //sumArea<typename IntegralImage2D<float, 3>::ElementType>(pos_x - rect_width_2_,  pos_y - rect_height_2_,  pos_x - rect_width_2_ + 1,  pos_y - rect_height_2_ + rect_height_, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), gradient_x);
00809     //gradient_x *= -1;
00810     //sumArea<typename IntegralImage2D<float, 3>::ElementType>(pos_x + rect_width_2_,  pos_y - rect_height_2_,  pos_x + rect_width_2_ + 1,  pos_y - rect_height_2_ + rect_height_, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), gradient_x);
00811 
00812     //sumArea<typename IntegralImage2D<float, 3>::ElementType>(pos_x - rect_width_2_,  pos_y - rect_height_2_,  pos_x - rect_width_2_ + rect_width_,  pos_y - rect_height_2_ + 1,  width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), gradient_y);
00813     //gradient_y *= -1;
00814     //sumArea<typename IntegralImage2D<float, 3>::ElementType>(pos_x - rect_width_2_,  pos_y + rect_height_2_,  pos_x - rect_width_2_ + rect_width_,  pos_y + rect_height_2_ + 1,  width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), gradient_y);
00815 
00816 
00817     //Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
00818     //double normal_length = normal_vector.squaredNorm ();
00819     //if (normal_length == 0.0f)
00820     //{
00821     //  normal.getNormalVector4fMap ().setConstant (bad_point);
00822     //  normal.curvature = bad_point;
00823     //  return;
00824     //}
00825 
00826     //normal_vector /= sqrt (normal_length);
00827 
00828     //float nx = static_cast<float> (normal_vector [0]);
00829     //float ny = static_cast<float> (normal_vector [1]);
00830     //float nz = static_cast<float> (normal_vector [2]);
00831 
00833     //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
00834     //
00835     //normal.normal_x = nx;
00836     //normal.normal_y = ny;
00837     //normal.normal_z = nz;
00838     //normal.curvature = bad_point;
00839     //return;
00840   }
00841 
00842   normal.getNormalVector4fMap ().setConstant (bad_point);
00843   normal.curvature = bad_point;
00844   return;
00845 }
00846 
00848 template <typename PointInT, typename PointOutT> void
00849 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00850 {
00851   output.sensor_origin_ = input_->sensor_origin_;
00852   output.sensor_orientation_ = input_->sensor_orientation_;
00853   
00854   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00855 
00856   // compute depth-change map
00857   unsigned char * depthChangeMap = new unsigned char[input_->points.size ()];
00858   memset (depthChangeMap, 255, input_->points.size ());
00859 
00860   unsigned index = 0;
00861   for (unsigned int ri = 0; ri < input_->height-1; ++ri)
00862   {
00863     for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
00864     {
00865       index = ri * input_->width + ci;
00866 
00867       const float depth  = input_->points [index].z;
00868       const float depthR = input_->points [index + 1].z;
00869       const float depthD = input_->points [index + input_->width].z;
00870 
00871       //const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f);
00872       const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f);
00873 
00874       if (fabs (depth - depthR) > depthDependendDepthChange
00875         || !pcl_isfinite (depth) || !pcl_isfinite (depthR))
00876       {
00877         depthChangeMap[index] = 0;
00878         depthChangeMap[index+1] = 0;
00879       }
00880       if (fabs (depth - depthD) > depthDependendDepthChange
00881         || !pcl_isfinite (depth) || !pcl_isfinite (depthD))
00882       {
00883         depthChangeMap[index] = 0;
00884         depthChangeMap[index + input_->width] = 0;
00885       }
00886     }
00887   }
00888 
00889   // compute distance map
00890   //float *distanceMap = new float[input_->points.size ()];
00891   if (distance_map_ != NULL) delete distance_map_;
00892   distance_map_ = new float[input_->points.size ()];
00893   float *distanceMap = distance_map_;
00894   for (size_t index = 0; index < input_->points.size (); ++index)
00895   {
00896     if (depthChangeMap[index] == 0)
00897       distanceMap[index] = 0.0f;
00898     else
00899       distanceMap[index] = static_cast<float> (input_->width + input_->height);
00900   }
00901 
00902   // first pass
00903   float* previous_row = distanceMap;
00904   float* current_row = previous_row + input_->width;
00905   for (size_t ri = 1; ri < input_->height; ++ri)
00906   {
00907     for (size_t ci = 1; ci < input_->width; ++ci)
00908     {
00909       const float upLeft  = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
00910       const float up      = previous_row [ci] + 1.0f;     //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
00911       const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
00912       const float left    = current_row  [ci - 1] + 1.0f;  //distanceMap[ri*input_->width + ci-1] + 1.0f;
00913       const float center  = current_row  [ci];             //distanceMap[ri*input_->width + ci];
00914 
00915       const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
00916 
00917       if (minValue < center)
00918         current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
00919     }
00920     previous_row = current_row;
00921     current_row += input_->width;
00922   }
00923 
00924   float* next_row    = distanceMap + input_->width * (input_->height - 1);
00925   current_row = next_row - input_->width;
00926   // second pass
00927   for (int ri = input_->height-2; ri >= 0; --ri)
00928   {
00929     for (int ci = input_->width-2; ci >= 0; --ci)
00930     {
00931       const float lowerLeft  = next_row [ci - 1] + 1.4f;    //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
00932       const float lower      = next_row [ci] + 1.0f;        //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
00933       const float lowerRight = next_row [ci + 1] + 1.4f;    //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
00934       const float right      = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
00935       const float center     = current_row [ci];            //distanceMap[ri*input_->width + ci];
00936 
00937       const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
00938 
00939       if (minValue < center)
00940         current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
00941     }
00942     next_row = current_row;
00943     current_row -= input_->width;
00944   }
00945 
00946   if (border_policy_ == BORDER_POLICY_IGNORE)
00947   {
00948     // Set all normals that we do not touch to NaN
00949     // top and bottom borders
00950     // That sets the output density to false!
00951     output.is_dense = false;
00952     unsigned border = int(normal_smoothing_size_);
00953     PointOutT* vec1 = &output [0];
00954     PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
00955 
00956     size_t count = border * input_->width;
00957     for (size_t idx = 0; idx < count; ++idx)
00958     {
00959       vec1 [idx].getNormalVector4fMap ().setConstant (bad_point);
00960       vec1 [idx].curvature = bad_point;
00961       vec2 [idx].getNormalVector4fMap ().setConstant (bad_point);
00962       vec2 [idx].curvature = bad_point;
00963     }
00964 
00965     // left and right borders actually columns
00966     vec1 = &output [border * input_->width];
00967     vec2 = vec1 + input_->width - border;
00968     for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
00969     {
00970       for (size_t ci = 0; ci < border; ++ci)
00971       {
00972         vec1 [ci].getNormalVector4fMap ().setConstant (bad_point);
00973         vec1 [ci].curvature = bad_point;
00974         vec2 [ci].getNormalVector4fMap ().setConstant (bad_point);
00975         vec2 [ci].curvature = bad_point;
00976       }
00977     }
00978 
00979     if (use_depth_dependent_smoothing_)
00980     {
00981       index = border + input_->width * border;
00982       unsigned skip = (border << 1);
00983       for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
00984       {
00985         for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
00986         {
00987           index = ri * input_->width + ci;
00988 
00989           const float depth = input_->points[index].z;
00990           if (!pcl_isfinite (depth))
00991           {
00992             output[index].getNormalVector4fMap ().setConstant (bad_point);
00993             output[index].curvature = bad_point;
00994             continue;
00995           }
00996 
00997           float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
00998 
00999           if (smoothing > 2.0f)
01000           {
01001             setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
01002             computePointNormal (ci, ri, index, output [index]);
01003           }
01004           else
01005           {
01006             output[index].getNormalVector4fMap ().setConstant (bad_point);
01007             output[index].curvature = bad_point;
01008           }
01009         }
01010       }
01011     }
01012     else
01013     {
01014       float smoothing_constant = normal_smoothing_size_;
01015 
01016       index = border + input_->width * border;
01017       unsigned skip = (border << 1);
01018       for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
01019       {
01020         for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
01021         {
01022           index = ri * input_->width + ci;
01023 
01024           if (!pcl_isfinite (input_->points[index].z))
01025           {
01026             output [index].getNormalVector4fMap ().setConstant (bad_point);
01027             output [index].curvature = bad_point;
01028             continue;
01029           }
01030 
01031           float smoothing = (std::min)(distanceMap[index], smoothing_constant);
01032 
01033           if (smoothing > 2.0f)
01034           {
01035             setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
01036             computePointNormal (ci, ri, index, output [index]);
01037           }
01038           else
01039           {
01040             output [index].getNormalVector4fMap ().setConstant (bad_point);
01041             output [index].curvature = bad_point;
01042           }
01043         }
01044       }
01045     }
01046   }
01047   else if (border_policy_ == BORDER_POLICY_MIRROR)
01048   {
01049     output.is_dense = false;
01050 
01051     if (use_depth_dependent_smoothing_)
01052     {
01053       //index = 0;
01054       //unsigned skip = 0;
01055       //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
01056       for (unsigned ri = 0; ri < input_->height; ++ri)
01057       {
01058         //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
01059         for (unsigned ci = 0; ci < input_->width; ++ci)
01060         {
01061           index = ri * input_->width + ci;
01062 
01063           const float depth = input_->points[index].z;
01064           if (!pcl_isfinite (depth))
01065           {
01066             output[index].getNormalVector4fMap ().setConstant (bad_point);
01067             output[index].curvature = bad_point;
01068             continue;
01069           }
01070 
01071           float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
01072 
01073           if (smoothing > 2.0f)
01074           {
01075             setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
01076             computePointNormalMirror (ci, ri, index, output [index]);
01077           }
01078           else
01079           {
01080             output[index].getNormalVector4fMap ().setConstant (bad_point);
01081             output[index].curvature = bad_point;
01082           }
01083         }
01084       }
01085     }
01086     else
01087     {
01088       float smoothing_constant = normal_smoothing_size_;
01089 
01090       //index = border + input_->width * border;
01091       //unsigned skip = (border << 1);
01092       //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
01093       for (unsigned ri = 0; ri < input_->height; ++ri)
01094       {
01095         //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
01096         for (unsigned ci = 0; ci < input_->width; ++ci)
01097         {
01098           index = ri * input_->width + ci;
01099 
01100           if (!pcl_isfinite (input_->points[index].z))
01101           {
01102             output [index].getNormalVector4fMap ().setConstant (bad_point);
01103             output [index].curvature = bad_point;
01104             continue;
01105           }
01106 
01107           float smoothing = (std::min)(distanceMap[index], smoothing_constant);
01108 
01109           if (smoothing > 2.0f)
01110           {
01111             setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
01112             computePointNormalMirror (ci, ri, index, output [index]);
01113           }
01114           else
01115           {
01116             output [index].getNormalVector4fMap ().setConstant (bad_point);
01117             output [index].curvature = bad_point;
01118           }
01119         }
01120       }
01121     }
01122   }
01123 
01124   delete[] depthChangeMap;
01125   //delete[] distanceMap;
01126 }
01127 
01129 template <typename PointInT, typename PointOutT> bool
01130 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCompute ()
01131 {
01132   if (!input_->isOrganized ())
01133   {
01134     PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
01135     return (false);
01136   }
01137   return (Feature<PointInT, PointOutT>::initCompute ());
01138 }
01139 
01140 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
01141 
01142 #endif
01143 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:28