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_FILTERS_RADIUS_OUTLIER_REMOVAL_H_ 00041 #define PCL_FILTERS_RADIUS_OUTLIER_REMOVAL_H_ 00042 00043 #include <pcl/filters/filter_indices.h> 00044 #include <pcl/search/pcl_search.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief @b RadiusOutlierRemoval filters points in a cloud based on the number of neighbors they have. 00049 * \details Iterates through the entire input once, and for each point, retrieves the number of neighbors within a certain radius. 00050 * The point will be considered an outlier if it has too few neighbors, as determined by setMinNeighborsInRadius(). 00051 * The radius can be changed using setRadiusSearch(). 00052 * <br> 00053 * The neighbors found for each query point will be found amongst ALL points of setInputCloud(), not just those indexed by setIndices(). 00054 * The setIndices() method only indexes the points that will be iterated through as search query points. 00055 * <br><br> 00056 * Usage example: 00057 * \code 00058 * pcl::RadiusOutlierRemoval<PointType> rorfilter (true); // Initializing with true will allow us to extract the removed indices 00059 * rorfilter.setInputCloud (cloud_in); 00060 * rorfilter.setRadiusSearch (0.1); 00061 * rorfilter.setMinNeighborsInRadius (5); 00062 * rorfilter.setNegative (true); 00063 * rorfilter.filter (*cloud_out); 00064 * // The resulting cloud_out contains all points of cloud_in that have 4 or less neighbors within the 0.1 search radius 00065 * indices_rem = rorfilter.getRemovedIndices (); 00066 * // The indices_rem array indexes all points of cloud_in that have 5 or more neighbors within the 0.1 search radius 00067 * \endcode 00068 * \author Radu Bogdan Rusu 00069 * \ingroup filters 00070 */ 00071 template<typename PointT> 00072 class RadiusOutlierRemoval : public FilterIndices<PointT> 00073 { 00074 protected: 00075 typedef typename FilterIndices<PointT>::PointCloud PointCloud; 00076 typedef typename PointCloud::Ptr PointCloudPtr; 00077 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00078 typedef typename pcl::search::Search<PointT>::Ptr SearcherPtr; 00079 00080 public: 00081 00082 typedef boost::shared_ptr< RadiusOutlierRemoval<PointT> > Ptr; 00083 typedef boost::shared_ptr< const RadiusOutlierRemoval<PointT> > ConstPtr; 00084 00085 00086 /** \brief Constructor. 00087 * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). 00088 */ 00089 RadiusOutlierRemoval (bool extract_removed_indices = false) : 00090 FilterIndices<PointT>::FilterIndices (extract_removed_indices), 00091 searcher_ (), 00092 search_radius_ (0.0), 00093 min_pts_radius_ (1) 00094 { 00095 filter_name_ = "RadiusOutlierRemoval"; 00096 } 00097 00098 /** \brief Set the radius of the sphere that will determine which points are neighbors. 00099 * \details The number of points within this distance from the query point will need to be equal or greater 00100 * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered). 00101 * \param[in] radius The radius of the sphere for nearest neighbor searching. 00102 */ 00103 inline void 00104 setRadiusSearch (double radius) 00105 { 00106 search_radius_ = radius; 00107 } 00108 00109 /** \brief Get the radius of the sphere that will determine which points are neighbors. 00110 * \details The number of points within this distance from the query point will need to be equal or greater 00111 * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered). 00112 * \return The radius of the sphere for nearest neighbor searching. 00113 */ 00114 inline double 00115 getRadiusSearch () 00116 { 00117 return (search_radius_); 00118 } 00119 00120 /** \brief Set the number of neighbors that need to be present in order to be classified as an inlier. 00121 * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater 00122 * than this number in order to be classified as an inlier point (i.e. will not be filtered). 00123 * \param min_pts The minimum number of neighbors (default = 1). 00124 */ 00125 inline void 00126 setMinNeighborsInRadius (int min_pts) 00127 { 00128 min_pts_radius_ = min_pts; 00129 } 00130 00131 /** \brief Get the number of neighbors that need to be present in order to be classified as an inlier. 00132 * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater 00133 * than this number in order to be classified as an inlier point (i.e. will not be filtered). 00134 * \param min_pts The minimum number of neighbors (default = 1). 00135 */ 00136 inline int 00137 getMinNeighborsInRadius () 00138 { 00139 return (min_pts_radius_); 00140 } 00141 00142 protected: 00143 using PCLBase<PointT>::input_; 00144 using PCLBase<PointT>::indices_; 00145 using Filter<PointT>::filter_name_; 00146 using Filter<PointT>::getClassName; 00147 using FilterIndices<PointT>::negative_; 00148 using FilterIndices<PointT>::keep_organized_; 00149 using FilterIndices<PointT>::user_filter_value_; 00150 using FilterIndices<PointT>::extract_removed_indices_; 00151 using FilterIndices<PointT>::removed_indices_; 00152 00153 /** \brief Filtered results are stored in a separate point cloud. 00154 * \param[out] output The resultant point cloud. 00155 */ 00156 void 00157 applyFilter (PointCloud &output); 00158 00159 /** \brief Filtered results are indexed by an indices array. 00160 * \param[out] indices The resultant indices. 00161 */ 00162 void 00163 applyFilter (std::vector<int> &indices) 00164 { 00165 applyFilterIndices (indices); 00166 } 00167 00168 /** \brief Filtered results are indexed by an indices array. 00169 * \param[out] indices The resultant indices. 00170 */ 00171 void 00172 applyFilterIndices (std::vector<int> &indices); 00173 00174 private: 00175 /** \brief A pointer to the spatial search object. */ 00176 SearcherPtr searcher_; 00177 00178 /** \brief The nearest neighbors search radius for each point. */ 00179 double search_radius_; 00180 00181 /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered an inlier. */ 00182 int min_pts_radius_; 00183 }; 00184 00185 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00186 /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain 00187 * search radius is smaller than a given K. 00188 * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. 00189 * \author Radu Bogdan Rusu 00190 * \ingroup filters 00191 */ 00192 template<> 00193 class PCL_EXPORTS RadiusOutlierRemoval<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2> 00194 { 00195 using Filter<pcl::PCLPointCloud2>::filter_name_; 00196 using Filter<pcl::PCLPointCloud2>::getClassName; 00197 00198 using Filter<pcl::PCLPointCloud2>::removed_indices_; 00199 using Filter<pcl::PCLPointCloud2>::extract_removed_indices_; 00200 00201 typedef pcl::search::Search<pcl::PointXYZ> KdTree; 00202 typedef pcl::search::Search<pcl::PointXYZ>::Ptr KdTreePtr; 00203 00204 typedef pcl::PCLPointCloud2 PCLPointCloud2; 00205 typedef PCLPointCloud2::Ptr PCLPointCloud2Ptr; 00206 typedef PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr; 00207 00208 public: 00209 /** \brief Empty constructor. */ 00210 RadiusOutlierRemoval (bool extract_removed_indices = false) : 00211 Filter<pcl::PCLPointCloud2>::Filter (extract_removed_indices), 00212 search_radius_ (0.0), min_pts_radius_ (1), tree_ () 00213 { 00214 filter_name_ = "RadiusOutlierRemoval"; 00215 } 00216 00217 /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors for filtering. 00218 * \param radius the sphere radius that is to contain all k-nearest neighbors 00219 */ 00220 inline void 00221 setRadiusSearch (double radius) 00222 { 00223 search_radius_ = radius; 00224 } 00225 00226 /** \brief Get the sphere radius used for determining the k-nearest neighbors. */ 00227 inline double 00228 getRadiusSearch () 00229 { 00230 return (search_radius_); 00231 } 00232 00233 /** \brief Set the minimum number of neighbors that a point needs to have in the given search radius in order to 00234 * be considered an inlier (i.e., valid). 00235 * \param min_pts the minimum number of neighbors 00236 */ 00237 inline void 00238 setMinNeighborsInRadius (int min_pts) 00239 { 00240 min_pts_radius_ = min_pts; 00241 } 00242 00243 /** \brief Get the minimum number of neighbors that a point needs to have in the given search radius to be 00244 * considered an inlier and avoid being filtered. 00245 */ 00246 inline double 00247 getMinNeighborsInRadius () 00248 { 00249 return (min_pts_radius_); 00250 } 00251 00252 protected: 00253 /** \brief The nearest neighbors search radius for each point. */ 00254 double search_radius_; 00255 00256 /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered 00257 * an inlier. 00258 */ 00259 int min_pts_radius_; 00260 00261 /** \brief A pointer to the spatial search object. */ 00262 KdTreePtr tree_; 00263 00264 void 00265 applyFilter (PCLPointCloud2 &output); 00266 }; 00267 } 00268 00269 #ifdef PCL_NO_PRECOMPILE 00270 #include <pcl/filters/impl/radius_outlier_removal.hpp> 00271 #endif 00272 00273 #endif // PCL_FILTERS_RADIUS_OUTLIER_REMOVAL_H_ 00274