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>
44 #include <pcl/console/time.h>
53 template<
typename Po
intT,
typename Po
intLT>
void
57 invalid_pt.
label = unsigned (0);
58 labels.
points.resize (input_->points.size (), invalid_pt);
59 labels.
width = input_->width;
60 labels.
height = input_->height;
62 extractEdges (labels);
64 assignLabelIndices (labels, label_indices);
68 template<
typename Po
intT,
typename Po
intLT>
void
71 const unsigned invalid_label = unsigned (0);
72 label_indices.resize (num_of_edgetype_);
73 for (
unsigned idx = 0; idx < input_->points.size (); idx++)
75 if (labels[idx].label != invalid_label)
77 for (
int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
79 if ((labels[idx].label >> edge_type) & 1)
80 label_indices[edge_type].indices.push_back (idx);
87 template<
typename Po
intT,
typename Po
intLT>
void
90 if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
93 const int num_of_ngbr = 8;
103 for (
int row = 1; row < int(input_->height) - 1; row++)
105 for (
int col = 1; col < int(input_->width) - 1; col++)
107 int curr_idx = row*int(input_->width) + col;
108 if (!pcl_isfinite (input_->points[curr_idx].z))
111 float curr_depth = fabsf (input_->points[curr_idx].z);
114 std::vector<float> nghr_dist;
115 nghr_dist.resize (8);
116 bool found_invalid_neighbor =
false;
117 for (
int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
119 int nghr_idx = curr_idx + directions[d_idx].
d_index;
120 assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
121 if (!pcl_isfinite (input_->points[nghr_idx].z))
123 found_invalid_neighbor =
true;
126 nghr_dist[d_idx] = curr_depth - fabsf (input_->points[nghr_idx].z);
129 if (!found_invalid_neighbor)
132 std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
133 std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
134 float nghr_dist_min = *min_itr;
135 float nghr_dist_max = *max_itr;
136 float dist_dominant = fabsf (nghr_dist_min) > fabsf (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
137 if (fabsf (dist_dominant) > th_depth_discon_*fabsf (curr_depth))
140 if (dist_dominant > 0.f)
142 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
143 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
147 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
148 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
159 int num_of_invalid_pt = 0;
160 for (
int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
162 int nghr_idx = curr_idx + directions[d_idx].
d_index;
163 assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
164 if (!pcl_isfinite (input_->points[nghr_idx].z))
166 dx += directions[d_idx].
d_x;
167 dy += directions[d_idx].
d_y;
173 assert (num_of_invalid_pt > 0);
174 float f_dx =
static_cast<float> (dx) /
static_cast<float> (num_of_invalid_pt);
175 float f_dy =
static_cast<float> (dy) /
static_cast<float> (num_of_invalid_pt);
178 float corr_depth = std::numeric_limits<float>::quiet_NaN ();
179 for (
int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
181 int s_row = row +
static_cast<int> (std::floor (f_dy*
static_cast<float> (s_idx)));
182 int s_col = col +
static_cast<int> (std::floor (f_dx*
static_cast<float> (s_idx)));
184 if (s_row < 0 || s_row >=
int(input_->height) || s_col < 0 || s_col >= int(input_->width))
187 if (pcl_isfinite (input_->points[s_row*
int(input_->width)+s_col].z))
189 corr_depth = fabsf (input_->points[s_row*
int(input_->width)+s_col].z);
194 if (!pcl_isnan (corr_depth))
197 float dist = curr_depth - corr_depth;
198 if (fabsf (dist) > th_depth_discon_*fabsf (curr_depth))
203 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
204 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
208 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
209 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
216 if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
217 labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
227 template<
typename Po
intT,
typename Po
intLT>
void
231 invalid_pt.
label = unsigned (0);
232 labels.
points.resize (input_->points.size (), invalid_pt);
233 labels.
width = input_->width;
234 labels.
height = input_->height;
237 extractEdges (labels);
239 this->assignLabelIndices (labels, label_indices);
243 template<
typename Po
intT,
typename Po
intLT>
void
246 if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
249 gray->
width = input_->width;
250 gray->
height = input_->height;
251 gray->
resize (input_->height*input_->width);
253 for (
size_t i = 0; i < input_->size (); ++i)
254 (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
263 for (uint32_t row=0; row<labels.
height; row++)
265 for (uint32_t col=0; col<labels.
width; col++)
267 if (img_edge_rgb (col, row).magnitude == 255.f)
268 labels[row * labels.
width + col].label |= EDGELABEL_RGB_CANNY;
275 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
279 invalid_pt.
label = unsigned (0);
280 labels.
points.resize (input_->points.size (), invalid_pt);
281 labels.
width = input_->width;
282 labels.
height = input_->height;
285 extractEdges (labels);
287 this->assignLabelIndices (labels, label_indices);
291 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
294 if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
298 nx.
width = normals_->width;
299 nx.
height = normals_->height;
300 nx.
resize (normals_->height*normals_->width);
302 ny.
width = normals_->width;
303 ny.
height = normals_->height;
304 ny.
resize (normals_->height*normals_->width);
306 for (uint32_t row=0; row<normals_->height; row++)
308 for (uint32_t col=0; col<normals_->width; col++)
310 nx (col, row).intensity = normals_->
points[row*normals_->width + col].normal_x;
311 ny (col, row).intensity = normals_->
points[row*normals_->width + col].normal_y;
319 edge.
canny (nx, ny, img_edge);
321 for (uint32_t row=0; row<labels.
height; row++)
323 for (uint32_t col=0; col<labels.
width; col++)
325 if (img_edge (col, row).magnitude == 255.f)
326 labels[row * labels.
width + col].label |= EDGELABEL_HIGH_CURVATURE;
333 template<
typename Po
intT,
typename Po
intNT,
typename Po
intLT>
void
337 invalid_pt.
label = unsigned (0);
338 labels.
points.resize (input_->points.size (), invalid_pt);
339 labels.
width = input_->width;
340 labels.
height = input_->height;
346 this->assignLabelIndices (labels, label_indices);
349 #define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
350 #define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
351 #define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
352 #define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
354 #endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_