Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
43 
44 // our headers
45 #include "pcl/gpu/people/label_blob2.h"
46 #include "pcl/gpu/people/label_common.h"
47 
48 // std
49 #include <vector>
50 
51 // opencv drawing stuff
52 //#include <opencv2/highgui/highgui.hpp>
53 
54 // PCL specific includes
55 #include <pcl/point_cloud.h>
56 #include <pcl/point_types.h>
57 #include <pcl/conversions.h>
58 #include <pcl/common/eigen.h>
59 #include <pcl/common/common.h>
60 #include <pcl/common/centroid.h>
61 #include <pcl/PointIndices.h>
62 
63 #include <pcl/common/time.h>
64 
65 namespace pcl
66 {
67  namespace gpu
68  {
69  namespace people
70  {
71  namespace label_skeleton
72  {
73  /*
74  * \brief this function smooths the label image based on label and depth
75  * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
76  * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
77  * \param[out] lmap_out the smoothed output labelmap as cvMat
78  * \param[in] patch_size make the patch size for smoothing
79  * \param[in] depthThres the z-distance threshold
80  * \todo add a Gaussian contribution function to depth and vote
81  **/
82  //inline void smoothLabelImage ( cv::Mat& lmap_in,
83  // cv::Mat& dmap,
84  // cv::Mat& lmap_out,
85  // unsigned int patch_size,
86  // unsigned int depthThres)
87  //{
88  // // check depth
89  // assert(lmap_in.depth() == CV_8UC1);
90  // assert(dmap.depth() == CV_16U);
91  // assert(lmap_out.depth() == CV_8UC1);
92  // // check size
93  // assert(lmap_in.rows == dmap.rows);
94  // assert(lmap_in.cols == dmap.cols);
95  // assert(lmap_out.rows == dmap.rows);
96  // assert(lmap_out.cols == dmap.cols);
97 
98  // unsigned int half_patch = static_cast<int> (patch_size/2);
99 
100  // // iterate over the height of the image (from 2 till 478)
101  // for(unsigned int h = (0 + half_patch); h < (lmap_in.rows - half_patch); h++)
102  // {
103  // // iterate over the width of the image (from 2 till 638)
104  // for(unsigned int w = (0 + half_patch); w < (lmap_in.cols - half_patch); w++)
105  // {
106  // short depth = dmap.at<short>(h, w);
107  // unsigned int votes[NUM_PARTS];
108  // //this should be unneeded but to test
109  // for(int j = 0 ; j< NUM_PARTS; j++)
110  // votes[j] = 0;
111 
112  // // iterate over the size of the patch in the height
113  // for(unsigned int h_l = (h - half_patch); h_l <= (h + half_patch); h_l++)
114  // {
115  // // iterate over the size of the patch in the width
116  // for(unsigned int w_l = (w - half_patch); w_l <= (w + half_patch); w_l++)
117  // {
118  // // get the depth of this part of the patch
119  // short depth_l = dmap.at<short>(h_l,w_l);
120  // // evaluate the difference to the centroid
121  // if(abs(depth - depth_l) < static_cast<int> (depthThres))
122  // {
123  // char label = lmap_in.at<char>(h_l,w_l);
124  // if(label >= 0 && label < NUM_PARTS)
125  // votes[static_cast<unsigned int> (label)]++;
126  // else
127  // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
128  // }
129  // }
130  // }
131 
132  // unsigned int max = 0;
133  // char label = lmap_in.at<char>(h,w);
134 
135  // // iterate over the bin to find the max
136  // for(char i=0; i<NUM_PARTS; i++)
137  // {
138  // if(votes[static_cast<int> (i)] > max)
139  // {
140  // max = votes[static_cast<int> (i)];
141  // label = i;
142  // }
143  // }
144  // lmap_out.at<char>(h,w) = label;
145  // }
146  // }
147  //}
148 
149  /*
150  * \brief this function smooths the label image based on label and depth
151  * \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
152  * \param[in] dmap the cvMat with the depths, must be CV_16U in mm
153  * \param[out] lmap_out the smoothed output labelmap as cvMat
154  * \todo make the patch size a parameter
155  * \todo make the z-distance a parameter
156  * \todo add a Gaussian contribution function to depth and vote
157  **/
158  //inline void smoothLabelImage2 ( cv::Mat& lmap_in,
159  // cv::Mat& dmap,
160  // cv::Mat& lmap_out)
161  //{
162  // // check depth
163  // assert(lmap_in.depth() == CV_8UC1);
164  // assert(dmap.depth() == CV_16U);
165  // assert(lmap_out.depth() == CV_8UC1);
166  // // check size
167  // assert(lmap_in.rows == dmap.rows);
168  // assert(lmap_in.cols == dmap.cols);
169  // assert(lmap_out.rows == dmap.rows);
170  // assert(lmap_out.cols == dmap.cols);
171 
172  // //unsigned int patch_size = 5;
173  // unsigned int half_patch = 2;
174  // unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
175 
176  // // iterate over the height of the image (from 2 till 478)
177  // unsigned int endrow = (lmap_in.rows - half_patch);
178  // unsigned int endcol = (lmap_in.cols - half_patch);
179  // for(unsigned int h = (0 + half_patch); h < endrow; h++)
180  // {
181  // unsigned int endheight = (h + half_patch);
182 
183  // // iterate over the width of the image (from 2 till 638)
184  // for(unsigned int w = (0 + half_patch); w < endcol; w++)
185  // {
186  // unsigned int endwidth = (w + half_patch);
187 
188  // short depth = dmap.at<short>(h, w);
189  // unsigned int votes[NUM_PARTS];
190  // //this should be unneeded but to test
191  // for(int j = 0 ; j< NUM_PARTS; j++)
192  // votes[j] = 0;
193 
194  // // iterate over the size of the patch in the height
195  // for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
196  // {
197  // // iterate over the size of the patch in the width
198  // for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
199  // {
200  // // get the depth of this part of the patch
201  // short depth_l = dmap.at<short>(h_l,w_l);
202  // // evaluate the difference to the centroid
203  // if(abs(depth - depth_l) < static_cast<int> (depthThres))
204  // {
205  // char label = lmap_in.at<char>(h_l,w_l);
206  // if(label >= 0 && label < NUM_PARTS)
207  // votes[static_cast<unsigned int>(label)]++;
208  // else
209  // std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
210  // }
211  // }
212  // }
213 
214  // unsigned int max = 0;
215  // char label = lmap_in.at<char>(h,w);
216 
217  // // iterate over the bin to find the max
218  // for(char i=0; i<NUM_PARTS; i++)
219  // {
220  // if(votes[static_cast<unsigned int>(i)] > max)
221  // {
222  // max = votes[static_cast<unsigned int>(i)];
223  // label = i;
224  // }
225  // }
226  // lmap_out.at<char>(h,w) = label;
227  // }
228  // }
229  //}
230 
231  /**
232  * @brief this function smooths the label image based on label and depth
233  * @param[in] lmap_in the cvMat with the labels, must be CV_8UC1
234  * @param[in] dmap the cvMat with the depths, must be CV_16U in mm
235  * @param[out] lmap_out the smoothed output labelmap as cvMat
236  * @todo make the patch size a parameter
237  * @todo make the z-distance a parameter
238  * @todo add a Gaussian contribution function to depth and vote
239  **/
240  inline void smoothLabelImage ( cv::Mat& lmap_in,
241  cv::Mat& dmap,
242  cv::Mat& lmap_out)
243  {
244  // check depth
245  assert(lmap_in.depth() == CV_8UC1);
246  assert(dmap.depth() == CV_16U);
247  assert(lmap_out.depth() == CV_8UC1);
248  // check size
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);
253 
254  //unsigned int patch_size = 5;
255  unsigned int half_patch = 2;
256  unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
257 
258  // iterate over the height of the image (from 2 till 478)
259  unsigned int endrow = (lmap_in.rows - half_patch);
260  unsigned int endcol = (lmap_in.cols - half_patch);
261  unsigned int votes[NUM_PARTS];
262  unsigned int endheight, endwidth;
263  const short* drow;
264  char *loutrow;
265  short depth;
266  const short* drow_offset;
267  const char* lrow_offset;
268  short depth_l;
269  char label;
270  for(unsigned int h = (0 + half_patch); h < endrow; h++)
271  {
272  endheight = (h + half_patch);
273 
274  drow = dmap.ptr<short>(h);
275  loutrow = lmap_out.ptr<char>(h);
276 
277  // iterate over the width of the image (from 2 till 638)
278  for(unsigned int w = (0 + half_patch); w < endcol; w++)
279  {
280  endwidth = (w + half_patch);
281 
282  depth = drow[w];
283  // reset votes
284  for(int j = 0 ; j< NUM_PARTS; j++)
285  votes[j] = 0;
286 
287  // iterate over the size of the patch in the height
288  for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
289  {
290  drow_offset = dmap.ptr<short>(h_l);
291  lrow_offset = lmap_in.ptr<char>(h_l);
292 
293  // iterate over the size of the patch in the width
294  for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
295  {
296  // get the depth of this part of the patch
297  depth_l = drow_offset[w_l];
298  // evaluate the difference to the centroid
299  if(abs(depth - depth_l) < static_cast<int> (depthThres))
300  {
301  label = lrow_offset[w_l];
302  votes[static_cast<unsigned int>(label)]++;
303  }
304  }
305  }
306 
307  unsigned int max = 0;
308 
309  // iterate over the bin to find the max
310  for(char i=0; i<NUM_PARTS; i++)
311  {
312  if(votes[static_cast<unsigned int>(i)] > max)
313  {
314  max = votes[static_cast<unsigned int>(i)];
315  loutrow[w] = i;
316  }
317  }
318  }
319  }
320  }
321 
322  /**
323  * @brief This function takes a number of indices with label and sorts them in the blob matrix
324  * @param[in] cloud_in the original input pointcloud from which everything was calculated
325  * @param[in] sizeThres the minimal size needed to be considered as a valid blob
326  * @param[out] sorted the matrix in which every blob will be pushed back
327  * @param[in] indices the matrix of PointIndices
328  * @todo implement the eigenvalue evaluation again
329  * @todo do we still need sizeThres?
330  **/
331  inline void sortIndicesToBlob2 ( const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
332  unsigned int sizeThres,
333  std::vector< std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
334  std::vector< std::vector<pcl::PointIndices> >& indices)
335  {
336  assert(sorted.size () == indices.size ());
337 
338  unsigned int id = 0;
339  // Iterate over all labels
340  for(unsigned int lab = 0; lab < indices.size(); ++lab)
341  {
342  unsigned int lid = 0;
343  // Iterate over all blobs of this label
344  for(unsigned int l = 0; l < indices[lab].size(); l++)
345  {
346  // If the blob has enough pixels
347  if(indices[lab][l].indices.size() > sizeThres)
348  {
349  Blob2 b;
350 
351  b.indices = indices[lab][l];
352 
353  pcl::compute3DCentroid(cloud_in, b.indices, b.mean);
354 #ifdef NEED_STATS
356  pcl::getMinMax3D(cloud_in, b.indices, b.min, b.max);
358 #endif
359  // Check if it is a valid blob
360  //if((b.eigenval(0) < LUT_max_part_size[(int) lab]) && (b.mean(2) != 0))
361  if((b.mean(2) != 0))
362  {
363  b.id = id;
364  id++;
365  b.label = static_cast<part_t> (lab);
366  b.lid = lid;
367  lid++;
368  sorted[lab].push_back(b);
369  }
370  }
371  }
372  }
373  }
374 
375  /**
376  * @brief This function is a stupid helper function to debug easilier, it print out how the matrix was sorted
377  * @param[in] sorted the matrix of blobs
378  * @return Zero if everything went well
379  **/
380  inline int giveSortedBlobsInfo ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted)
381  {
382  std::cout << "(I) : giveSortedBlobsInfo(): Size of outer vector: " << sorted.size() << std::endl;
383  for(unsigned int i = 0; i < sorted.size(); i++)
384  {
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++)
388  {
389  std::cout << " id:" << sorted[i][j].id << " lid:" << sorted[i][j].lid;
390  }
391  std::cout << std::endl;
392  }
393  return 0;
394  }
395  } // end namespace label_skeleton
396  } // end namespace people
397  } // end namespace gpu
398 } // end namespace pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
part_t
Our code is forseen to use maximal use 32 labels.
Definition: label_common.h:75
Eigen::Vector4f min
Definition: label_blob2.h:75
void smoothLabelImage(cv::Mat &lmap_in, cv::Mat &dmap, cv::Mat &lmap_out)
this function smooths the label image based on label and depth
pcl::PointIndices indices
Definition: label_blob2.h:74
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:76
Eigen::Matrix3f cov
Definition: label_blob2.h:64
Eigen::Vector3f eigenval
Definition: label_blob2.h:65
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:66
Eigen::Vector4f mean
Definition: label_blob2.h:63
This structure contains all parameters to describe blobs and their parent/child relations.
Definition: label_blob2.h:57