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>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
132 std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
133 float nghr_dist_min = *min_itr;
134 float nghr_dist_max = *max_itr;
135 float dist_dominant = std::abs (nghr_dist_min) > std::abs (nghr_dist_max) ? nghr_dist_min : 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);
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);
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);
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_