Point Cloud Library (PCL)  1.9.1
label_segment.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: $
37  * @author Koen Buys
38  * @file segment.h
39  * @brief This file contains the function prototypes for the segmentation functions
40  */
41 
42 #ifndef PCL_GPU_PEOPLE_LABEL_SEGMENT_H_
43 #define PCL_GPU_PEOPLE_LABEL_SEGMENT_H_
44 
45 // our headers
46 #include "pcl/gpu/people/label_blob2.h"
47 #include "pcl/gpu/people/label_common.h"
48 
49 // std
50 #include <vector>
51 
52 // opencv drawing stuff
53 //#include <opencv2/highgui/highgui.hpp>
54 
55 // PCL specific includes
56 #include <pcl/point_cloud.h>
57 #include <pcl/point_types.h>
58 #include <pcl/conversions.h>
59 #include <pcl/common/eigen.h>
60 #include <pcl/common/common.h>
61 #include <pcl/common/centroid.h>
62 #include <pcl/PointIndices.h>
63 
64 #include <pcl/common/time.h>
65 
66 namespace pcl
67 {
68  namespace gpu
69  {
70  namespace people
71  {
72  namespace label_skeleton
73  {
74  /*
75  * \brief this function smooths the label image based on label and depth
76  * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
77  * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
78  * \param[out] lmap_out the smoothed output labelmap as cvMat
79  * \param[in] patch_size make the patch size for smoothing
80  * \param[in] depthThres the z-distance threshold
81  * \todo add a Gaussian contribution function to depth and vote
82  **/
83  //inline void smoothLabelImage ( cv::Mat& lmap_in,
84  // cv::Mat& dmap,
85  // cv::Mat& lmap_out,
86  // unsigned int patch_size,
87  // unsigned int depthThres)
88  //{
89  // // check depth
90  // assert(lmap_in.depth() == CV_8UC1);
91  // assert(dmap.depth() == CV_16U);
92  // assert(lmap_out.depth() == CV_8UC1);
93  // // check size
94  // assert(lmap_in.rows == dmap.rows);
95  // assert(lmap_in.cols == dmap.cols);
96  // assert(lmap_out.rows == dmap.rows);
97  // assert(lmap_out.cols == dmap.cols);
98 
99  // unsigned int half_patch = static_cast<int> (patch_size/2);
100 
101  // // iterate over the height of the image (from 2 till 478)
102  // for(unsigned int h = (0 + half_patch); h < (lmap_in.rows - half_patch); h++)
103  // {
104  // // iterate over the width of the image (from 2 till 638)
105  // for(unsigned int w = (0 + half_patch); w < (lmap_in.cols - half_patch); w++)
106  // {
107  // short depth = dmap.at<short>(h, w);
108  // unsigned int votes[NUM_PARTS];
109  // //this should be unneeded but to test
110  // for(int j = 0 ; j< NUM_PARTS; j++)
111  // votes[j] = 0;
112 
113  // // iterate over the size of the patch in the height
114  // for(unsigned int h_l = (h - half_patch); h_l <= (h + half_patch); h_l++)
115  // {
116  // // iterate over the size of the patch in the width
117  // for(unsigned int w_l = (w - half_patch); w_l <= (w + half_patch); w_l++)
118  // {
119  // // get the depth of this part of the patch
120  // short depth_l = dmap.at<short>(h_l,w_l);
121  // // evaluate the difference to the centroid
122  // if(abs(depth - depth_l) < static_cast<int> (depthThres))
123  // {
124  // char label = lmap_in.at<char>(h_l,w_l);
125  // if(label >= 0 && label < NUM_PARTS)
126  // votes[static_cast<unsigned int> (label)]++;
127  // else
128  // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
129  // }
130  // }
131  // }
132 
133  // unsigned int max = 0;
134  // char label = lmap_in.at<char>(h,w);
135 
136  // // iterate over the bin to find the max
137  // for(char i=0; i<NUM_PARTS; i++)
138  // {
139  // if(votes[static_cast<int> (i)] > max)
140  // {
141  // max = votes[static_cast<int> (i)];
142  // label = i;
143  // }
144  // }
145  // lmap_out.at<char>(h,w) = label;
146  // }
147  // }
148  //}
149 
150  /*
151  * \brief this function smooths the label image based on label and depth
152  * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
153  * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
154  * \param[out] lmap_out the smoothed output labelmap as cvMat
155  * \todo make the patch size a parameter
156  * \todo make the z-distance a parameter
157  * \todo add a Gaussian contribution function to depth and vote
158  **/
159  //inline void smoothLabelImage2 ( cv::Mat& lmap_in,
160  // cv::Mat& dmap,
161  // cv::Mat& lmap_out)
162  //{
163  // // check depth
164  // assert(lmap_in.depth() == CV_8UC1);
165  // assert(dmap.depth() == CV_16U);
166  // assert(lmap_out.depth() == CV_8UC1);
167  // // check size
168  // assert(lmap_in.rows == dmap.rows);
169  // assert(lmap_in.cols == dmap.cols);
170  // assert(lmap_out.rows == dmap.rows);
171  // assert(lmap_out.cols == dmap.cols);
172 
173  // //unsigned int patch_size = 5;
174  // unsigned int half_patch = 2;
175  // unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
176 
177  // // iterate over the height of the image (from 2 till 478)
178  // unsigned int endrow = (lmap_in.rows - half_patch);
179  // unsigned int endcol = (lmap_in.cols - half_patch);
180  // for(unsigned int h = (0 + half_patch); h < endrow; h++)
181  // {
182  // unsigned int endheight = (h + half_patch);
183 
184  // // iterate over the width of the image (from 2 till 638)
185  // for(unsigned int w = (0 + half_patch); w < endcol; w++)
186  // {
187  // unsigned int endwidth = (w + half_patch);
188 
189  // short depth = dmap.at<short>(h, w);
190  // unsigned int votes[NUM_PARTS];
191  // //this should be unneeded but to test
192  // for(int j = 0 ; j< NUM_PARTS; j++)
193  // votes[j] = 0;
194 
195  // // iterate over the size of the patch in the height
196  // for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
197  // {
198  // // iterate over the size of the patch in the width
199  // for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
200  // {
201  // // get the depth of this part of the patch
202  // short depth_l = dmap.at<short>(h_l,w_l);
203  // // evaluate the difference to the centroid
204  // if(abs(depth - depth_l) < static_cast<int> (depthThres))
205  // {
206  // char label = lmap_in.at<char>(h_l,w_l);
207  // if(label >= 0 && label < NUM_PARTS)
208  // votes[static_cast<unsigned int>(label)]++;
209  // else
210  // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
211  // }
212  // }
213  // }
214 
215  // unsigned int max = 0;
216  // char label = lmap_in.at<char>(h,w);
217 
218  // // iterate over the bin to find the max
219  // for(char i=0; i<NUM_PARTS; i++)
220  // {
221  // if(votes[static_cast<unsigned int>(i)] > max)
222  // {
223  // max = votes[static_cast<unsigned int>(i)];
224  // label = i;
225  // }
226  // }
227  // lmap_out.at<char>(h,w) = label;
228  // }
229  // }
230  //}
231 
232  /**
233  * @brief this function smooths the label image based on label and depth
234  * @param[in] lmap_in the cvMat with the labels, must be CV_8UC1
235  * @param[in] dmap the cvMat with the depths, must be CV_16U in mm
236  * @param[out] lmap_out the smoothed output labelmap as cvMat
237  * @todo make the patch size a parameter
238  * @todo make the z-distance a parameter
239  * @todo add a Gaussian contribution function to depth and vote
240  **/
241  inline void smoothLabelImage ( cv::Mat& lmap_in,
242  cv::Mat& dmap,
243  cv::Mat& lmap_out)
244  {
245  // check depth
246  assert(lmap_in.depth() == CV_8UC1);
247  assert(dmap.depth() == CV_16U);
248  assert(lmap_out.depth() == CV_8UC1);
249  // check size
250  assert(lmap_in.rows == dmap.rows);
251  assert(lmap_in.cols == dmap.cols);
252  assert(lmap_out.rows == dmap.rows);
253  assert(lmap_out.cols == dmap.cols);
254 
255  //unsigned int patch_size = 5;
256  unsigned int half_patch = 2;
257  unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
258 
259  // iterate over the height of the image (from 2 till 478)
260  unsigned int endrow = (lmap_in.rows - half_patch);
261  unsigned int endcol = (lmap_in.cols - half_patch);
262  unsigned int votes[NUM_PARTS];
263  unsigned int endheight, endwidth;
264  const short* drow;
265  char *loutrow;
266  short depth;
267  const short* drow_offset;
268  const char* lrow_offset;
269  short depth_l;
270  char label;
271  for(unsigned int h = (0 + half_patch); h < endrow; h++)
272  {
273  endheight = (h + half_patch);
274 
275  drow = dmap.ptr<short>(h);
276  loutrow = lmap_out.ptr<char>(h);
277 
278  // iterate over the width of the image (from 2 till 638)
279  for(unsigned int w = (0 + half_patch); w < endcol; w++)
280  {
281  endwidth = (w + half_patch);
282 
283  depth = drow[w];
284  // reset votes
285  for(int j = 0 ; j< NUM_PARTS; j++)
286  votes[j] = 0;
287 
288  // iterate over the size of the patch in the height
289  for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
290  {
291  drow_offset = dmap.ptr<short>(h_l);
292  lrow_offset = lmap_in.ptr<char>(h_l);
293 
294  // iterate over the size of the patch in the width
295  for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
296  {
297  // get the depth of this part of the patch
298  depth_l = drow_offset[w_l];
299  // evaluate the difference to the centroid
300  if(abs(depth - depth_l) < static_cast<int> (depthThres))
301  {
302  label = lrow_offset[w_l];
303  votes[static_cast<unsigned int>(label)]++;
304  }
305  }
306  }
307 
308  unsigned int max = 0;
309 
310  // iterate over the bin to find the max
311  for(char i=0; i<NUM_PARTS; i++)
312  {
313  if(votes[static_cast<unsigned int>(i)] > max)
314  {
315  max = votes[static_cast<unsigned int>(i)];
316  loutrow[w] = i;
317  }
318  }
319  }
320  }
321  }
322 
323  /**
324  * @brief This function takes a number of indices with label and sorts them in the blob matrix
325  * @param[in] cloud_in the original input pointcloud from which everything was calculated
326  * @param[in] sizeThres the minimal size needed to be considered as a valid blob
327  * @param[out] sorted the matrix in which every blob will be pushed back
328  * @param[in] indices the matrix of PointIndices
329  * @todo implement the eigenvalue evaluation again
330  * @todo do we still need sizeThres?
331  **/
332  inline void sortIndicesToBlob2 ( const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
333  unsigned int sizeThres,
334  std::vector< std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
335  std::vector< std::vector<pcl::PointIndices> >& indices)
336  {
337  assert(sorted.size () == indices.size ());
338 
339  unsigned int id = 0;
340  // Iterate over all labels
341  for(unsigned int lab = 0; lab < indices.size(); ++lab)
342  {
343  unsigned int lid = 0;
344  // Iterate over all blobs of this label
345  for(unsigned int l = 0; l < indices[lab].size(); l++)
346  {
347  // If the blob has enough pixels
348  if(indices[lab][l].indices.size() > sizeThres)
349  {
350  Blob2 b;
351 
352  b.indices = indices[lab][l];
353 
354  pcl::compute3DCentroid(cloud_in, b.indices, b.mean);
355 #ifdef NEED_STATS
357  pcl::getMinMax3D(cloud_in, b.indices, b.min, b.max);
359 #endif
360  // Check if it is a valid blob
361  //if((b.eigenval(0) < LUT_max_part_size[(int) lab]) && (b.mean(2) != 0))
362  if((b.mean(2) != 0))
363  {
364  b.id = id;
365  id++;
366  b.label = static_cast<part_t> (lab);
367  b.lid = lid;
368  lid++;
369  sorted[lab].push_back(b);
370  }
371  }
372  }
373  }
374  }
375 
376  /**
377  * @brief This function is a stupid helper function to debug easilier, it print out how the matrix was sorted
378  * @param[in] sorted the matrix of blobs
379  * @return Zero if everything went well
380  **/
381  inline int giveSortedBlobsInfo ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted)
382  {
383  std::cout << "(I) : giveSortedBlobsInfo(): Size of outer vector: " << sorted.size() << std::endl;
384  for(unsigned int i = 0; i < sorted.size(); i++)
385  {
386  std::cout << "(I) : giveSortedBlobsInfo(): Found " << sorted[i].size() << " parts of type " << i << std::endl;
387  std::cout << "(I) : giveSortedBlobsInfo(): indices : ";
388  for(unsigned int j = 0; j < sorted[i].size(); j++)
389  {
390  std::cout << " id:" << sorted[i][j].id << " lid:" << sorted[i][j].lid;
391  }
392  std::cout << std::endl;
393  }
394  return 0;
395  }
396  } // end namespace label_skeleton
397  } // end namespace people
398  } // end namespace gpu
399 } // end namespace pcl
400 
401 #endif //#ifndef LABELSKEL_SEGMENT_H
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Eigen::Vector4f min
Definition: label_blob2.h:76
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
Definition: label_blob2.h:75
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.
Definition: common.hpp:242
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...
Definition: eigen.hpp:251
Eigen::Vector4f max
Definition: label_blob2.h:77
Eigen::Matrix3f cov
Definition: label_blob2.h:65
Eigen::Vector3f eigenval
Definition: label_blob2.h:66
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition: centroid.hpp:251
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
Eigen::Matrix3f eigenvect
Definition: label_blob2.h:67
Define methods for measuring time spent in code blocks.
Eigen::Vector4f mean
Definition: label_blob2.h:64
Define methods for centroid estimation and covariance matrix calculus.
This structure contains all parameters to describe blobs and their parent/child relations.
Definition: label_blob2.h:58