Point Cloud Library (PCL) 1.13.0
rsd.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_FEATURES_IMPL_RSD_H_
42#define PCL_FEATURES_IMPL_RSD_H_
43
44#include <limits>
45#include <pcl/features/rsd.h>
46
47//////////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
50 const pcl::Indices &indices, double max_dist,
51 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
52{
53 // Check if the full histogram has to be saved or not
54 Eigen::MatrixXf histogram;
55 if (compute_histogram)
56 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
57
58 // Check if enough points are provided or not
59 if (indices.size () < 2)
60 {
61 radii.r_max = 0;
62 radii.r_min = 0;
63 return histogram;
64 }
65
66 // Initialize minimum and maximum angle values in each distance bin
67 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
68 for (auto& minmax: min_max_angle_by_dist)
69 {
70 minmax.resize (2);
71 minmax[0] = std::numeric_limits<double>::max();
72 minmax[1] = -std::numeric_limits<double>::max();
73 }
74 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
75
76 // Compute distance by normal angle distribution for points
77 pcl::Indices::const_iterator i, begin (indices.begin()), end (indices.end());
78 for (i = begin+1; i != end; ++i)
79 {
80 // compute angle between the two lines going through normals (disregard orientation!)
81 double cosine = normals[*i].normal[0] * normals[*begin].normal[0] +
82 normals[*i].normal[1] * normals[*begin].normal[1] +
83 normals[*i].normal[2] * normals[*begin].normal[2];
84 if (cosine > 1) cosine = 1;
85 if (cosine < -1) cosine = -1;
86 double angle = std::acos (cosine);
87 if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
88
89 // Compute point to point distance
90 double dist = sqrt ((surface[*i].x - surface[*begin].x) * (surface[*i].x - surface[*begin].x) +
91 (surface[*i].y - surface[*begin].y) * (surface[*i].y - surface[*begin].y) +
92 (surface[*i].z - surface[*begin].z) * (surface[*i].z - surface[*begin].z));
93
94 if (dist > max_dist)
95 continue; /// \note: we neglect points that are outside the specified interval!
96
97 // compute bins and increase
98 int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
99 if (compute_histogram)
100 {
101 int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
102 histogram(bin_a, bin_d)++;
103 }
104
105 // update min-max values for distance bins
106 min_max_angle_by_dist[bin_d][0] = std::min(angle, min_max_angle_by_dist[bin_d][0]);
107 min_max_angle_by_dist[bin_d][1] = std::max(angle, min_max_angle_by_dist[bin_d][1]);
108 }
109
110 // Estimate radius from min and max lines
111 double Amint_Amin = 0, Amint_d = 0;
112 double Amaxt_Amax = 0, Amaxt_d = 0;
113 for (int di=0; di<nr_subdiv; di++)
114 {
115 // combute the members of A'*A*r = A'*D
116 if (min_max_angle_by_dist[di][1] >= 0)
117 {
118 double p_min = min_max_angle_by_dist[di][0];
119 double p_max = min_max_angle_by_dist[di][1];
120 double f = (di+0.5)*max_dist/nr_subdiv;
121 Amint_Amin += p_min * p_min;
122 Amint_d += p_min * f;
123 Amaxt_Amax += p_max * p_max;
124 Amaxt_d += p_max * f;
125 }
126 }
127 float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
128 float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
129
130 // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
131 min_radius *= 1.1f;
132 max_radius *= 0.9f;
133 if (min_radius < max_radius)
134 {
135 radii.r_min = min_radius;
136 radii.r_max = max_radius;
137 }
138 else
139 {
140 radii.r_max = min_radius;
141 radii.r_min = max_radius;
142 }
143
144 return histogram;
145}
146
147//////////////////////////////////////////////////////////////////////////////////////////////
148template <typename PointNT, typename PointOutT> Eigen::MatrixXf
150 const pcl::Indices &indices, const std::vector<float> &sqr_dists, double max_dist,
151 int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
152{
153 // Check if the full histogram has to be saved or not
154 Eigen::MatrixXf histogram;
155 if (compute_histogram)
156 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
157
158 // Check if enough points are provided or not
159 if (indices.size () < 2)
160 {
161 radii.r_max = 0;
162 radii.r_min = 0;
163 return histogram;
164 }
165
166 // Initialize minimum and maximum angle values in each distance bin
167 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
168 min_max_angle_by_dist[0].resize (2);
169 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
170 for (int di=1; di<nr_subdiv; di++)
171 {
172 min_max_angle_by_dist[di].resize (2);
173 min_max_angle_by_dist[di][0] = std::numeric_limits<double>::max();
174 min_max_angle_by_dist[di][1] = std::numeric_limits<double>::lowest();
175 }
176
177 // Compute distance by normal angle distribution for points
178 pcl::Indices::const_iterator i, begin (indices.begin()), end (indices.end());
179 for (i = begin+1; i != end; ++i)
180 {
181 // compute angle between the two lines going through normals (disregard orientation!)
182 double cosine = normals[*i].normal[0] * normals[*begin].normal[0] +
183 normals[*i].normal[1] * normals[*begin].normal[1] +
184 normals[*i].normal[2] * normals[*begin].normal[2];
185 if (cosine > 1) cosine = 1;
186 if (cosine < -1) cosine = -1;
187 double angle = std::acos (cosine);
188 if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
189
190 // Compute point to point distance
191 double dist = sqrt (sqr_dists[i-begin]);
192
193 if (dist > max_dist)
194 continue; /// \note: we neglect points that are outside the specified interval!
195
196 // compute bins and increase
197 int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
198 if (compute_histogram)
199 {
200 int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
201 histogram(bin_a, bin_d)++;
202 }
203
204 // update min-max values for distance bins
205 if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
206 if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
207 }
208
209 // Estimate radius from min and max lines
210 double Amint_Amin = 0, Amint_d = 0;
211 double Amaxt_Amax = 0, Amaxt_d = 0;
212 for (int di=0; di<nr_subdiv; di++)
213 {
214 // combute the members of A'*A*r = A'*D
215 if (min_max_angle_by_dist[di][1] >= 0)
216 {
217 double p_min = min_max_angle_by_dist[di][0];
218 double p_max = min_max_angle_by_dist[di][1];
219 double f = (di+0.5)*max_dist/nr_subdiv;
220 Amint_Amin += p_min * p_min;
221 Amint_d += p_min * f;
222 Amaxt_Amax += p_max * p_max;
223 Amaxt_d += p_max * f;
224 }
225 }
226 float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
227 float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
228
229 // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
230 min_radius *= 1.1f;
231 max_radius *= 0.9f;
232 if (min_radius < max_radius)
233 {
234 radii.r_min = min_radius;
235 radii.r_max = max_radius;
236 }
237 else
238 {
239 radii.r_max = min_radius;
240 radii.r_min = max_radius;
241 }
242
243 return histogram;
244}
245
246//////////////////////////////////////////////////////////////////////////////////////////////
247template <typename PointInT, typename PointNT, typename PointOutT> void
249{
250 // Check if search_radius_ was set
251 if (search_radius_ < 0)
252 {
253 PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
254 output.width = output.height = 0;
255 output.clear ();
256 return;
257 }
258
259 // List of indices and corresponding squared distances for a neighborhood
260 // \note resize is irrelevant for a radiusSearch ().
261 pcl::Indices nn_indices;
262 std::vector<float> nn_sqr_dists;
263
264 // Check if the full histogram has to be saved or not
265 if (save_histograms_)
266 {
267 // Reserve space for the output histogram dataset
268 histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
269 histograms_->reserve (output.size ());
270
271 // Iterating over the entire index vector
272 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
273 {
274 // Compute and store r_min and r_max in the output cloud
275 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
276 //histograms_->push_back (computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output[idx], true));
277 histograms_->push_back (computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output[idx], true));
278 }
279 }
280 else
281 {
282 // Iterating over the entire index vector
283 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
284 {
285 // Compute and store r_min and r_max in the output cloud
286 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
287 //computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output[idx], false);
288 computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output[idx], false);
289 }
290 }
291}
292
293#define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
294
295#endif // PCL_FEATURES_IMPL_RSD_H_
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
void computeFeature(PointCloudOut &output) override
Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by <setInpu...
Definition: rsd.hpp:248
Eigen::MatrixXf computeRSD(const pcl::PointCloud< PointInT > &surface, const pcl::PointCloud< PointNT > &normals, const pcl::Indices &indices, double max_dist, int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram=false)
Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhoo...
Definition: rsd.hpp:49
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201