45 #include "pcl/gpu/people/label_blob2.h" 46 #include "pcl/gpu/people/label_common.h" 55 #include <pcl/point_cloud.h> 57 #include <pcl/conversions.h> 58 #include <pcl/common/eigen.h> 61 #include <pcl/PointIndices.h> 71 namespace label_skeleton
245 assert(lmap_in.depth() == CV_8UC1);
246 assert(dmap.depth() == CV_16U);
247 assert(lmap_out.depth() == CV_8UC1);
249 assert(lmap_in.rows == dmap.rows);
250 assert(lmap_in.cols == dmap.cols);
251 assert(lmap_out.rows == dmap.rows);
252 assert(lmap_out.cols == dmap.cols);
255 unsigned int half_patch = 2;
256 unsigned int depthThres = 300;
259 unsigned int endrow = (lmap_in.rows - half_patch);
260 unsigned int endcol = (lmap_in.cols - half_patch);
262 unsigned int endheight, endwidth;
266 const short* drow_offset;
267 const char* lrow_offset;
270 for(
unsigned int h = (0 + half_patch); h < endrow; h++)
272 endheight = (h + half_patch);
274 drow = dmap.ptr<
short>(h);
275 loutrow = lmap_out.ptr<
char>(h);
278 for(
unsigned int w = (0 + half_patch); w < endcol; w++)
280 endwidth = (w + half_patch);
288 for(
unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
290 drow_offset = dmap.ptr<
short>(h_l);
291 lrow_offset = lmap_in.ptr<
char>(h_l);
294 for(
unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
297 depth_l = drow_offset[w_l];
299 if(std::abs(depth - depth_l) <
static_cast<int> (depthThres))
301 label = lrow_offset[w_l];
302 votes[
static_cast<unsigned int>(label)]++;
307 unsigned int max = 0;
312 if(votes[static_cast<unsigned int>(i)] > max)
314 max = votes[
static_cast<unsigned int>(i)];
332 unsigned int sizeThres,
333 std::vector< std::vector<
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
334 std::vector< std::vector<pcl::PointIndices> >& indices)
336 assert(sorted.size () == indices.size ());
340 for(
unsigned int lab = 0; lab < indices.size(); ++lab)
342 unsigned int lid = 0;
344 for(
unsigned int l = 0; l < indices[lab].size(); l++)
347 if(indices[lab][l].indices.size() > sizeThres)
368 sorted[lab].push_back(b);
382 std::cout <<
"(I) : giveSortedBlobsInfo(): Size of outer vector: " << sorted.size() << std::endl;
383 for(
unsigned int i = 0; i < sorted.size(); i++)
385 std::cout <<
"(I) : giveSortedBlobsInfo(): Found " << sorted[i].size() <<
" parts of type " << i << std::endl;
386 std::cout <<
"(I) : giveSortedBlobsInfo(): indices : ";
387 for(
unsigned int j = 0; j < sorted[i].size(); j++)
389 std::cout <<
" id:" << sorted[i][j].id <<
" lid:" << sorted[i][j].lid;
391 std::cout << std::endl;
part_t
Our code is forseen to use maximal use 32 labels.
void smoothLabelImage(cv::Mat &lmap_in, cv::Mat &dmap, cv::Mat &lmap_out)
this function smooths the label image based on label and depth
Define standard C methods and C++ classes that are common to all methods.
pcl::PointIndices indices
Defines all the PCL implemented PointT point type structures.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
int giveSortedBlobsInfo(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted)
This function is a stupid helper function to debug easilier, it print out how the matrix was sorted...
void sortIndicesToBlob2(const pcl::PointCloud< pcl::PointXYZ > &cloud_in, unsigned int sizeThres, std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, std::vector< std::vector< pcl::PointIndices > > &indices)
This function takes a number of indices with label and sorts them in the blob matrix.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Eigen::Matrix3f eigenvect
Define methods for measuring time spent in code blocks.
Define methods for centroid estimation and covariance matrix calculus.
This structure contains all parameters to describe blobs and their parent/child relations.