Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
sampling_surface_normal.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
40#include <pcl/filters/filter.h>
41#include <ctime>
42
43namespace pcl
44{
45 /** \brief @b SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N points,
46 * and samples points randomly within each grid. Normal is computed using the N points of each grid. All points
47 * sampled within a grid are assigned the same normal.
48 *
49 * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher)
50 * \ingroup filters
51 */
52 template<typename PointT>
53 class SamplingSurfaceNormal: public Filter<PointT>
54 {
59
60 using PointCloud = typename Filter<PointT>::PointCloud;
61 using PointCloudPtr = typename PointCloud::Ptr;
62 using PointCloudConstPtr = typename PointCloud::ConstPtr;
63
64 using Vector = Eigen::Matrix<float, 3, 1>;
65
66 public:
67
68 using Ptr = shared_ptr<SamplingSurfaceNormal<PointT> >;
69 using ConstPtr = shared_ptr<const SamplingSurfaceNormal<PointT> >;
70
71 /** \brief Empty constructor. */
73 {
74 filter_name_ = "SamplingSurfaceNormal";
75 srand (seed_);
76 }
77
78 /** \brief Set maximum number of samples in each grid
79 * \param[in] sample maximum number of samples in each grid
80 */
81 inline void
82 setSample (unsigned int sample)
83 {
84 sample_ = sample;
85 }
86
87 /** \brief Get the value of the internal \a sample parameter. */
88 inline unsigned int
89 getSample () const
90 {
91 return (sample_);
92 }
93
94 /** \brief Set seed of random function.
95 * \param[in] seed the input seed
96 */
97 inline void
98 setSeed (unsigned int seed)
99 {
100 seed_ = seed;
101 srand (seed_);
102 }
103
104 /** \brief Get the value of the internal \a seed parameter. */
105 inline unsigned int
106 getSeed () const
107 {
108 return (seed_);
109 }
110
111 /** \brief Set ratio of points to be sampled in each grid
112 * \param[in] ratio sample the ratio of points to be sampled in each grid
113 */
114 inline void
115 setRatio (float ratio)
116 {
117 ratio_ = ratio;
118 }
119
120 /** \brief Get the value of the internal \a ratio parameter. */
121 inline float
122 getRatio () const
123 {
124 return ratio_;
125 }
126
127 protected:
128
129 /** \brief Maximum number of samples in each grid. */
130 unsigned int sample_{10};
131 /** \brief Random number seed. */
132 unsigned int seed_{static_cast<unsigned int> (time (nullptr))};
133 /** \brief Ratio of points to be sampled in each grid */
134 float ratio_{0.0f};
135
136 /** \brief Sample of point indices into a separate PointCloud
137 * \param[out] output the resultant point cloud
138 */
139 void
140 applyFilter (PointCloud &output) override;
141
142 private:
143
144 /** \brief @b CompareDim is a comparator object for sorting across a specific dimension (i,.e X, Y or Z)
145 */
146 struct CompareDim
147 {
148 /** \brief The dimension to sort */
149 const int dim;
150 /** \brief The input point cloud to sort */
151 const pcl::PointCloud <PointT>& cloud;
152
153 /** \brief Constructor. */
154 CompareDim (const int dim, const pcl::PointCloud <PointT>& cloud) : dim (dim), cloud (cloud)
155 {
156 }
157
158 /** \brief The operator function for sorting. */
159 bool
160 operator () (const int& p0, const int& p1)
161 {
162 if (dim == 0)
163 return (cloud[p0].x < cloud[p1].x);
164 if (dim == 1)
165 return (cloud[p0].y < cloud[p1].y);
166 if (dim == 2)
167 return (cloud[p0].z < cloud[p1].z);
168 return (false);
169 }
170 };
171
172 /** \brief Finds the max and min values in each dimension
173 * \param[in] cloud the input cloud
174 * \param[out] max_vec the max value vector
175 * \param[out] min_vec the min value vector
176 */
177 void
178 findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec);
179
180 /** \brief Recursively partition the point cloud, stopping when each grid contains less than sample_ points
181 * Points are randomly sampled when a grid is found
182 * \param cloud
183 * \param first
184 * \param last
185 * \param min_values
186 * \param max_values
187 * \param indices
188 * \param[out] outcloud output the resultant point cloud
189 */
190 void
191 partition (const PointCloud& cloud, const int first, const int last,
192 const Vector min_values, const Vector max_values,
193 Indices& indices, PointCloud& outcloud);
194
195 /** \brief Randomly sample the points in each grid.
196 * \param[in] data
197 * \param[in] first
198 * \param[in] last
199 * \param[out] indices
200 * \param[out] output the resultant point cloud
201 */
202 void
203 samplePartition (const PointCloud& data, const int first, const int last,
204 Indices& indices, PointCloud& outcloud);
205
206 /** \brief Returns the threshold for splitting in a given dimension.
207 * \param[in] cloud the input cloud
208 * \param[in] cut_dim the input dimension (0=x, 1=y, 2=z)
209 * \param[in] cut_index the input index in the cloud
210 */
211 float
212 findCutVal (const PointCloud& cloud, const int cut_dim, const int cut_index);
213
214 /** \brief Computes the normal for points in a grid. This is a port from features to avoid features dependency for
215 * filters
216 * \param[in] cloud The input cloud
217 * \param[out] normal the computed normal
218 * \param[out] curvature the computed curvature
219 */
220 void
221 computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature);
222
223 /** \brief Computes the covariance matrix for points in the cloud. This is a port from features to avoid features dependency for
224 * filters
225 * \param[in] cloud The input cloud
226 * \param[out] covariance_matrix the covariance matrix
227 * \param[out] centroid the centroid
228 */
229 unsigned int
230 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
231 Eigen::Matrix3f &covariance_matrix,
232 Eigen::Vector4f &centroid);
233
234 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
235 * plane normal and surface curvature.
236 * \param[in] covariance_matrix the 3x3 covariance matrix
237 * \param[out] (nx ny nz) plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0)
238 * \param[out] curvature the estimated surface curvature as a measure of
239 */
240 void
241 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
242 float &nx, float &ny, float &nz, float &curvature);
243 };
244}
245
246#ifdef PCL_NO_PRECOMPILE
247#include <pcl/filters/impl/sampling_surface_normal.hpp>
248#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
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N poin...
void setSample(unsigned int sample)
Set maximum number of samples in each grid.
shared_ptr< const SamplingSurfaceNormal< PointT > > ConstPtr
void setSeed(unsigned int seed)
Set seed of random function.
unsigned int getSample() const
Get the value of the internal sample parameter.
void setRatio(float ratio)
Set ratio of points to be sampled in each grid.
float getRatio() const
Get the value of the internal ratio parameter.
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
SamplingSurfaceNormal()
Empty constructor.
float ratio_
Ratio of points to be sampled in each grid.
unsigned int getSeed() const
Get the value of the internal seed parameter.
unsigned int sample_
Maximum number of samples in each grid.
shared_ptr< SamplingSurfaceNormal< PointT > > Ptr
unsigned int seed_
Random number seed.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.