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