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_CONVOLUTION_3D_H 00041 #define PCL_FILTERS_CONVOLUTION_3D_H 00042 00043 #include <pcl/pcl_base.h> 00044 #include <pcl/filters/boost.h> 00045 #include <pcl/search/pcl_search.h> 00046 00047 namespace pcl 00048 { 00049 namespace filters 00050 { 00051 /** \brief Class ConvolvingKernel base class for all convolving kernels 00052 * \ingroup filters 00053 */ 00054 template<typename PointInT, typename PointOutT> 00055 class ConvolvingKernel 00056 { 00057 public: 00058 typedef boost::shared_ptr<ConvolvingKernel<PointInT, PointOutT> > Ptr; 00059 typedef boost::shared_ptr<const ConvolvingKernel<PointInT, PointOutT> > ConstPtr; 00060 00061 typedef typename PointCloud<PointInT>::ConstPtr PointCloudInConstPtr; 00062 00063 /// \brief empty constructor 00064 ConvolvingKernel () {} 00065 00066 /// \brief empty destructor 00067 virtual ~ConvolvingKernel () {} 00068 00069 /** \brief Set input cloud 00070 * \param[in] input source point cloud 00071 */ 00072 void 00073 setInputCloud (const PointCloudInConstPtr& input) { input_ = input; } 00074 00075 /** \brief Convolve point at the center of this local information 00076 * \param[in] indices indices of the point in the source point cloud 00077 * \param[in] distances euclidean distance squared from the query point 00078 * \return the convolved point 00079 */ 00080 virtual PointOutT 00081 operator() (const std::vector<int>& indices, const std::vector<float>& distances) = 0; 00082 00083 /** \brief Must call this methode before doing any computation 00084 * \note make sure to override this with at least 00085 * \code 00086 * bool initCompute () 00087 * { 00088 * return (true); 00089 * } 00090 * \endcode 00091 * in your kernel interface, else you are going nowhere! 00092 */ 00093 virtual bool 00094 initCompute () { return false; } 00095 00096 /** \brief Utility function that annihilates a point making it fail the \ref pcl::isFinite test 00097 * \param[in/out] p point to annihilate 00098 */ 00099 static void 00100 makeInfinite (PointOutT& p) 00101 { 00102 p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN (); 00103 } 00104 00105 protected: 00106 /// source cloud 00107 PointCloudInConstPtr input_; 00108 }; 00109 00110 /** \brief Gaussian kernel implementation interface 00111 * Use this as implementation reference 00112 * \ingroup filters 00113 */ 00114 template<typename PointInT, typename PointOutT> 00115 class GaussianKernel : public ConvolvingKernel <PointInT, PointOutT> 00116 { 00117 public: 00118 using ConvolvingKernel<PointInT, PointOutT>::initCompute; 00119 using ConvolvingKernel<PointInT, PointOutT>::input_; 00120 using ConvolvingKernel<PointInT, PointOutT>::operator (); 00121 using ConvolvingKernel<PointInT, PointOutT>::makeInfinite; 00122 typedef boost::shared_ptr<GaussianKernel<PointInT, PointOutT> > Ptr; 00123 typedef boost::shared_ptr<GaussianKernel<PointInT, PointOutT> > ConstPtr; 00124 00125 /** Default constructor */ 00126 GaussianKernel () 00127 : ConvolvingKernel <PointInT, PointOutT> () 00128 , sigma_ (0) 00129 , threshold_ (std::numeric_limits<float>::infinity ()) 00130 {} 00131 00132 virtual ~GaussianKernel () {} 00133 00134 /** Set the sigma parameter of the Gaussian 00135 * \param[in] sigma 00136 */ 00137 inline void 00138 setSigma (float sigma) { sigma_ = sigma; } 00139 00140 /** Set the distance threshold relative to a sigma factor i.e. points such as 00141 * ||pi - q|| > sigma_coefficient^2 * sigma^2 are not considered. 00142 */ 00143 inline void 00144 setThresholdRelativeToSigma (float sigma_coefficient) 00145 { 00146 sigma_coefficient_.reset (sigma_coefficient); 00147 } 00148 00149 /** Set the distance threshold such as pi, ||pi - q|| > threshold are not considered. */ 00150 inline void 00151 setThreshold (float threshold) { threshold_ = threshold; } 00152 00153 /** Must call this methode before doing any computation */ 00154 bool initCompute (); 00155 00156 virtual PointOutT 00157 operator() (const std::vector<int>& indices, const std::vector<float>& distances); 00158 00159 protected: 00160 float sigma_; 00161 float sigma_sqr_; 00162 float threshold_; 00163 boost::optional<float> sigma_coefficient_; 00164 }; 00165 00166 /** \brief Gaussian kernel implementation interface with RGB channel handling 00167 * Use this as implementation reference 00168 * \ingroup filters 00169 */ 00170 template<typename PointInT, typename PointOutT> 00171 class GaussianKernelRGB : public GaussianKernel <PointInT, PointOutT> 00172 { 00173 public: 00174 using GaussianKernel<PointInT, PointOutT>::initCompute; 00175 using GaussianKernel<PointInT, PointOutT>::input_; 00176 using GaussianKernel<PointInT, PointOutT>::operator (); 00177 using GaussianKernel<PointInT, PointOutT>::makeInfinite; 00178 using GaussianKernel<PointInT, PointOutT>::sigma_sqr_; 00179 using GaussianKernel<PointInT, PointOutT>::threshold_; 00180 typedef boost::shared_ptr<GaussianKernelRGB<PointInT, PointOutT> > Ptr; 00181 typedef boost::shared_ptr<GaussianKernelRGB<PointInT, PointOutT> > ConstPtr; 00182 00183 /** Default constructor */ 00184 GaussianKernelRGB () 00185 : GaussianKernel <PointInT, PointOutT> () 00186 {} 00187 00188 ~GaussianKernelRGB () {} 00189 00190 PointOutT 00191 operator() (const std::vector<int>& indices, const std::vector<float>& distances); 00192 }; 00193 00194 /** Convolution3D handles the non organized case where width and height are unknown or if you 00195 * are only interested in convolving based on local neighborhood information. 00196 * The convolving kernel MUST be a radial symmetric and implement \ref ConvolvingKernel 00197 * interface. 00198 */ 00199 template <typename PointIn, typename PointOut, typename KernelT> 00200 class Convolution3D : public pcl::PCLBase <PointIn> 00201 { 00202 public: 00203 typedef typename pcl::PointCloud<PointIn> PointCloudIn; 00204 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; 00205 typedef typename pcl::search::Search<PointIn> KdTree; 00206 typedef typename pcl::search::Search<PointIn>::Ptr KdTreePtr; 00207 typedef typename pcl::PointCloud<PointOut> PointCloudOut; 00208 typedef boost::shared_ptr<Convolution3D<PointIn, PointOut, KernelT> > Ptr; 00209 typedef boost::shared_ptr<Convolution3D<PointIn, PointOut, KernelT> > ConstPtr; 00210 00211 using pcl::PCLBase<PointIn>::indices_; 00212 using pcl::PCLBase<PointIn>::input_; 00213 00214 /** \brief Constructor */ 00215 Convolution3D (); 00216 00217 /** \brief Empty destructor */ 00218 ~Convolution3D () {} 00219 00220 /** \brief Initialize the scheduler and set the number of threads to use. 00221 * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) 00222 */ 00223 inline void 00224 setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } 00225 00226 /** \brief Set convolving kernel 00227 * \param[in] kernel convolving element 00228 */ 00229 inline void 00230 setKernel (const KernelT& kernel) { kernel_ = kernel; } 00231 00232 /** \brief Provide a pointer to the input dataset that we need to estimate features at every point for. 00233 * \param cloud the const boost shared pointer to a PointCloud message 00234 */ 00235 inline void 00236 setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; } 00237 00238 /** \brief Get a pointer to the surface point cloud dataset. */ 00239 inline PointCloudInConstPtr 00240 getSearchSurface () { return (surface_); } 00241 00242 /** \brief Provide a pointer to the search object. 00243 * \param tree a pointer to the spatial search object. 00244 */ 00245 inline void 00246 setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } 00247 00248 /** \brief Get a pointer to the search method used. */ 00249 inline KdTreePtr 00250 getSearchMethod () { return (tree_); } 00251 00252 /** \brief Set the sphere radius that is to be used for determining the nearest neighbors 00253 * \param radius the sphere radius used as the maximum distance to consider a point a neighbor 00254 */ 00255 inline void 00256 setRadiusSearch (double radius) { search_radius_ = radius; } 00257 00258 /** \brief Get the sphere radius used for determining the neighbors. */ 00259 inline double 00260 getRadiusSearch () { return (search_radius_); } 00261 00262 /** Convolve point cloud. 00263 * \param[out] output the convolved cloud 00264 */ 00265 void 00266 convolve (PointCloudOut& output); 00267 00268 protected: 00269 /** \brief initialize computation */ 00270 bool initCompute (); 00271 00272 /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ 00273 PointCloudInConstPtr surface_; 00274 00275 /** \brief A pointer to the spatial search object. */ 00276 KdTreePtr tree_; 00277 00278 /** \brief The nearest neighbors search radius for each point. */ 00279 double search_radius_; 00280 00281 /** \brief number of threads */ 00282 unsigned int threads_; 00283 00284 /** \brief convlving kernel */ 00285 KernelT kernel_; 00286 }; 00287 } 00288 } 00289 00290 #include <pcl/filters/impl/convolution_3d.hpp> 00291 00292 #endif // PCL_FILTERS_CONVOLUTION_3D_H