Point Cloud Library (PCL) 1.12.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
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 Indices& 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
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<Indices > 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, Indices (), 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 {
192 using Filter<NormalT>::input_;
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 */
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< Indices >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
217 {
218 filter_name_ = "NormalRefinement";
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< Indices >& 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< Indices >& 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
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
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< Indices > 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
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.