Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_INTEGRALIMAGE_BASED_NORMAL_ESTIMATOR_H_ 00040 #define PCL_INTEGRALIMAGE_BASED_NORMAL_ESTIMATOR_H_ 00041 00042 #include <pcl/point_cloud.h> 00043 #include <pcl/point_types.h> 00044 #include <pcl/features/feature.h> 00045 #include <pcl/features/integral_image2D.h> 00046 00047 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00048 #pragma GCC diagnostic ignored "-Weffc++" 00049 #endif 00050 namespace pcl 00051 { 00052 /** \brief Surface normal estimation on organized data using integral images. 00053 * \author Stefan Holzer 00054 */ 00055 template <typename PointInT, typename PointOutT> 00056 class IntegralImageNormalEstimation: public Feature<PointInT, PointOutT> 00057 { 00058 using Feature<PointInT, PointOutT>::input_; 00059 using Feature<PointInT, PointOutT>::feature_name_; 00060 using Feature<PointInT, PointOutT>::tree_; 00061 using Feature<PointInT, PointOutT>::k_; 00062 00063 public: 00064 typedef boost::shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> > Ptr; 00065 typedef boost::shared_ptr<const IntegralImageNormalEstimation<PointInT, PointOutT> > ConstPtr; 00066 00067 /** \brief Different types of border handling. */ 00068 enum BorderPolicy 00069 { 00070 BORDER_POLICY_IGNORE, 00071 BORDER_POLICY_MIRROR 00072 }; 00073 00074 /** \brief Different normal estimation methods. 00075 * <ul> 00076 * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point 00077 * from the covariance matrix of its local neighborhood.</li> 00078 * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of 00079 * horizontal and vertical 3D gradients and computes the normals using the cross-product between these 00080 * two gradients. 00081 * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals 00082 * from the average depth changes. 00083 * </ul> 00084 */ 00085 enum NormalEstimationMethod 00086 { 00087 COVARIANCE_MATRIX, 00088 AVERAGE_3D_GRADIENT, 00089 AVERAGE_DEPTH_CHANGE, 00090 SIMPLE_3D_GRADIENT 00091 }; 00092 00093 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn; 00094 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00095 00096 /** \brief Constructor */ 00097 IntegralImageNormalEstimation () 00098 : normal_estimation_method_(AVERAGE_3D_GRADIENT) 00099 , border_policy_ (BORDER_POLICY_IGNORE) 00100 , rect_width_ (0), rect_width_2_ (0), rect_width_4_ (0) 00101 , rect_height_ (0), rect_height_2_ (0), rect_height_4_ (0) 00102 , distance_threshold_ (0) 00103 , integral_image_DX_ (false) 00104 , integral_image_DY_ (false) 00105 , integral_image_depth_ (false) 00106 , integral_image_XYZ_ (true) 00107 , diff_x_ (NULL) 00108 , diff_y_ (NULL) 00109 , depth_data_ (NULL) 00110 , distance_map_ (NULL) 00111 , use_depth_dependent_smoothing_ (false) 00112 , max_depth_change_factor_ (20.0f*0.001f) 00113 , normal_smoothing_size_ (10.0f) 00114 , init_covariance_matrix_ (false) 00115 , init_average_3d_gradient_ (false) 00116 , init_simple_3d_gradient_ (false) 00117 , init_depth_change_ (false) 00118 , vpx_ (0.0f) 00119 , vpy_ (0.0f) 00120 , vpz_ (0.0f) 00121 , use_sensor_origin_ (true) 00122 { 00123 feature_name_ = "IntegralImagesNormalEstimation"; 00124 tree_.reset (); 00125 k_ = 1; 00126 } 00127 00128 /** \brief Destructor **/ 00129 virtual ~IntegralImageNormalEstimation (); 00130 00131 /** \brief Set the regions size which is considered for normal estimation. 00132 * \param[in] width the width of the search rectangle 00133 * \param[in] height the height of the search rectangle 00134 */ 00135 void 00136 setRectSize (const int width, const int height); 00137 00138 /** \brief Sets the policy for handling borders. 00139 * \param[in] border_policy the border policy. 00140 */ 00141 void 00142 setBorderPolicy (const BorderPolicy border_policy) 00143 { 00144 border_policy_ = border_policy; 00145 } 00146 00147 /** \brief Computes the normal at the specified position. 00148 * \param[in] pos_x x position (pixel) 00149 * \param[in] pos_y y position (pixel) 00150 * \param[in] point_index the position index of the point 00151 * \param[out] normal the output estimated normal 00152 */ 00153 void 00154 computePointNormal (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal); 00155 00156 /** \brief Computes the normal at the specified position with mirroring for border handling. 00157 * \param[in] pos_x x position (pixel) 00158 * \param[in] pos_y y position (pixel) 00159 * \param[in] point_index the position index of the point 00160 * \param[out] normal the output estimated normal 00161 */ 00162 void 00163 computePointNormalMirror (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal); 00164 00165 /** \brief The depth change threshold for computing object borders 00166 * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on 00167 * depth changes 00168 */ 00169 void 00170 setMaxDepthChangeFactor (float max_depth_change_factor) 00171 { 00172 max_depth_change_factor_ = max_depth_change_factor; 00173 } 00174 00175 /** \brief Set the normal smoothing size 00176 * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals 00177 * (depth dependent if useDepthDependentSmoothing is true) 00178 */ 00179 void 00180 setNormalSmoothingSize (float normal_smoothing_size) 00181 { 00182 if (normal_smoothing_size <= 0) 00183 { 00184 PCL_ERROR ("[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%f). Allowed ranges are: 0 < N. Defaulting to %f.\n", 00185 feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_); 00186 return; 00187 } 00188 normal_smoothing_size_ = normal_smoothing_size; 00189 } 00190 00191 /** \brief Set the normal estimation method. The current implemented algorithms are: 00192 * <ul> 00193 * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point 00194 * from the covariance matrix of its local neighborhood.</li> 00195 * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of 00196 * horizontal and vertical 3D gradients and computes the normals using the cross-product between these 00197 * two gradients. 00198 * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals 00199 * from the average depth changes. 00200 * </ul> 00201 * \param[in] normal_estimation_method the method used for normal estimation 00202 */ 00203 void 00204 setNormalEstimationMethod (NormalEstimationMethod normal_estimation_method) 00205 { 00206 normal_estimation_method_ = normal_estimation_method; 00207 } 00208 00209 /** \brief Set whether to use depth depending smoothing or not 00210 * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent 00211 */ 00212 void 00213 setDepthDependentSmoothing (bool use_depth_dependent_smoothing) 00214 { 00215 use_depth_dependent_smoothing_ = use_depth_dependent_smoothing; 00216 } 00217 00218 /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) 00219 * \param[in] cloud the const boost shared pointer to a PointCloud message 00220 */ 00221 virtual inline void 00222 setInputCloud (const typename PointCloudIn::ConstPtr &cloud) 00223 { 00224 input_ = cloud; 00225 if (!cloud->isOrganized ()) 00226 { 00227 PCL_ERROR ("[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n"); 00228 return; 00229 } 00230 00231 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false; 00232 00233 if (use_sensor_origin_) 00234 { 00235 vpx_ = input_->sensor_origin_.coeff (0); 00236 vpy_ = input_->sensor_origin_.coeff (1); 00237 vpz_ = input_->sensor_origin_.coeff (2); 00238 } 00239 00240 // Initialize the correct data structure based on the normal estimation method chosen 00241 initData (); 00242 } 00243 00244 /** \brief Returns a pointer to the distance map which was computed internally 00245 */ 00246 inline float* 00247 getDistanceMap () 00248 { 00249 return (distance_map_); 00250 } 00251 00252 /** \brief Set the viewpoint. 00253 * \param vpx the X coordinate of the viewpoint 00254 * \param vpy the Y coordinate of the viewpoint 00255 * \param vpz the Z coordinate of the viewpoint 00256 */ 00257 inline void 00258 setViewPoint (float vpx, float vpy, float vpz) 00259 { 00260 vpx_ = vpx; 00261 vpy_ = vpy; 00262 vpz_ = vpz; 00263 use_sensor_origin_ = false; 00264 } 00265 00266 /** \brief Get the viewpoint. 00267 * \param [out] vpx x-coordinate of the view point 00268 * \param [out] vpy y-coordinate of the view point 00269 * \param [out] vpz z-coordinate of the view point 00270 * \note this method returns the currently used viewpoint for normal flipping. 00271 * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates. 00272 * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0) 00273 */ 00274 inline void 00275 getViewPoint (float &vpx, float &vpy, float &vpz) 00276 { 00277 vpx = vpx_; 00278 vpy = vpy_; 00279 vpz = vpz_; 00280 } 00281 00282 /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the 00283 * normal estimation method uses the sensor origin of the input cloud. 00284 * to use a user defined view point, use the method setViewPoint 00285 */ 00286 inline void 00287 useSensorOriginAsViewPoint () 00288 { 00289 use_sensor_origin_ = true; 00290 if (input_) 00291 { 00292 vpx_ = input_->sensor_origin_.coeff (0); 00293 vpy_ = input_->sensor_origin_.coeff (1); 00294 vpz_ = input_->sensor_origin_.coeff (2); 00295 } 00296 else 00297 { 00298 vpx_ = 0; 00299 vpy_ = 0; 00300 vpz_ = 0; 00301 } 00302 } 00303 00304 protected: 00305 00306 /** \brief Computes the normal for the complete cloud. 00307 * \param[out] output the resultant normals 00308 */ 00309 void 00310 computeFeature (PointCloudOut &output); 00311 00312 /** \brief Initialize the data structures, based on the normal estimation method chosen. */ 00313 void 00314 initData (); 00315 00316 private: 00317 00318 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 00319 * \param point a given point 00320 * \param vp_x the X coordinate of the viewpoint 00321 * \param vp_y the X coordinate of the viewpoint 00322 * \param vp_z the X coordinate of the viewpoint 00323 * \param nx the resultant X component of the plane normal 00324 * \param ny the resultant Y component of the plane normal 00325 * \param nz the resultant Z component of the plane normal 00326 * \ingroup features 00327 */ 00328 inline void 00329 flipNormalTowardsViewpoint (const PointInT &point, 00330 float vp_x, float vp_y, float vp_z, 00331 float &nx, float &ny, float &nz) 00332 { 00333 // See if we need to flip any plane normals 00334 vp_x -= point.x; 00335 vp_y -= point.y; 00336 vp_z -= point.z; 00337 00338 // Dot product between the (viewpoint - point) and the plane normal 00339 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); 00340 00341 // Flip the plane normal 00342 if (cos_theta < 0) 00343 { 00344 nx *= -1; 00345 ny *= -1; 00346 nz *= -1; 00347 } 00348 } 00349 00350 /** \brief The normal estimation method to use. Currently, 3 implementations are provided: 00351 * 00352 * - COVARIANCE_MATRIX 00353 * - AVERAGE_3D_GRADIENT 00354 * - AVERAGE_DEPTH_CHANGE 00355 */ 00356 NormalEstimationMethod normal_estimation_method_; 00357 00358 /** \brief The policy for handling borders. */ 00359 BorderPolicy border_policy_; 00360 00361 /** The width of the neighborhood region used for computing the normal. */ 00362 int rect_width_; 00363 int rect_width_2_; 00364 int rect_width_4_; 00365 /** The height of the neighborhood region used for computing the normal. */ 00366 int rect_height_; 00367 int rect_height_2_; 00368 int rect_height_4_; 00369 00370 /** the threshold used to detect depth discontinuities */ 00371 float distance_threshold_; 00372 00373 /** integral image in x-direction */ 00374 IntegralImage2D<float, 3> integral_image_DX_; 00375 /** integral image in y-direction */ 00376 IntegralImage2D<float, 3> integral_image_DY_; 00377 /** integral image */ 00378 IntegralImage2D<float, 1> integral_image_depth_; 00379 /** integral image xyz */ 00380 IntegralImage2D<float, 3> integral_image_XYZ_; 00381 00382 /** derivatives in x-direction */ 00383 float *diff_x_; 00384 /** derivatives in y-direction */ 00385 float *diff_y_; 00386 00387 /** depth data */ 00388 float *depth_data_; 00389 00390 /** distance map */ 00391 float *distance_map_; 00392 00393 /** \brief Smooth data based on depth (true/false). */ 00394 bool use_depth_dependent_smoothing_; 00395 00396 /** \brief Threshold for detecting depth discontinuities */ 00397 float max_depth_change_factor_; 00398 00399 /** \brief */ 00400 float normal_smoothing_size_; 00401 00402 /** \brief True when a dataset has been received and the covariance_matrix data has been initialized. */ 00403 bool init_covariance_matrix_; 00404 00405 /** \brief True when a dataset has been received and the average 3d gradient data has been initialized. */ 00406 bool init_average_3d_gradient_; 00407 00408 /** \brief True when a dataset has been received and the simple 3d gradient data has been initialized. */ 00409 bool init_simple_3d_gradient_; 00410 00411 /** \brief True when a dataset has been received and the depth change data has been initialized. */ 00412 bool init_depth_change_; 00413 00414 /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit 00415 * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */ 00416 float vpx_, vpy_, vpz_; 00417 00418 /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/ 00419 bool use_sensor_origin_; 00420 00421 /** \brief This method should get called before starting the actual computation. */ 00422 bool 00423 initCompute (); 00424 00425 /** \brief Internal initialization method for COVARIANCE_MATRIX estimation. */ 00426 void 00427 initCovarianceMatrixMethod (); 00428 00429 /** \brief Internal initialization method for AVERAGE_3D_GRADIENT estimation. */ 00430 void 00431 initAverage3DGradientMethod (); 00432 00433 /** \brief Internal initialization method for AVERAGE_DEPTH_CHANGE estimation. */ 00434 void 00435 initAverageDepthChangeMethod (); 00436 00437 /** \brief Internal initialization method for SIMPLE_3D_GRADIENT estimation. */ 00438 void 00439 initSimple3DGradientMethod (); 00440 00441 public: 00442 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00443 }; 00444 } 00445 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00446 #pragma GCC diagnostic warning "-Weffc++" 00447 #endif 00448 00449 #ifdef PCL_NO_PRECOMPILE 00450 #include <pcl/features/impl/integral_image_normal.hpp> 00451 #endif 00452 00453 #endif 00454