Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
organized_edge_detection.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, 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#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
39#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
40
41#include <pcl/2d/edge.h>
42#include <pcl/features/organized_edge_detection.h>
43
44/**
45 * Directions: 1 2 3
46 * 0 x 4
47 * 7 6 5
48 * e.g. direction y means we came from pixel with label y to the center pixel x
49 */
50//////////////////////////////////////////////////////////////////////////////
51template<typename PointT, typename PointLT> void
52pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
53{
54 pcl::Label invalid_pt;
55 invalid_pt.label = unsigned (0);
56 labels.resize (input_->size (), invalid_pt);
57 labels.width = input_->width;
58 labels.height = input_->height;
59
60 extractEdges (labels);
61
62 assignLabelIndices (labels, label_indices);
63}
64
65//////////////////////////////////////////////////////////////////////////////
66template<typename PointT, typename PointLT> void
67pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
68{
69 const unsigned invalid_label = unsigned (0);
70 label_indices.resize (num_of_edgetype_);
71 for (std::size_t idx = 0; idx < input_->size (); idx++)
72 {
73 if (labels[idx].label != invalid_label)
74 {
75 for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
76 {
77 if ((labels[idx].label >> edge_type) & 1)
78 label_indices[edge_type].indices.push_back (idx);
79 }
80 }
81 }
82}
83
84//////////////////////////////////////////////////////////////////////////////
85template<typename PointT, typename PointLT> void
87{
88 if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
89 {
90 // Fill lookup table for next points to visit
91 const int num_of_ngbr = 8;
92 Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
93 Neighbor(-1, -1, -labels.width - 1),
94 Neighbor( 0, -1, -labels.width ),
95 Neighbor( 1, -1, -labels.width + 1),
96 Neighbor( 1, 0, 1),
97 Neighbor( 1, 1, labels.width + 1),
98 Neighbor( 0, 1, labels.width ),
99 Neighbor(-1, 1, labels.width - 1)};
100
101 for (int row = 1; row < int(input_->height) - 1; row++)
102 {
103 for (int col = 1; col < int(input_->width) - 1; col++)
104 {
105 int curr_idx = row*int(input_->width) + col;
106 if (!std::isfinite ((*input_)[curr_idx].z))
107 continue;
108
109 float curr_depth = std::abs ((*input_)[curr_idx].z);
110
111 // Calculate depth distances between current point and neighboring points
112 std::vector<float> nghr_dist;
113 nghr_dist.resize (8);
114 bool found_invalid_neighbor = false;
115 for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
116 {
117 int nghr_idx = curr_idx + directions[d_idx].d_index;
118 assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
119 if (!std::isfinite ((*input_)[nghr_idx].z))
120 {
121 found_invalid_neighbor = true;
122 break;
123 }
124 nghr_dist[d_idx] = curr_depth - std::abs ((*input_)[nghr_idx].z);
125 }
126
127 if (!found_invalid_neighbor)
128 {
129 // Every neighboring points are valid
130 std::vector<float>::const_iterator min_itr, max_itr;
131 std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ());
132 float nghr_dist_min = *min_itr;
133 float nghr_dist_max = *max_itr;
134 float dist_dominant = std::abs (nghr_dist_min) > std::abs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
135 if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth))
136 {
137 // Found a depth discontinuity
138 if (dist_dominant > 0.f)
139 {
140 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
141 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
142 }
143 else
144 {
145 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
146 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
147 }
148 }
149 }
150 else
151 {
152 // Some neighboring points are not valid (nan points)
153 // Search for corresponding point across invalid points
154 // Search direction is determined by nan point locations with respect to current point
155 int dx = 0;
156 int dy = 0;
157 int num_of_invalid_pt = 0;
158 for (const auto &direction : directions)
159 {
160 int nghr_idx = curr_idx + direction.d_index;
161 assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
162 if (!std::isfinite ((*input_)[nghr_idx].z))
163 {
164 dx += direction.d_x;
165 dy += direction.d_y;
166 num_of_invalid_pt++;
167 }
168 }
169
170 // Search directions
171 assert (num_of_invalid_pt > 0);
172 float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
173 float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
174
175 // Search for corresponding point across invalid points
176 float corr_depth = std::numeric_limits<float>::quiet_NaN ();
177 for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
178 {
179 int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
180 int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
181
182 if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
183 break;
184
185 if (std::isfinite ((*input_)[s_row*int(input_->width)+s_col].z))
186 {
187 corr_depth = std::abs ((*input_)[s_row*int(input_->width)+s_col].z);
188 break;
189 }
190 }
191
192 if (!std::isnan (corr_depth))
193 {
194 // Found a corresponding point
195 float dist = curr_depth - corr_depth;
196 if (std::abs (dist) > th_depth_discon_*std::abs (curr_depth))
197 {
198 // Found a depth discontinuity
199 if (dist > 0.f)
200 {
201 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
202 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
203 }
204 else
205 {
206 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
207 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
208 }
209 }
210 }
211 else
212 {
213 // Not found a corresponding point, just nan boundary edge
214 if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
215 labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
216 }
217 }
218 }
219 }
220 }
221}
222
223
224//////////////////////////////////////////////////////////////////////////////
225template<typename PointT, typename PointLT> void
226pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
227{
228 pcl::Label invalid_pt;
229 invalid_pt.label = unsigned (0);
230 labels.resize (input_->size (), invalid_pt);
231 labels.width = input_->width;
232 labels.height = input_->height;
233
235 extractEdges (labels);
236
237 this->assignLabelIndices (labels, label_indices);
238}
239
240//////////////////////////////////////////////////////////////////////////////
241template<typename PointT, typename PointLT> void
243{
244 if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
245 {
247 gray->width = input_->width;
248 gray->height = input_->height;
249 gray->resize (input_->height*input_->width);
250
251 for (std::size_t i = 0; i < input_->size (); ++i)
252 (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
253
256 edge.setInputCloud (gray);
257 edge.setHysteresisThresholdLow (th_rgb_canny_low_);
258 edge.setHysteresisThresholdHigh (th_rgb_canny_high_);
259 edge.detectEdgeCanny (img_edge_rgb);
260
261 for (std::uint32_t row=0; row<labels.height; row++)
262 {
263 for (std::uint32_t col=0; col<labels.width; col++)
264 {
265 if (img_edge_rgb (col, row).magnitude == 255.f)
266 labels[row * labels.width + col].label |= EDGELABEL_RGB_CANNY;
267 }
268 }
269 }
270}
271
272//////////////////////////////////////////////////////////////////////////////
273template<typename PointT, typename PointNT, typename PointLT> void
274pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
275{
276 pcl::Label invalid_pt;
277 invalid_pt.label = unsigned (0);
278 labels.resize (input_->size (), invalid_pt);
279 labels.width = input_->width;
280 labels.height = input_->height;
281
283 extractEdges (labels);
284
285 this->assignLabelIndices (labels, label_indices);
286}
287
288//////////////////////////////////////////////////////////////////////////////
289template<typename PointT, typename PointNT, typename PointLT> void
291{
292 if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
293 {
294
296 nx.width = normals_->width;
297 nx.height = normals_->height;
298 nx.resize (normals_->height*normals_->width);
299
300 ny.width = normals_->width;
301 ny.height = normals_->height;
302 ny.resize (normals_->height*normals_->width);
303
304 for (std::uint32_t row=0; row<normals_->height; row++)
305 {
306 for (std::uint32_t col=0; col<normals_->width; col++)
307 {
308 nx (col, row).intensity = (*normals_)[row*normals_->width + col].normal_x;
309 ny (col, row).intensity = (*normals_)[row*normals_->width + col].normal_y;
310 }
311 }
312
315 edge.setHysteresisThresholdLow (th_hc_canny_low_);
316 edge.setHysteresisThresholdHigh (th_hc_canny_high_);
317 edge.canny (nx, ny, img_edge);
318
319 for (std::uint32_t row=0; row<labels.height; row++)
320 {
321 for (std::uint32_t col=0; col<labels.width; col++)
322 {
323 if (img_edge (col, row).magnitude == 255.f)
324 labels[row * labels.width + col].label |= EDGELABEL_HIGH_CURVATURE;
325 }
326 }
327 }
328}
329
330//////////////////////////////////////////////////////////////////////////////
331template<typename PointT, typename PointNT, typename PointLT> void
332pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
333{
334 pcl::Label invalid_pt;
335 invalid_pt.label = unsigned (0);
336 labels.resize (input_->size (), invalid_pt);
337 labels.width = input_->width;
338 labels.height = input_->height;
339
343
344 this->assignLabelIndices (labels, label_indices);
345}
346
347#define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
348#define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
349#define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
350#define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
351
352#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
void setHysteresisThresholdHigh(float threshold)
Definition edge.h:155
void canny(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Perform Canny edge detection with two separated input images for horizontal and vertical derivatives.
Definition edge.hpp:376
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition edge.hpp:312
void setHysteresisThresholdLow(float threshold)
Definition edge.h:149
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
Definition edge.h:304
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
void assignLabelIndices(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Assign point indices for each edge label.
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each ed...
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assig...
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions)
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge)
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point ...
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature r...
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
shared_ptr< PointCloud< PointT > > Ptr
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::uint32_t label