Point Cloud Library (PCL)
1.7.0
|
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