Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
00041 #define PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
00042 
00043 #include <pcl/segmentation/planar_region.h>
00044 #include <pcl/pcl_base.h>
00045 #include <pcl/common/angles.h>
00046 #include <pcl/PointIndices.h>
00047 #include <pcl/ModelCoefficients.h>
00048 #include <pcl/segmentation/plane_coefficient_comparator.h>
00049 #include <pcl/segmentation/plane_refinement_comparator.h>
00050 
00051 namespace pcl
00052 {
00053   /** \brief OrganizedMultiPlaneSegmentation finds all planes present in the
00054     * input cloud, and outputs a vector of plane equations, as well as a vector
00055     * of point clouds corresponding to the inliers of each detected plane.  Only
00056     * planes with more than min_inliers points are detected.
00057     * Templated on point type, normal type, and label type
00058     *
00059     * \author Alex Trevor, Suat Gedikli
00060     */
00061   template<typename PointT, typename PointNT, typename PointLT>
00062   class OrganizedMultiPlaneSegmentation : public PCLBase<PointT>
00063   {
00064     using PCLBase<PointT>::input_;
00065     using PCLBase<PointT>::indices_;
00066     using PCLBase<PointT>::initCompute;
00067     using PCLBase<PointT>::deinitCompute;
00068 
00069     public:
00070       typedef pcl::PointCloud<PointT> PointCloud;
00071       typedef typename PointCloud::Ptr PointCloudPtr;
00072       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00073 
00074       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00075       typedef typename PointCloudN::Ptr PointCloudNPtr;
00076       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00077 
00078       typedef typename pcl::PointCloud<PointLT> PointCloudL;
00079       typedef typename PointCloudL::Ptr PointCloudLPtr;
00080       typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
00081 
00082       typedef typename pcl::PlaneCoefficientComparator<PointT, PointNT> PlaneComparator;
00083       typedef typename PlaneComparator::Ptr PlaneComparatorPtr;
00084       typedef typename PlaneComparator::ConstPtr PlaneComparatorConstPtr;
00085 
00086       typedef typename pcl::PlaneRefinementComparator<PointT, PointNT, PointLT> PlaneRefinementComparator;
00087       typedef typename PlaneRefinementComparator::Ptr PlaneRefinementComparatorPtr;
00088       typedef typename PlaneRefinementComparator::ConstPtr PlaneRefinementComparatorConstPtr;
00089 
00090       /** \brief Constructor for OrganizedMultiPlaneSegmentation. */
00091       OrganizedMultiPlaneSegmentation () :
00092         normals_ (), 
00093         min_inliers_ (1000), 
00094         angular_threshold_ (pcl::deg2rad (3.0)), 
00095         distance_threshold_ (0.02),
00096         maximum_curvature_ (0.001),
00097         project_points_ (false), 
00098         compare_ (new PlaneComparator ()), refinement_compare_ (new PlaneRefinementComparator ())
00099       {
00100       }
00101 
00102       /** \brief Destructor for OrganizedMultiPlaneSegmentation. */
00103       virtual
00104       ~OrganizedMultiPlaneSegmentation ()
00105       {
00106       }
00107 
00108       /** \brief Provide a pointer to the input normals.
00109         * \param[in] normals the input normal cloud
00110         */
00111       inline void
00112       setInputNormals (const PointCloudNConstPtr &normals) 
00113       {
00114         normals_ = normals;
00115       }
00116 
00117       /** \brief Get the input normals. */
00118       inline PointCloudNConstPtr
00119       getInputNormals () const
00120       {
00121         return (normals_);
00122       }
00123 
00124       /** \brief Set the minimum number of inliers required for a plane
00125         * \param[in] min_inliers the minimum number of inliers required per plane
00126         */
00127       inline void
00128       setMinInliers (unsigned min_inliers)
00129       {
00130         min_inliers_ = min_inliers;
00131       }
00132 
00133       /** \brief Get the minimum number of inliers required per plane. */
00134       inline unsigned
00135       getMinInliers () const
00136       {
00137         return (min_inliers_);
00138       }
00139 
00140       /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
00141         * \param[in] angular_threshold the tolerance in radians
00142         */
00143       inline void
00144       setAngularThreshold (double angular_threshold)
00145       {
00146         angular_threshold_ = angular_threshold;
00147       }
00148 
00149       /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
00150       inline double
00151       getAngularThreshold () const
00152       {
00153         return (angular_threshold_);
00154       }
00155 
00156       /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
00157         * \param[in] distance_threshold the tolerance in meters
00158         */
00159       inline void
00160       setDistanceThreshold (double distance_threshold)
00161       {
00162         distance_threshold_ = distance_threshold;
00163       }
00164 
00165       /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
00166       inline double
00167       getDistanceThreshold () const
00168       {
00169         return (distance_threshold_);
00170       }
00171 
00172       /** \brief Set the maximum curvature allowed for a planar region.
00173         * \param[in] maximum_curvature the maximum curvature
00174         */
00175       inline void
00176       setMaximumCurvature (double maximum_curvature)
00177       {
00178         maximum_curvature_ = maximum_curvature;
00179       }
00180 
00181       /** \brief Get the maximum curvature allowed for a planar region. */
00182       inline double
00183       getMaximumCurvature () const
00184       {
00185         return (maximum_curvature_);
00186       }
00187 
00188       /** \brief Provide a pointer to the comparator to be used for segmentation.
00189         * \param[in] compare A pointer to the comparator to be used for segmentation.
00190         */
00191       void
00192       setComparator (const PlaneComparatorPtr& compare)
00193       {
00194         compare_ = compare;
00195       }
00196 
00197       /** \brief Provide a pointer to the comparator to be used for refinement.
00198         * \param[in] compare A pointer to the comparator to be used for refinement.
00199         */
00200       void
00201       setRefinementComparator (const PlaneRefinementComparatorPtr& compare)
00202       {
00203         refinement_compare_ = compare;
00204       }
00205 
00206       /** \brief Set whether or not to project boundary points to the plane, or leave them in the original 3D space.
00207         * \param[in] project_points true if points should be projected, false if not.
00208         */
00209       void
00210       setProjectPoints (bool project_points)
00211       {
00212         project_points_ = project_points;
00213       }
00214 
00215       /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
00216         * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
00217         * \param[out] inlier_indices a vector of inliers for each detected plane
00218         * \param[out] centroids a vector of centroids for each plane
00219         * \param[out] covariances a vector of covariance matricies for the inliers of each plane
00220         * \param[out] labels a point cloud for the connected component labels of each pixel
00221         * \param[out] label_indices a vector of PointIndices for each labeled component
00222         */
00223       void
00224       segment (std::vector<ModelCoefficients>& model_coefficients, 
00225                std::vector<PointIndices>& inlier_indices,
00226                std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
00227                std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
00228                pcl::PointCloud<PointLT>& labels, 
00229                std::vector<pcl::PointIndices>& label_indices);
00230 
00231       /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
00232         * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
00233         * \param[out] inlier_indices a vector of inliers for each detected plane
00234         */
00235       void
00236       segment (std::vector<ModelCoefficients>& model_coefficients, 
00237                std::vector<PointIndices>& inlier_indices);
00238 
00239       /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
00240         * \param[out] regions a list of resultant planar polygonal regions
00241         */
00242       void
00243       segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
00244       
00245       /** \brief Perform a segmentation, as well as an additional refinement step.  This helps with including points whose normals may not match neighboring points well, but may match the planar model well.
00246         * \param[out] regions A list of regions generated by segmentation and refinement.
00247         */
00248       void
00249       segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
00250 
00251       /** \brief Perform a segmentation, as well as additional refinement step.  Returns intermediate data structures for use in
00252         * subsequent processing.
00253         * \param[out] regions A vector of PlanarRegions generated by segmentation
00254         * \param[out] model_coefficients A vector of model coefficients for each segmented plane
00255         * \param[out] inlier_indices A vector of PointIndices, indicating the inliers to each segmented plane
00256         * \param[out] labels A PointCloud<PointLT> corresponding to the resulting segmentation.
00257         * \param[out] label_indices A vector of PointIndices for each label
00258         * \param[out] boundary_indices A vector of PointIndices corresponding to the outer boundary / contour of each label
00259         */
00260       void
00261       segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions,
00262                         std::vector<ModelCoefficients>& model_coefficients,
00263                         std::vector<PointIndices>& inlier_indices,
00264                         PointCloudLPtr& labels,
00265                         std::vector<pcl::PointIndices>& label_indices,
00266                         std::vector<pcl::PointIndices>& boundary_indices);
00267       
00268       /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation.
00269         * \param [in] model_coefficients The list of segmented model coefficients
00270         * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model
00271         * \param [in] centroids The list of centroids corresponding to each segmented plane
00272         * \param [in] covariances The list of covariances corresponding to each segemented plane
00273         * \param [in] labels The labels produced by the initial segmentation
00274         * \param [in] label_indices The list of indices corresponding to each label
00275         */
00276       void
00277       refine (std::vector<ModelCoefficients>& model_coefficients, 
00278               std::vector<PointIndices>& inlier_indices,
00279               std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
00280               std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
00281               PointCloudLPtr& labels,
00282               std::vector<pcl::PointIndices>& label_indices);
00283 
00284     protected:
00285 
00286       /** \brief A pointer to the input normals */
00287       PointCloudNConstPtr normals_;
00288 
00289       /** \brief The minimum number of inliers required for each plane. */
00290       unsigned min_inliers_;
00291 
00292       /** \brief The tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
00293       double angular_threshold_;
00294 
00295       /** \brief The tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. */
00296       double distance_threshold_;
00297 
00298       /** \brief The tolerance for maximum curvature after fitting a plane.  Used to remove smooth, but non-planar regions. */
00299       double maximum_curvature_;
00300 
00301       /** \brief Whether or not points should be projected to the plane, or left in the original 3D space. */
00302       bool project_points_;
00303 
00304       /** \brief A comparator for comparing neighboring pixels' plane equations. */
00305       PlaneComparatorPtr compare_;
00306 
00307       /** \brief A comparator for use on the refinement step.  Compares points to regions segmented in the first pass. */
00308       PlaneRefinementComparatorPtr refinement_compare_;
00309 
00310       /** \brief Class getName method. */
00311       virtual std::string
00312       getClassName () const
00313       {
00314         return ("OrganizedMultiPlaneSegmentation");
00315       }
00316   };
00317 
00318 }
00319 
00320 #ifdef PCL_NO_PRECOMPILE
00321 #include <pcl/segmentation/impl/organized_multi_plane_segmentation.hpp>
00322 #endif
00323 
00324 #endif //#ifndef PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_