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