Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/normal_refinement.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder(s) nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id$
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_NORMAL_REFINEMENT_H_
00039 #define PCL_FILTERS_NORMAL_REFINEMENT_H_
00040 
00041 #include <pcl/filters/filter.h>
00042 
00043 namespace pcl
00044 {
00045   /** \brief Assign weights of nearby normals used for refinement
00046    * \todo Currently, this function equalizes all weights to 1
00047    * @param cloud the point cloud data
00048    * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
00049    * @param k_indices indices of neighboring points
00050    * @param k_sqr_distances squared distances to the neighboring points
00051    * @return weights
00052    * \ingroup filters
00053    */
00054   template <typename NormalT> inline std::vector<float>
00055   assignNormalWeights (const PointCloud<NormalT>&,
00056                        int,
00057                        const std::vector<int>& k_indices,
00058                        const std::vector<float>& k_sqr_distances)
00059   {
00060     // Check inputs
00061     if (k_indices.size () != k_sqr_distances.size ())
00062       PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n");
00063     
00064     // TODO: For now we use uniform weights
00065     return (std::vector<float> (k_indices.size (), 1.0f));
00066   }
00067   
00068   /** \brief Refine an indexed point based on its neighbors, this function only writes to the normal_* fields
00069    * 
00070    * \note If the indexed point has only NaNs in its neighborhood, the resulting normal will be zero.
00071    * 
00072    * @param cloud the point cloud data
00073    * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
00074    * @param k_indices indices of neighboring points
00075    * @param k_sqr_distances squared distances to the neighboring points
00076    * @param point the output point, only normal_* fields are written
00077    * @return false if an error occurred (norm of summed normals zero or all neighbors NaN)
00078    * \ingroup filters
00079    */
00080   template <typename NormalT> inline bool
00081   refineNormal (const PointCloud<NormalT>& cloud,
00082                 int index,
00083                 const std::vector<int>& k_indices,
00084                 const std::vector<float>& k_sqr_distances,
00085                 NormalT& point)
00086   {
00087     // Start by zeroing result
00088     point.normal_x = 0.0f;
00089     point.normal_y = 0.0f;
00090     point.normal_z = 0.0f;
00091     
00092     // Check inputs
00093     if (k_indices.size () != k_sqr_distances.size ())
00094     {
00095       PCL_ERROR("[pcl::refineNormal] inequal size of neighbor indices and distances!\n");
00096       return (false);
00097     }
00098     
00099     // Get weights
00100     const std::vector<float> weights = assignNormalWeights (cloud, index, k_indices, k_sqr_distances);
00101     
00102     // Loop over all neighbors and accumulate sum of normal components
00103     float nx = 0.0f;
00104     float ny = 0.0f;
00105     float nz = 0.0f;
00106     for (unsigned int i = 0; i < k_indices.size (); ++i) {
00107       // Neighbor
00108       const NormalT& pointi = cloud[k_indices[i]];
00109       
00110       // Accumulate if not NaN
00111       if (pcl_isfinite (pointi.normal_x) && pcl_isfinite (pointi.normal_y) && pcl_isfinite (pointi.normal_z))
00112       {
00113         const float& weighti = weights[i];
00114         nx += weighti * pointi.normal_x;
00115         ny += weighti * pointi.normal_y;
00116         nz += weighti * pointi.normal_z;
00117       }
00118     }
00119     
00120     // Normalize if norm valid and non-zero
00121     const float norm = sqrtf (nx * nx + ny * ny + nz * nz);
00122     if (pcl_isfinite (norm) && norm > std::numeric_limits<float>::epsilon ())
00123     {
00124       point.normal_x = nx / norm;
00125       point.normal_y = ny / norm;
00126       point.normal_z = nz / norm;
00127 
00128       return (true);
00129     }
00130 
00131     return (false);
00132   }
00133   
00134   /** \brief %Normal vector refinement class 
00135     *
00136     * This class refines a set of already estimated normals by iteratively updating each normal to the (weighted)
00137     * mean of all normals in its neighborhood. The intention is that you reuse the same point correspondences
00138     * as used when estimating the original normals in order to avoid repeating a nearest neighbor search.
00139     * 
00140     * \note This class avoids points for which a NaN is encountered in the neighborhood. In the special case
00141     * where a point has only NaNs in its neighborhood, the resultant refined normal will be set to zero,
00142     * i.e. this class only produces finite normals.
00143     * 
00144     * \details Usage example:
00145     * 
00146     * \code
00147     * // Input point cloud
00148     * pcl::PointCloud<PointT> cloud;
00149     * 
00150     * // Fill cloud...
00151     * 
00152     * // Estimated and refined normals
00153     * pcl::PointCloud<NormalT> normals;
00154     * pcl::PointCloud<NormalT> normals_refined;
00155     * 
00156     * // Search parameters
00157     * const int k = 5;
00158     * std::vector<std::vector<int> > k_indices;
00159     * std::vector<std::vector<float> > k_sqr_distances;
00160     * 
00161     * // Run search
00162     * pcl::search::KdTree<pcl::PointXYZRGB> search;
00163     * search.setInputCloud (cloud.makeShared ());
00164     * search.nearestKSearch (cloud, std::vector<int> (), k, k_indices, k_sqr_distances);
00165     * 
00166     * // Use search results for normal estimation
00167     * pcl::NormalEstimation<PointT, NormalT> ne;
00168     * for (unsigned int i = 0; i < cloud.size (); ++i)
00169     * {
00170     *   NormalT normal;
00171     *   ne.computePointNormal (cloud, k_indices[i]
00172     *                          normal.normal_x, normal.normal_y, normal.normal_z, normal.curvature);
00173     *   pcl::flipNormalTowardsViewpoint (cloud[i], cloud.sensor_origin_[0], cloud.sensor_origin_[1], cloud.sensor_origin_[2],
00174     *                                    normal.normal_x, normal.normal_y, normal.normal_z);
00175     *   normals.push_back (normal);
00176     * }
00177     * 
00178     * // Run refinement using search results
00179     * pcl::NormalRefinement<NormalT> nr (k_indices, k_sqr_distances);
00180     * nr.setInputCloud (normals.makeShared ());
00181     * nr.filter (normals_refined);
00182     * \endcode
00183     *
00184     * \author Anders Glent Buch
00185     * \ingroup filters
00186     */
00187   template<typename NormalT>
00188   class NormalRefinement : public Filter<NormalT>
00189   {
00190     using Filter<NormalT>::input_;
00191     using Filter<NormalT>::filter_name_;
00192     using Filter<NormalT>::getClassName;
00193 
00194     typedef typename Filter<NormalT>::PointCloud PointCloud;
00195     typedef typename PointCloud::Ptr PointCloudPtr;
00196     typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00197 
00198     public:
00199       /** \brief Empty constructor, sets default convergence parameters
00200         */
00201       NormalRefinement () :
00202         Filter<NormalT>::Filter ()
00203       {
00204         filter_name_ = "NormalRefinement";
00205         setMaxIterations (15);
00206         setConvergenceThreshold (0.00001f);
00207       }
00208       
00209       /** \brief Constructor for setting correspondences, sets default convergence parameters
00210        * @param k_indices indices of neighboring points
00211        * @param k_sqr_distances squared distances to the neighboring points
00212        */
00213       NormalRefinement (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
00214         Filter<NormalT>::Filter ()
00215       {
00216         filter_name_ = "NormalRefinement";
00217         setCorrespondences (k_indices, k_sqr_distances);
00218         setMaxIterations (15);
00219         setConvergenceThreshold (0.00001f);
00220       }
00221       
00222       /** \brief Set correspondences calculated from nearest neighbor search
00223        * @param k_indices indices of neighboring points
00224        * @param k_sqr_distances squared distances to the neighboring points
00225        */
00226       inline void
00227       setCorrespondences (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
00228       {
00229         k_indices_ = k_indices;
00230         k_sqr_distances_ = k_sqr_distances;
00231       }
00232       
00233       /** \brief Get correspondences (copy)
00234        * @param k_indices indices of neighboring points
00235        * @param k_sqr_distances squared distances to the neighboring points
00236        */
00237       inline void
00238       getCorrespondences (std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
00239       {
00240         k_indices.assign (k_indices_.begin (), k_indices_.end ());
00241         k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ());
00242       }
00243       
00244       /** \brief Set maximum iterations
00245        * @param max_iterations maximum iterations
00246        */
00247       inline void
00248       setMaxIterations (unsigned int max_iterations)
00249       {
00250         max_iterations_ = max_iterations;
00251       }
00252       
00253       /** \brief Get maximum iterations
00254        * @return maximum iterations
00255        */
00256       inline unsigned int
00257       getMaxIterations ()
00258       {
00259         return max_iterations_;
00260       }
00261       
00262       /** \brief Set convergence threshold
00263        * @param convergence_threshold convergence threshold
00264        */
00265       inline void
00266       setConvergenceThreshold (float convergence_threshold)
00267       {
00268         convergence_threshold_ = convergence_threshold;
00269       }
00270 
00271       /** \brief Get convergence threshold
00272        * @return convergence threshold
00273        */
00274       inline float
00275       getConvergenceThreshold ()
00276       {
00277         return convergence_threshold_;
00278       }
00279 
00280     protected:
00281       /** \brief Filter a Point Cloud.
00282         * \param output the resultant point cloud message
00283         */
00284       void
00285       applyFilter (PointCloud &output);
00286       
00287     private:
00288       /** \brief indices of neighboring points */
00289       std::vector< std::vector<int> > k_indices_;
00290       
00291       /** \brief squared distances to the neighboring points */
00292       std::vector< std::vector<float> > k_sqr_distances_;
00293       
00294       /** \brief Maximum allowable iterations over the whole point cloud for refinement */
00295       unsigned int max_iterations_;
00296       
00297       /** \brief Convergence threshold in the interval [0,1] on the mean of 1 - dot products between previous iteration and the current */
00298       float convergence_threshold_;
00299   };
00300 }
00301 
00302 #ifdef PCL_NO_PRECOMPILE
00303 #include <pcl/filters/impl/normal_refinement.hpp>
00304 #else
00305 #define PCL_INSTANTIATE_NormalRefinement(T) template class PCL_EXPORTS pcl::NormalRefinement<T>;
00306 #endif
00307 
00308 #endif