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