Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
44namespace 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 {
196 using Filter<NormalT>::input_;
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 */
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) :
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
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition filter.h:174
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.
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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.