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> 51 template<
typename Po
intT,
typename Po
intLT>
void 55 invalid_pt.
label = unsigned (0);
56 labels.
points.resize (input_->size (), invalid_pt);
57 labels.
width = input_->width;
58 labels.
height = input_->height;
60 extractEdges (labels);
62 assignLabelIndices (labels, label_indices);
66 template<
typename Po
intT,
typename Po
intLT>
void 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++)
73 if (labels[idx].label != invalid_label)
75 for (
int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
77 if ((labels[idx].label >> edge_type) & 1)
78 label_indices[edge_type].indices.push_back (idx);
85 template<
typename Po
intT,
typename Po
intLT>
void 88 if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
91 const int num_of_ngbr = 8;
101 for (
int row = 1; row < int(input_->height) - 1; row++)
103 for (
int col = 1; col < int(input_->width) - 1; col++)
105 int curr_idx = row*int(input_->width) + col;
106 if (!std::isfinite ((*input_)[curr_idx].z))
109 float curr_depth = std::abs ((*input_)[curr_idx].z);
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++)
117 int nghr_idx = curr_idx + directions[d_idx].
d_index;
118 assert (nghr_idx >= 0 && nghr_idx < input_->size ());
119 if (!std::isfinite ((*input_)[nghr_idx].z))
121 found_invalid_neighbor =
true;
124 nghr_dist[d_idx] = curr_depth - std::abs ((*input_)[nghr_idx].z);
127 if (!found_invalid_neighbor)
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))
138 if (dist_dominant > 0.f)
140 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
141 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
145 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
146 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
157 int num_of_invalid_pt = 0;
158 for (
const auto &direction : directions)
160 int nghr_idx = curr_idx + direction.d_index;
161 assert (nghr_idx >= 0 && nghr_idx < input_->size ());
162 if (!std::isfinite ((*input_)[nghr_idx].z))
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);
176 float corr_depth = std::numeric_limits<float>::quiet_NaN ();
177 for (
int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
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)));
182 if (s_row < 0 || s_row >=
int(input_->height) || s_col < 0 || s_col >= int(input_->width))
185 if (std::isfinite ((*input_)[s_row*
int(input_->width)+s_col].z))
187 corr_depth = std::abs ((*input_)[s_row*
int(input_->width)+s_col].z);
192 if (!std::isnan (corr_depth))
195 float dist = curr_depth - corr_depth;
196 if (std::abs (dist) > th_depth_discon_*std::abs (curr_depth))
201 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
202 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
206 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
207 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
214 if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
215 labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
225 template<
typename Po
intT,
typename Po
intLT>
void 229 invalid_pt.
label = unsigned (0);
230 labels.
points.resize (input_->size (), invalid_pt);
231 labels.
width = input_->width;
232 labels.
height = input_->height;
235 extractEdges (labels);
237 this->assignLabelIndices (labels, label_indices);
241 template<
typename Po
intT,
typename Po
intLT>
void 244 if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
247 gray->
width = input_->width;
248 gray->
height = input_->height;
249 gray->
resize (input_->height*input_->width);
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);
265 if (img_edge_rgb (col, row).magnitude == 255.f)
266 labels[row * labels.
width + col].label |= EDGELABEL_RGB_CANNY;
273 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void 277 invalid_pt.
label = unsigned (0);
278 labels.
points.resize (input_->size (), invalid_pt);
279 labels.
width = input_->width;
280 labels.
height = input_->height;
283 extractEdges (labels);
285 this->assignLabelIndices (labels, label_indices);
289 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void 292 if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
296 nx.
width = normals_->width;
297 nx.
height = normals_->height;
298 nx.
resize (normals_->height*normals_->width);
300 ny.
width = normals_->width;
301 ny.
height = normals_->height;
302 ny.
resize (normals_->height*normals_->width);
308 nx (col, row).intensity = (*normals_)[row*normals_->width + col].normal_x;
309 ny (col, row).intensity = (*normals_)[row*normals_->width + col].normal_y;
317 edge.
canny (nx, ny, img_edge);
323 if (img_edge (col, row).magnitude == 255.f)
324 labels[row * labels.
width + col].label |= EDGELABEL_HIGH_CURVATURE;
331 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void 335 invalid_pt.
label = unsigned (0);
336 labels.
points.resize (input_->size (), invalid_pt);
337 labels.
width = input_->width;
338 labels.
height = input_->height;
344 this->assignLabelIndices (labels, label_indices);
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>; 352 #endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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, RGB Canny edge, and high curvature r...
void setHysteresisThresholdHigh(float threshold)
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) ...
void setHysteresisThresholdLow(float threshold)
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
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 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 assignLabelIndices(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Assign point indices for each edge label.
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.