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) 2009-2011, 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 */ 00037 00038 #ifndef PCL_FILTERS_SAMPLING_SURFACE_NORMAL_H_ 00039 #define PCL_FILTERS_SAMPLING_SURFACE_NORMAL_H_ 00040 00041 #include <pcl/filters/filter.h> 00042 #include <time.h> 00043 #include <limits.h> 00044 00045 namespace pcl 00046 { 00047 /** \brief @b SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N points, 00048 * and samples points randomly within each grid. Normal is computed using the N points of each grid. All points 00049 * sampled within a grid are assigned the same normal. 00050 * 00051 * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher) 00052 * \ingroup filters 00053 */ 00054 template<typename PointT> 00055 class SamplingSurfaceNormal: public Filter<PointT> 00056 { 00057 using Filter<PointT>::filter_name_; 00058 using Filter<PointT>::getClassName; 00059 using Filter<PointT>::indices_; 00060 using Filter<PointT>::input_; 00061 00062 typedef typename Filter<PointT>::PointCloud PointCloud; 00063 typedef typename PointCloud::Ptr PointCloudPtr; 00064 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00065 00066 typedef typename Eigen::Matrix<float, Eigen::Dynamic, 1> Vector; 00067 00068 public: 00069 00070 typedef boost::shared_ptr< SamplingSurfaceNormal<PointT> > Ptr; 00071 typedef boost::shared_ptr< const SamplingSurfaceNormal<PointT> > ConstPtr; 00072 00073 /** \brief Empty constructor. */ 00074 SamplingSurfaceNormal () : 00075 sample_ (10), seed_ (static_cast<unsigned int> (time (NULL))), ratio_ () 00076 { 00077 filter_name_ = "SamplingSurfaceNormal"; 00078 srand (seed_); 00079 } 00080 00081 /** \brief Set maximum number of samples in each grid 00082 * \param[in] sample maximum number of samples in each grid 00083 */ 00084 inline void 00085 setSample (unsigned int sample) 00086 { 00087 sample_ = sample; 00088 } 00089 00090 /** \brief Get the value of the internal \a sample parameter. */ 00091 inline unsigned int 00092 getSample () const 00093 { 00094 return (sample_); 00095 } 00096 00097 /** \brief Set seed of random function. 00098 * \param[in] seed the input seed 00099 */ 00100 inline void 00101 setSeed (unsigned int seed) 00102 { 00103 seed_ = seed; 00104 srand (seed_); 00105 } 00106 00107 /** \brief Get the value of the internal \a seed parameter. */ 00108 inline unsigned int 00109 getSeed () const 00110 { 00111 return (seed_); 00112 } 00113 00114 /** \brief Set ratio of points to be sampled in each grid 00115 * \param[in] ratio sample the ratio of points to be sampled in each grid 00116 */ 00117 inline void 00118 setRatio (float ratio) 00119 { 00120 ratio_ = ratio; 00121 } 00122 00123 /** \brief Get the value of the internal \a ratio parameter. */ 00124 inline float 00125 getRatio () const 00126 { 00127 return ratio_; 00128 } 00129 00130 protected: 00131 00132 /** \brief Maximum number of samples in each grid. */ 00133 unsigned int sample_; 00134 /** \brief Random number seed. */ 00135 unsigned int seed_; 00136 /** \brief Ratio of points to be sampled in each grid */ 00137 float ratio_; 00138 00139 /** \brief Sample of point indices into a separate PointCloud 00140 * \param[out] output the resultant point cloud 00141 */ 00142 void 00143 applyFilter (PointCloud &output); 00144 00145 private: 00146 00147 /** \brief @b CompareDim is a comparator object for sorting across a specific dimenstion (i,.e X, Y or Z) 00148 */ 00149 struct CompareDim 00150 { 00151 /** \brief The dimension to sort */ 00152 const int dim; 00153 /** \brief The input point cloud to sort */ 00154 const pcl::PointCloud <PointT>& cloud; 00155 00156 /** \brief Constructor. */ 00157 CompareDim (const int dim, const pcl::PointCloud <PointT>& cloud) : dim (dim), cloud (cloud) 00158 { 00159 } 00160 00161 /** \brief The operator function for sorting. */ 00162 bool 00163 operator () (const int& p0, const int& p1) 00164 { 00165 if (dim == 0) 00166 return (cloud.points[p0].x < cloud.points[p1].x); 00167 else if (dim == 1) 00168 return (cloud.points[p0].y < cloud.points[p1].y); 00169 else if (dim == 2) 00170 return (cloud.points[p0].z < cloud.points[p1].z); 00171 return (false); 00172 } 00173 }; 00174 00175 /** \brief Finds the max and min values in each dimension 00176 * \param[in] cloud the input cloud 00177 * \param[out] max_vec the max value vector 00178 * \param[out] min_vec the min value vector 00179 */ 00180 void 00181 findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec); 00182 00183 /** \brief Recursively partition the point cloud, stopping when each grid contains less than sample_ points 00184 * Points are randomly sampled when a grid is found 00185 * \param cloud 00186 * \param first 00187 * \param last 00188 * \param min_values 00189 * \param max_values 00190 * \param indices 00191 * \param[out] outcloud output the resultant point cloud 00192 */ 00193 void 00194 partition (const PointCloud& cloud, const int first, const int last, 00195 const Vector min_values, const Vector max_values, 00196 std::vector<int>& indices, PointCloud& outcloud); 00197 00198 /** \brief Randomly sample the points in each grid. 00199 * \param[in] data 00200 * \param[in] first 00201 * \param[in] last 00202 * \param[out] indices 00203 * \param[out] output the resultant point cloud 00204 */ 00205 void 00206 samplePartition (const PointCloud& data, const int first, const int last, 00207 std::vector<int>& indices, PointCloud& outcloud); 00208 00209 /** \brief Returns the threshold for splitting in a given dimension. 00210 * \param[in] cloud the input cloud 00211 * \param[in] cut_dim the input dimension (0=x, 1=y, 2=z) 00212 * \param[in] cut_index the input index in the cloud 00213 */ 00214 float 00215 findCutVal (const PointCloud& cloud, const int cut_dim, const int cut_index); 00216 00217 /** \brief Computes the normal for points in a grid. This is a port from features to avoid features dependency for 00218 * filters 00219 * \param[in] cloud The input cloud 00220 * \param[out] normal the computed normal 00221 * \param[out] curvature the computed curvature 00222 */ 00223 void 00224 computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature); 00225 00226 /** \brief Computes the covariance matrix for points in the cloud. This is a port from features to avoid features dependency for 00227 * filters 00228 * \param[in] cloud The input cloud 00229 * \param[out] covariance_matrix the covariance matrix 00230 * \param[out] centroid the centroid 00231 */ 00232 unsigned int 00233 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00234 Eigen::Matrix3f &covariance_matrix, 00235 Eigen::Vector4f ¢roid); 00236 00237 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares 00238 * plane normal and surface curvature. 00239 * \param[in] covariance_matrix the 3x3 covariance matrix 00240 * \param[out] (nx ny nz) plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0) 00241 * \param[out] curvature the estimated surface curvature as a measure of 00242 */ 00243 void 00244 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 00245 float &nx, float &ny, float &nz, float &curvature); 00246 }; 00247 } 00248 00249 #ifdef PCL_NO_PRECOMPILE 00250 #include <pcl/filters/impl/sampling_surface_normal.hpp> 00251 #endif 00252 00253 #endif //#ifndef PCL_FILTERS_SAMPLING_SURFACE_NORMAL_H_