Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/integral_image_normal.h
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