Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/sampling_surface_normal.h
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 &centroid);
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_