38 #ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
39 #define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
41 #include <pcl/2d/edge.h>
42 #include <pcl/features/organized_edge_detection.h>
43 #include <pcl/console/print.h>
52 template<
typename Po
intT,
typename Po
intLT>
void
56 invalid_pt.
label = unsigned (0);
57 labels.
points.resize (input_->points.size (), invalid_pt);
58 labels.
width = input_->width;
59 labels.
height = input_->height;
61 extractEdges (labels);
63 assignLabelIndices (labels, label_indices);
67 template<
typename Po
intT,
typename Po
intLT>
void
70 const unsigned invalid_label = unsigned (0);
71 label_indices.resize (num_of_edgetype_);
72 for (std::size_t idx = 0; idx < input_->points.size (); idx++)
74 if (labels[idx].label != invalid_label)
76 for (
int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
78 if ((labels[idx].label >> edge_type) & 1)
79 label_indices[edge_type].indices.push_back (idx);
86 template<
typename Po
intT,
typename Po
intLT>
void
89 if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
92 const int num_of_ngbr = 8;
102 for (
int row = 1; row < int(input_->height) - 1; row++)
104 for (
int col = 1; col < int(input_->width) - 1; col++)
106 int curr_idx = row*int(input_->width) + col;
107 if (!std::isfinite (input_->points[curr_idx].z))
110 float curr_depth = std::abs (input_->points[curr_idx].z);
113 std::vector<float> nghr_dist;
114 nghr_dist.resize (8);
115 bool found_invalid_neighbor =
false;
116 for (
int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
118 int nghr_idx = curr_idx + directions[d_idx].
d_index;
119 assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
120 if (!std::isfinite (input_->points[nghr_idx].z))
122 found_invalid_neighbor =
true;
125 nghr_dist[d_idx] = curr_depth - std::abs (input_->points[nghr_idx].z);
128 if (!found_invalid_neighbor)
131 std::vector<float>::const_iterator min_itr, max_itr;
132 std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ());
133 float nghr_dist_min = *min_itr;
134 float nghr_dist_max = *max_itr;
135 float dist_dominant = std::max(std::abs (nghr_dist_min),std::abs (nghr_dist_max));
136 if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth))
139 if (dist_dominant > 0.f)
141 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
142 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
146 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
147 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
158 int num_of_invalid_pt = 0;
159 for (
const auto &direction : directions)
161 int nghr_idx = curr_idx + direction.d_index;
162 assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
163 if (!std::isfinite (input_->points[nghr_idx].z))
172 assert (num_of_invalid_pt > 0);
173 float f_dx =
static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
174 float f_dy =
static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
177 float corr_depth = std::numeric_limits<float>::quiet_NaN ();
178 for (
int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
180 int s_row = row +
static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
181 int s_col = col +
static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
183 if (s_row < 0 || s_row >=
int(input_->height) || s_col < 0 || s_col >= int(input_->width))
186 if (std::isfinite (input_->points[s_row*
int(input_->width)+s_col].z))
188 corr_depth = std::abs (input_->points[s_row*
int(input_->width)+s_col].z);
193 if (!std::isnan (corr_depth))
196 float dist = curr_depth - corr_depth;
197 if (std::abs (dist) > th_depth_discon_*std::abs (curr_depth))
202 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
203 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
207 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
208 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
215 if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
216 labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
226 template<
typename Po
intT,
typename Po
intLT>
void
230 invalid_pt.
label = unsigned (0);
231 labels.
points.resize (input_->points.size (), invalid_pt);
232 labels.
width = input_->width;
233 labels.
height = input_->height;
236 extractEdges (labels);
238 this->assignLabelIndices (labels, label_indices);
242 template<
typename Po
intT,
typename Po
intLT>
void
245 if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
248 gray->
width = input_->width;
249 gray->
height = input_->height;
250 gray->
resize (input_->height*input_->width);
252 for (std::size_t i = 0; i < input_->size (); ++i)
253 (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
262 for (std::uint32_t row=0; row<labels.
height; row++)
264 for (std::uint32_t col=0; col<labels.
width; col++)
266 if (img_edge_rgb (col, row).magnitude == 255.f)
267 labels[row * labels.
width + col].label |= EDGELABEL_RGB_CANNY;
274 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
278 invalid_pt.
label = unsigned (0);
279 labels.
points.resize (input_->points.size (), invalid_pt);
280 labels.
width = input_->width;
281 labels.
height = input_->height;
284 extractEdges (labels);
286 this->assignLabelIndices (labels, label_indices);
290 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
293 if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
297 nx.
width = normals_->width;
298 nx.
height = normals_->height;
299 nx.
resize (normals_->height*normals_->width);
301 ny.
width = normals_->width;
302 ny.
height = normals_->height;
303 ny.
resize (normals_->height*normals_->width);
305 for (std::uint32_t row=0; row<normals_->height; row++)
307 for (std::uint32_t col=0; col<normals_->width; col++)
309 nx (col, row).intensity = normals_->
points[row*normals_->width + col].normal_x;
310 ny (col, row).intensity = normals_->
points[row*normals_->width + col].normal_y;
318 edge.
canny (nx, ny, img_edge);
320 for (std::uint32_t row=0; row<labels.
height; row++)
322 for (std::uint32_t col=0; col<labels.
width; col++)
324 if (img_edge (col, row).magnitude == 255.f)
325 labels[row * labels.
width + col].label |= EDGELABEL_HIGH_CURVATURE;
332 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
336 invalid_pt.
label = unsigned (0);
337 labels.
points.resize (input_->points.size (), invalid_pt);
338 labels.
width = input_->width;
339 labels.
height = input_->height;
345 this->assignLabelIndices (labels, label_indices);
348 #define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
349 #define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
350 #define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
351 #define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
353 #endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setHysteresisThresholdHigh(float threshold)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
shared_ptr< PointCloud< PointT > > Ptr
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...
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 setHysteresisThresholdLow(float threshold)
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) ...
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...
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
std::uint32_t height
The point cloud height (if organized as an image-structure).
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 high curvature regions) ...
void resize(std::size_t n)
Resize the cloud.
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 and high curvature regions) and assig...
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.