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-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_