Point Cloud Library (PCL)  1.8.1-dev
normal_refinement.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_FILTERS_NORMAL_REFINEMENT_H_
39 #define PCL_FILTERS_NORMAL_REFINEMENT_H_
40 
41 #include <pcl/filters/filter.h>
42 
43 namespace pcl
44 {
45  /** \brief Assign weights of nearby normals used for refinement
46  * \todo Currently, this function equalizes all weights to 1
47  * @param cloud the point cloud data
48  * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
49  * @param k_indices indices of neighboring points
50  * @param k_sqr_distances squared distances to the neighboring points
51  * @return weights
52  * \ingroup filters
53  */
54  template <typename NormalT> inline std::vector<float>
56  int,
57  const std::vector<int>& k_indices,
58  const std::vector<float>& k_sqr_distances)
59  {
60  // Check inputs
61  if (k_indices.size () != k_sqr_distances.size ())
62  PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n");
63 
64  // TODO: For now we use uniform weights
65  return (std::vector<float> (k_indices.size (), 1.0f));
66  }
67 
68  /** \brief Refine an indexed point based on its neighbors, this function only writes to the normal_* fields
69  *
70  * \note If the indexed point has only NaNs in its neighborhood, the resulting normal will be zero.
71  *
72  * @param cloud the point cloud data
73  * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
74  * @param k_indices indices of neighboring points
75  * @param k_sqr_distances squared distances to the neighboring points
76  * @param point the output point, only normal_* fields are written
77  * @return false if an error occurred (norm of summed normals zero or all neighbors NaN)
78  * \ingroup filters
79  */
80  template <typename NormalT> inline bool
82  int index,
83  const std::vector<int>& k_indices,
84  const std::vector<float>& k_sqr_distances,
85  NormalT& point)
86  {
87  // Start by zeroing result
88  point.normal_x = 0.0f;
89  point.normal_y = 0.0f;
90  point.normal_z = 0.0f;
91 
92  // Check inputs
93  if (k_indices.size () != k_sqr_distances.size ())
94  {
95  PCL_ERROR("[pcl::refineNormal] inequal size of neighbor indices and distances!\n");
96  return (false);
97  }
98 
99  // Get weights
100  const std::vector<float> weights = assignNormalWeights (cloud, index, k_indices, k_sqr_distances);
101 
102  // Loop over all neighbors and accumulate sum of normal components
103  float nx = 0.0f;
104  float ny = 0.0f;
105  float nz = 0.0f;
106  for (unsigned int i = 0; i < k_indices.size (); ++i) {
107  // Neighbor
108  const NormalT& pointi = cloud[k_indices[i]];
109 
110  // Accumulate if not NaN
111  if (pcl_isfinite (pointi.normal_x) && pcl_isfinite (pointi.normal_y) && pcl_isfinite (pointi.normal_z))
112  {
113  const float& weighti = weights[i];
114  nx += weighti * pointi.normal_x;
115  ny += weighti * pointi.normal_y;
116  nz += weighti * pointi.normal_z;
117  }
118  }
119 
120  // Normalize if norm valid and non-zero
121  const float norm = std::sqrt (nx * nx + ny * ny + nz * nz);
122  if (pcl_isfinite (norm) && norm > std::numeric_limits<float>::epsilon ())
123  {
124  point.normal_x = nx / norm;
125  point.normal_y = ny / norm;
126  point.normal_z = nz / norm;
127 
128  return (true);
129  }
130 
131  return (false);
132  }
133 
134  /** \brief %Normal vector refinement class
135  *
136  * This class refines a set of already estimated normals by iteratively updating each normal to the (weighted)
137  * mean of all normals in its neighborhood. The intention is that you reuse the same point correspondences
138  * as used when estimating the original normals in order to avoid repeating a nearest neighbor search.
139  *
140  * \note This class avoids points for which a NaN is encountered in the neighborhood. In the special case
141  * where a point has only NaNs in its neighborhood, the resultant refined normal will be set to zero,
142  * i.e. this class only produces finite normals.
143  *
144  * \details Usage example:
145  *
146  * \code
147  * // Input point cloud
148  * pcl::PointCloud<PointT> cloud;
149  *
150  * // Fill cloud...
151  *
152  * // Estimated and refined normals
153  * pcl::PointCloud<NormalT> normals;
154  * pcl::PointCloud<NormalT> normals_refined;
155  *
156  * // Search parameters
157  * const int k = 5;
158  * std::vector<std::vector<int> > k_indices;
159  * std::vector<std::vector<float> > k_sqr_distances;
160  *
161  * // Run search
162  * pcl::search::KdTree<pcl::PointXYZRGB> search;
163  * search.setInputCloud (cloud.makeShared ());
164  * search.nearestKSearch (cloud, std::vector<int> (), k, k_indices, k_sqr_distances);
165  *
166  * // Use search results for normal estimation
167  * pcl::NormalEstimation<PointT, NormalT> ne;
168  * for (unsigned int i = 0; i < cloud.size (); ++i)
169  * {
170  * NormalT normal;
171  * ne.computePointNormal (cloud, k_indices[i]
172  * normal.normal_x, normal.normal_y, normal.normal_z, normal.curvature);
173  * pcl::flipNormalTowardsViewpoint (cloud[i], cloud.sensor_origin_[0], cloud.sensor_origin_[1], cloud.sensor_origin_[2],
174  * normal.normal_x, normal.normal_y, normal.normal_z);
175  * normals.push_back (normal);
176  * }
177  *
178  * // Run refinement using search results
179  * pcl::NormalRefinement<NormalT> nr (k_indices, k_sqr_distances);
180  * nr.setInputCloud (normals.makeShared ());
181  * nr.filter (normals_refined);
182  * \endcode
183  *
184  * \author Anders Glent Buch
185  * \ingroup filters
186  */
187  template<typename NormalT>
188  class NormalRefinement : public Filter<NormalT>
189  {
193 
194  typedef typename Filter<NormalT>::PointCloud PointCloud;
195  typedef typename PointCloud::Ptr PointCloudPtr;
196  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
197 
198  public:
199  /** \brief Empty constructor, sets default convergence parameters
200  */
202  Filter<NormalT>::Filter ()
203  {
204  filter_name_ = "NormalRefinement";
205  setMaxIterations (15);
206  setConvergenceThreshold (0.00001f);
207  }
208 
209  /** \brief Constructor for setting correspondences, sets default convergence parameters
210  * @param k_indices indices of neighboring points
211  * @param k_sqr_distances squared distances to the neighboring points
212  */
213  NormalRefinement (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
214  Filter<NormalT>::Filter ()
215  {
216  filter_name_ = "NormalRefinement";
217  setCorrespondences (k_indices, k_sqr_distances);
218  setMaxIterations (15);
219  setConvergenceThreshold (0.00001f);
220  }
221 
222  /** \brief Set correspondences calculated from nearest neighbor search
223  * @param k_indices indices of neighboring points
224  * @param k_sqr_distances squared distances to the neighboring points
225  */
226  inline void
227  setCorrespondences (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
228  {
229  k_indices_ = k_indices;
230  k_sqr_distances_ = k_sqr_distances;
231  }
232 
233  /** \brief Get correspondences (copy)
234  * @param k_indices indices of neighboring points
235  * @param k_sqr_distances squared distances to the neighboring points
236  */
237  inline void
238  getCorrespondences (std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
239  {
240  k_indices.assign (k_indices_.begin (), k_indices_.end ());
241  k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ());
242  }
243 
244  /** \brief Set maximum iterations
245  * @param max_iterations maximum iterations
246  */
247  inline void
248  setMaxIterations (unsigned int max_iterations)
249  {
250  max_iterations_ = max_iterations;
251  }
252 
253  /** \brief Get maximum iterations
254  * @return maximum iterations
255  */
256  inline unsigned int
258  {
259  return max_iterations_;
260  }
261 
262  /** \brief Set convergence threshold
263  * @param convergence_threshold convergence threshold
264  */
265  inline void
266  setConvergenceThreshold (float convergence_threshold)
267  {
268  convergence_threshold_ = convergence_threshold;
269  }
270 
271  /** \brief Get convergence threshold
272  * @return convergence threshold
273  */
274  inline float
276  {
277  return convergence_threshold_;
278  }
279 
280  protected:
281  /** \brief Filter a Point Cloud.
282  * \param output the resultant point cloud message
283  */
284  void
285  applyFilter (PointCloud &output);
286 
287  private:
288  /** \brief indices of neighboring points */
289  std::vector< std::vector<int> > k_indices_;
290 
291  /** \brief squared distances to the neighboring points */
292  std::vector< std::vector<float> > k_sqr_distances_;
293 
294  /** \brief Maximum allowable iterations over the whole point cloud for refinement */
295  unsigned int max_iterations_;
296 
297  /** \brief Convergence threshold in the interval [0,1] on the mean of 1 - dot products between previous iteration and the current */
298  float convergence_threshold_;
299  };
300 }
301 
302 #ifdef PCL_NO_PRECOMPILE
303 #include <pcl/filters/impl/normal_refinement.hpp>
304 #else
305 #define PCL_INSTANTIATE_NormalRefinement(T) template class PCL_EXPORTS pcl::NormalRefinement<T>;
306 #endif
307 
308 #endif
A point structure representing normal coordinates and the surface curvature estimate.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void applyFilter(PointCloud &output)
Filter a Point Cloud.
NormalRefinement()
Empty constructor, sets default convergence parameters.
std::vector< float > assignNormalWeights(const PointCloud< NormalT > &, int, const std::vector< int > &k_indices, const std::vector< float > &k_sqr_distances)
Assign weights of nearby normals used for refinement.
float getConvergenceThreshold()
Get convergence threshold.
Filter represents the base filter class.
Definition: filter.h:84
Normal vector refinement class
NormalRefinement(const std::vector< std::vector< int > > &k_indices, const std::vector< std::vector< float > > &k_sqr_distances)
Constructor for setting correspondences, sets default convergence parameters.
bool refineNormal(const PointCloud< NormalT > &cloud, int index, const std::vector< int > &k_indices, const std::vector< float > &k_sqr_distances, NormalT &point)
Refine an indexed point based on its neighbors, this function only writes to the normal_* fields...
void getCorrespondences(std::vector< std::vector< int > > &k_indices, std::vector< std::vector< float > > &k_sqr_distances)
Get correspondences (copy)
void setMaxIterations(unsigned int max_iterations)
Set maximum iterations.
void setCorrespondences(const std::vector< std::vector< int > > &k_indices, const std::vector< std::vector< float > > &k_sqr_distances)
Set correspondences calculated from nearest neighbor search.
std::string filter_name_
The filter name.
Definition: filter.h:166
void setConvergenceThreshold(float convergence_threshold)
Set convergence threshold.
unsigned int getMaxIterations()
Get maximum iterations.