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_