Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
65namespace 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(std::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(std::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(std::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 **/
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
Define methods for centroid estimation and covariance matrix calculus.
PointCloud represents the base class in PCL for storing collections of 3D points.
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
Define methods for measuring time spent in code blocks.
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:295
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:268
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:296
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:56
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 smoothLabelImage(cv::Mat &lmap_in, cv::Mat &dmap, cv::Mat &lmap_out)
this function smooths the label image based on label and depth
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.
part_t
Our code is forseen to use maximal use 32 labels.
This structure contains all parameters to describe blobs and their parent/child relations.
Definition label_blob2.h:58
Eigen::Matrix3f eigenvect
Definition label_blob2.h:66
Eigen::Vector4f mean
Definition label_blob2.h:63
Eigen::Matrix3f cov
Definition label_blob2.h:64
pcl::PointIndices indices
Definition label_blob2.h:74
Eigen::Vector3f eigenval
Definition label_blob2.h:65
Eigen::Vector4f min
Definition label_blob2.h:75
Eigen::Vector4f max
Definition label_blob2.h:76