Point Cloud Library (PCL)  1.9.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 #pragma once
39 
40 #include <pcl/filters/filter.h>
41 
42 namespace pcl
43 {
44  /** \brief Assign weights of nearby normals used for refinement
45  * \todo Currently, this function equalizes all weights to 1
46  * @param cloud the point cloud data
47  * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
48  * @param k_indices indices of neighboring points
49  * @param k_sqr_distances squared distances to the neighboring points
50  * @return weights
51  * \ingroup filters
52  */
53  template <typename NormalT> inline std::vector<float>
55  int,
56  const std::vector<int>& k_indices,
57  const std::vector<float>& k_sqr_distances)
58  {
59  // Check inputs
60  if (k_indices.size () != k_sqr_distances.size ())
61  PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n");
62 
63  // TODO: For now we use uniform weights
64  return (std::vector<float> (k_indices.size (), 1.0f));
65  }
66 
67  /** \brief Refine an indexed point based on its neighbors, this function only writes to the normal_* fields
68  *
69  * \note If the indexed point has only NaNs in its neighborhood, the resulting normal will be zero.
70  *
71  * @param cloud the point cloud data
72  * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
73  * @param k_indices indices of neighboring points
74  * @param k_sqr_distances squared distances to the neighboring points
75  * @param point the output point, only normal_* fields are written
76  * @return false if an error occurred (norm of summed normals zero or all neighbors NaN)
77  * \ingroup filters
78  */
79  template <typename NormalT> inline bool
81  int index,
82  const std::vector<int>& k_indices,
83  const std::vector<float>& k_sqr_distances,
84  NormalT& point)
85  {
86  // Start by zeroing result
87  point.normal_x = 0.0f;
88  point.normal_y = 0.0f;
89  point.normal_z = 0.0f;
90 
91  // Check inputs
92  if (k_indices.size () != k_sqr_distances.size ())
93  {
94  PCL_ERROR("[pcl::refineNormal] inequal size of neighbor indices and distances!\n");
95  return (false);
96  }
97 
98  // Get weights
99  const std::vector<float> weights = assignNormalWeights (cloud, index, k_indices, k_sqr_distances);
100 
101  // Loop over all neighbors and accumulate sum of normal components
102  float nx = 0.0f;
103  float ny = 0.0f;
104  float nz = 0.0f;
105  for (size_t i = 0; i < k_indices.size (); ++i) {
106  // Neighbor
107  const NormalT& pointi = cloud[k_indices[i]];
108 
109  // Accumulate if not NaN
110  if (std::isfinite (pointi.normal_x) && std::isfinite (pointi.normal_y) && std::isfinite (pointi.normal_z))
111  {
112  const float& weighti = weights[i];
113  nx += weighti * pointi.normal_x;
114  ny += weighti * pointi.normal_y;
115  nz += weighti * pointi.normal_z;
116  }
117  }
118 
119  // Normalize if norm valid and non-zero
120  const float norm = std::sqrt (nx * nx + ny * ny + nz * nz);
121  if (std::isfinite (norm) && norm > std::numeric_limits<float>::epsilon ())
122  {
123  point.normal_x = nx / norm;
124  point.normal_y = ny / norm;
125  point.normal_z = nz / norm;
126 
127  return (true);
128  }
129 
130  return (false);
131  }
132 
133  /** \brief %Normal vector refinement class
134  *
135  * This class refines a set of already estimated normals by iteratively updating each normal to the (weighted)
136  * mean of all normals in its neighborhood. The intention is that you reuse the same point correspondences
137  * as used when estimating the original normals in order to avoid repeating a nearest neighbor search.
138  *
139  * \note This class avoids points for which a NaN is encountered in the neighborhood. In the special case
140  * where a point has only NaNs in its neighborhood, the resultant refined normal will be set to zero,
141  * i.e. this class only produces finite normals.
142  *
143  * \details Usage example:
144  *
145  * \code
146  * // Input point cloud
147  * pcl::PointCloud<PointT> cloud;
148  *
149  * // Fill cloud...
150  *
151  * // Estimated and refined normals
152  * pcl::PointCloud<NormalT> normals;
153  * pcl::PointCloud<NormalT> normals_refined;
154  *
155  * // Search parameters
156  * const int k = 5;
157  * std::vector<std::vector<int> > k_indices;
158  * std::vector<std::vector<float> > k_sqr_distances;
159  *
160  * // Run search
161  * pcl::search::KdTree<pcl::PointXYZRGB> search;
162  * search.setInputCloud (cloud.makeShared ());
163  * search.nearestKSearch (cloud, std::vector<int> (), k, k_indices, k_sqr_distances);
164  *
165  * // Use search results for normal estimation
166  * pcl::NormalEstimation<PointT, NormalT> ne;
167  * for (unsigned int i = 0; i < cloud.size (); ++i)
168  * {
169  * NormalT normal;
170  * ne.computePointNormal (cloud, k_indices[i]
171  * normal.normal_x, normal.normal_y, normal.normal_z, normal.curvature);
172  * pcl::flipNormalTowardsViewpoint (cloud[i], cloud.sensor_origin_[0], cloud.sensor_origin_[1], cloud.sensor_origin_[2],
173  * normal.normal_x, normal.normal_y, normal.normal_z);
174  * normals.push_back (normal);
175  * }
176  *
177  * // Run refinement using search results
178  * pcl::NormalRefinement<NormalT> nr (k_indices, k_sqr_distances);
179  * nr.setInputCloud (normals.makeShared ());
180  * nr.filter (normals_refined);
181  * \endcode
182  *
183  * \author Anders Glent Buch
184  * \ingroup filters
185  */
186  template<typename NormalT>
187  class NormalRefinement : public Filter<NormalT>
188  {
192 
193  using PointCloud = typename Filter<NormalT>::PointCloud;
194  using PointCloudPtr = typename PointCloud::Ptr;
195  using PointCloudConstPtr = typename PointCloud::ConstPtr;
196 
197  public:
198  /** \brief Empty constructor, sets default convergence parameters
199  */
201  Filter<NormalT>::Filter ()
202  {
203  filter_name_ = "NormalRefinement";
204  setMaxIterations (15);
205  setConvergenceThreshold (0.00001f);
206  }
207 
208  /** \brief Constructor for setting correspondences, sets default convergence parameters
209  * @param k_indices indices of neighboring points
210  * @param k_sqr_distances squared distances to the neighboring points
211  */
212  NormalRefinement (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
213  Filter<NormalT>::Filter ()
214  {
215  filter_name_ = "NormalRefinement";
216  setCorrespondences (k_indices, k_sqr_distances);
217  setMaxIterations (15);
218  setConvergenceThreshold (0.00001f);
219  }
220 
221  /** \brief Set correspondences calculated from nearest neighbor search
222  * @param k_indices indices of neighboring points
223  * @param k_sqr_distances squared distances to the neighboring points
224  */
225  inline void
226  setCorrespondences (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
227  {
228  k_indices_ = k_indices;
229  k_sqr_distances_ = k_sqr_distances;
230  }
231 
232  /** \brief Get correspondences (copy)
233  * @param k_indices indices of neighboring points
234  * @param k_sqr_distances squared distances to the neighboring points
235  */
236  inline void
237  getCorrespondences (std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
238  {
239  k_indices.assign (k_indices_.begin (), k_indices_.end ());
240  k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ());
241  }
242 
243  /** \brief Set maximum iterations
244  * @param max_iterations maximum iterations
245  */
246  inline void
247  setMaxIterations (unsigned int max_iterations)
248  {
249  max_iterations_ = max_iterations;
250  }
251 
252  /** \brief Get maximum iterations
253  * @return maximum iterations
254  */
255  inline unsigned int
257  {
258  return max_iterations_;
259  }
260 
261  /** \brief Set convergence threshold
262  * @param convergence_threshold convergence threshold
263  */
264  inline void
265  setConvergenceThreshold (float convergence_threshold)
266  {
267  convergence_threshold_ = convergence_threshold;
268  }
269 
270  /** \brief Get convergence threshold
271  * @return convergence threshold
272  */
273  inline float
275  {
276  return convergence_threshold_;
277  }
278 
279  protected:
280  /** \brief Filter a Point Cloud.
281  * \param output the resultant point cloud message
282  */
283  void
284  applyFilter (PointCloud &output) override;
285 
286  private:
287  /** \brief indices of neighboring points */
288  std::vector< std::vector<int> > k_indices_;
289 
290  /** \brief squared distances to the neighboring points */
291  std::vector< std::vector<float> > k_sqr_distances_;
292 
293  /** \brief Maximum allowable iterations over the whole point cloud for refinement */
294  unsigned int max_iterations_;
295 
296  /** \brief Convergence threshold in the interval [0,1] on the mean of 1 - dot products between previous iteration and the current */
297  float convergence_threshold_;
298  };
299 }
300 
301 #ifdef PCL_NO_PRECOMPILE
302 #include <pcl/filters/impl/normal_refinement.hpp>
303 #else
304 #define PCL_INSTANTIATE_NormalRefinement(T) template class PCL_EXPORTS pcl::NormalRefinement<T>;
305 #endif
A point structure representing normal coordinates and the surface curvature estimate.
void applyFilter(PointCloud &output) override
Filter a Point Cloud.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:83
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)
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
void setMaxIterations(unsigned int max_iterations)
Set maximum iterations.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
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:164
void setConvergenceThreshold(float convergence_threshold)
Set convergence threshold.
unsigned int getMaxIterations()
Get maximum iterations.