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_
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)
uint32_t height
The point cloud height (if organized as an image-structure).
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
boost::shared_ptr< PointCloud< PointT > > Ptr
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
uint32_t width
The point cloud width (if organized as an image-structure).
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 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 resize(size_t n)
Resize the cloud.
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.