Point Cloud Library (PCL) 1.12.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, Eigen::Dynamic, 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 sample_ (10), seed_ (static_cast<unsigned int> (time (nullptr))), ratio_ ()
74 {
75 filter_name_ = "SamplingSurfaceNormal";
76 srand (seed_);
77 }
78
79 /** \brief Set maximum number of samples in each grid
80 * \param[in] sample maximum number of samples in each grid
81 */
82 inline void
83 setSample (unsigned int sample)
84 {
85 sample_ = sample;
86 }
87
88 /** \brief Get the value of the internal \a sample parameter. */
89 inline unsigned int
90 getSample () const
91 {
92 return (sample_);
93 }
94
95 /** \brief Set seed of random function.
96 * \param[in] seed the input seed
97 */
98 inline void
99 setSeed (unsigned int seed)
100 {
101 seed_ = seed;
102 srand (seed_);
103 }
104
105 /** \brief Get the value of the internal \a seed parameter. */
106 inline unsigned int
107 getSeed () const
108 {
109 return (seed_);
110 }
111
112 /** \brief Set ratio of points to be sampled in each grid
113 * \param[in] ratio sample the ratio of points to be sampled in each grid
114 */
115 inline void
116 setRatio (float ratio)
117 {
118 ratio_ = ratio;
119 }
120
121 /** \brief Get the value of the internal \a ratio parameter. */
122 inline float
123 getRatio () const
124 {
125 return ratio_;
126 }
127
128 protected:
129
130 /** \brief Maximum number of samples in each grid. */
131 unsigned int sample_;
132 /** \brief Random number seed. */
133 unsigned int seed_;
134 /** \brief Ratio of points to be sampled in each grid */
135 float ratio_;
136
137 /** \brief Sample of point indices into a separate PointCloud
138 * \param[out] output the resultant point cloud
139 */
140 void
141 applyFilter (PointCloud &output) override;
142
143 private:
144
145 /** \brief @b CompareDim is a comparator object for sorting across a specific dimension (i,.e X, Y or Z)
146 */
147 struct CompareDim
148 {
149 /** \brief The dimension to sort */
150 const int dim;
151 /** \brief The input point cloud to sort */
152 const pcl::PointCloud <PointT>& cloud;
153
154 /** \brief Constructor. */
155 CompareDim (const int dim, const pcl::PointCloud <PointT>& cloud) : dim (dim), cloud (cloud)
156 {
157 }
158
159 /** \brief The operator function for sorting. */
160 bool
161 operator () (const int& p0, const int& p1)
162 {
163 if (dim == 0)
164 return (cloud[p0].x < cloud[p1].x);
165 if (dim == 1)
166 return (cloud[p0].y < cloud[p1].y);
167 if (dim == 2)
168 return (cloud[p0].z < cloud[p1].z);
169 return (false);
170 }
171 };
172
173 /** \brief Finds the max and min values in each dimension
174 * \param[in] cloud the input cloud
175 * \param[out] max_vec the max value vector
176 * \param[out] min_vec the min value vector
177 */
178 void
179 findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec);
180
181 /** \brief Recursively partition the point cloud, stopping when each grid contains less than sample_ points
182 * Points are randomly sampled when a grid is found
183 * \param cloud
184 * \param first
185 * \param last
186 * \param min_values
187 * \param max_values
188 * \param indices
189 * \param[out] outcloud output the resultant point cloud
190 */
191 void
192 partition (const PointCloud& cloud, const int first, const int last,
193 const Vector min_values, const Vector max_values,
194 Indices& indices, PointCloud& outcloud);
195
196 /** \brief Randomly sample the points in each grid.
197 * \param[in] data
198 * \param[in] first
199 * \param[in] last
200 * \param[out] indices
201 * \param[out] output the resultant point cloud
202 */
203 void
204 samplePartition (const PointCloud& data, const int first, const int last,
205 Indices& indices, PointCloud& outcloud);
206
207 /** \brief Returns the threshold for splitting in a given dimension.
208 * \param[in] cloud the input cloud
209 * \param[in] cut_dim the input dimension (0=x, 1=y, 2=z)
210 * \param[in] cut_index the input index in the cloud
211 */
212 float
213 findCutVal (const PointCloud& cloud, const int cut_dim, const int cut_index);
214
215 /** \brief Computes the normal for points in a grid. This is a port from features to avoid features dependency for
216 * filters
217 * \param[in] cloud The input cloud
218 * \param[out] normal the computed normal
219 * \param[out] curvature the computed curvature
220 */
221 void
222 computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature);
223
224 /** \brief Computes the covariance matrix for points in the cloud. This is a port from features to avoid features dependency for
225 * filters
226 * \param[in] cloud The input cloud
227 * \param[out] covariance_matrix the covariance matrix
228 * \param[out] centroid the centroid
229 */
230 unsigned int
231 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
232 Eigen::Matrix3f &covariance_matrix,
233 Eigen::Vector4f &centroid);
234
235 /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
236 * plane normal and surface curvature.
237 * \param[in] covariance_matrix the 3x3 covariance matrix
238 * \param[out] (nx ny nz) plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0)
239 * \param[out] curvature the estimated surface curvature as a measure of
240 */
241 void
242 solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
243 float &nx, float &ny, float &nz, float &curvature);
244 };
245}
246
247#ifdef PCL_NO_PRECOMPILE
248#include <pcl/filters/impl/sampling_surface_normal.hpp>
249#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.