Point Cloud Library (PCL)  1.9.0-dev
label_tree.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 tree.h
39  * @brief This file contains the function prototypes for the tree building functions.
40  */
41 
42 #ifndef PCL_GPU_PEOPLE_LABEL_TREE_H_
43 #define PCL_GPU_PEOPLE_LABEL_TREE_H_
44 
45 // our headers
46 #include "pcl/gpu/people/label_blob2.h" //this one defines the blob structure
47 #include "pcl/gpu/people/label_common.h" //this one defines the LUT's
48 #include "pcl/gpu/people/person_attribs.h"
49 
50 // std
51 #include <vector>
52 #include <iostream>
53 #include <algorithm>
54 #include <stdexcept>
55 
56 // PCL specific includes
57 #include <pcl/conversions.h>
58 #include <pcl/point_cloud.h>
59 #include <pcl/point_types.h>
60 #include <pcl/console/print.h>
61 
62 #include <pcl/common/eigen.h>
63 #include <pcl/common/common.h>
64 #include <pcl/common/centroid.h>
65 
66 namespace pcl
67 {
68  namespace gpu
69  {
70  namespace people
71  {
72  /**
73  * @brief This structure contains all parameters to describe the segmented tree
74  */
75  struct Tree2
76  {
77  //Inline constructor
79  {
80  for(int i=0;i<NUM_PARTS;i++)
81  parts_lid[i] = NO_CHILD;
82  }
83 
84  int id; // specific identification number of this tree
85  part_t label; // labels which part the root of this tree is
86  int lid; // label id, which number of this type of part is this
87  int nr_parts; // the number of parts in this tree
88  int parts_lid[NUM_PARTS]; // Indicate the used parts
89  float total_dist_error; // sum of all distance errors
90  float norm_dist_error; // total_dist_error/nr_parts
91 
92  Eigen::Vector4f mean; // mean in xyz
93  Eigen::Matrix3f cov; // covariance in 3x3 matrix
94  Eigen::Vector3f eigenval; // eigenvalue of blob
95  Eigen::Matrix3f eigenvect; // eigenvector of blob
96 
97  pcl::PointIndices indices; // The indices of the pointcloud
98  Eigen::Vector4f min; // The min of the bounding box
99  Eigen::Vector4f max; // The max of the bounding box
100  };
101 
102  inline std::ostream& operator << (std::ostream& os, const Tree2& t)
103  {
104  os << " Tree2 id " << t.id << " label " << t.label << " lid " << t.lid << " nr_parts " << t.nr_parts << std::endl;
105  os << " total_dist_error " << t.total_dist_error << " norm_dist_error " << t.norm_dist_error << std::endl;
106  os << " mean " << t.mean(0) << " , " << t.mean(1) << " , " << t.mean(2) << " , " << t.mean(3) << std::endl;
107  os << " cov " << std::endl << t.cov << std::endl;
108  os << " eigenval " << t.eigenval(0) << " , " << t.eigenval(1) << " , " << t.eigenval(2) << std::endl;
109  os << " eigenvect " << std::endl << t.eigenvect << std::endl;
110  os << " min " << t.min(0) << " , " << t.min(1) << " , " << t.min(2) << " , " << t.min(3) << std::endl;
111  os << " max " << t.max(0) << " , " << t.max(1) << " , " << t.max(2) << " , " << t.max(3) << std::endl;
112  os << " indices length " << t.indices.indices.size() << std::endl;
113  return (os);
114  }
115 
116  /**
117  * @brief This function sets the children of the leaf nodes to leaf, meaning that we came to the correct end
118  * @param[in] sorted The matrix of all blobs
119  * @param[in] label The label of which all children are to be set as leafs
120  * @return Zero if everything went well
121  **/
122  inline int
123  leafBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
124  int label )
125  {
126  if(sorted[label].size() == 0)
127  return 0;
128  for(size_t i = 0; i < sorted[label].size(); i++)
129  {
130  for(int j = 0; j < MAX_CHILD; j++)
131  sorted[label][i].child_id[j] = LEAF;
132  }
133  return 0;
134  }
135 
136  /**
137  * @brief This function sets the specific child of the vector to no child, meaning that there are no such children
138  * @param[in] sorted The matrix of all blobs
139  * @param[in] label The label of which the child must be set to NO_CHILD
140  * @param[in] child_number The index of the respective child that must be set
141  * @return Zero if everything went well
142  **/
143  inline int
144  noChildBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
145  int label,
146  int child_number)
147  {
148  if(sorted[label].size() == 0)
149  return 0;
150  for(size_t i = 0; i < sorted[label].size(); i++){
151  sorted[label][i].child_id[child_number] = NO_CHILD;
152  }
153  return 0;
154  }
155 
156  /**
157  * @brief This function test if children were found for this label
158  * @return True if this label has valid children
159  **/
160  inline bool
161  hasThisLabelChildren ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
162  part_t label,
163  int child_number)
164  {
165  if(sorted[label].size() == 0)
166  return 0;
167  for(size_t i = 0; i < sorted[label].size(); i++)
168  if((sorted[label][i].child_id[child_number] != NO_CHILD) && (sorted[label][i].child_id[child_number] != LEAF))
169  return 1;
170  return 0;
171  }
172 
173  /**
174  * @brief This is the evaluation function used to compare two blobs
175  * @param[in] parent pointer to the parent blob
176  * @param[in] child pointer to the child blob
177  * @param[in] child_nr the number of the child
178  * @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
179  * @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
180  **/
181  inline float
182  evaluateBlobs (Blob2& parent, Blob2& child, int child_nr)
183  {
184  float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
185  pow(parent.mean(1) - child.mean(1), 2) +
186  pow(parent.mean(2) - child.mean(2), 2));
187  float offset = fabs(LUT_ideal_length[(int)parent.label][child_nr] - root);
188  if(offset > LUT_max_length_offset[(int)parent.label][child_nr])
189  return -1.0;
190  else
191  return offset;
192  }
193 
194  /**
195  * @brief This is the evaluation function used to compare two blobs
196  * @param[in] parent pointer to the parent blob
197  * @param[in] child pointer to the child blob
198  * @param[in] child_nr the number of the child
199  * @param person_attribs
200  * @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
201  * @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
202  **/
203  inline float
205  Blob2& child,
206  int child_nr,
207  PersonAttribs::Ptr person_attribs)
208  {
209  float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
210  pow(parent.mean(1) - child.mean(1), 2) +
211  pow(parent.mean(2) - child.mean(2), 2));
212  float offset = fabs(person_attribs->part_ideal_length_[(int)parent.label][child_nr] - root);
213  if(offset > person_attribs->max_length_offset_[(int)parent.label][child_nr])
214  return -1.0;
215  else
216  return offset;
217  }
218 
219  /**
220  * @brief This function evaluates an entire row of parent segments for the best child segments
221  * @param[in] sorted this is the array of all blobs
222  * @param[in] parent_label this is the part label that indicates the row
223  * @param[in] child_label this is the part label that indicates the childs needed to be investigated
224  * @param[in] child_number the number of this child in the parent, some parents have multiple childs
225  * @return zero if successful
226  * @todo once we have good evaluation function reconsider best_value
227  **/
228  inline int
229  evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
230  unsigned int parent_label,
231  int child_label,
232  int child_number)
233  {
234  // Check the input data
235  assert(parent_label < NUM_PARTS);
236  assert(child_label >= 0);
237  assert(child_number >= 0);
238  assert(child_number < MAX_CHILD);
239 
240  if(sorted[parent_label].size() == 0){
241  return 0; //if my size is 0, this is solved by my parent in his iteration
242  }
243  if(sorted[child_label].size() == 0){
244  noChildBlobVector(sorted, parent_label, child_number);
245  return 0;
246  }
247  // go over all parents in this vector
248  for(size_t p = 0; p < sorted[parent_label].size(); p++){
249  float best_value = std::numeric_limits<float>::max();
250  int best_child_id = NO_CHILD;
251  int best_child_lid = 0; // this must be as low as possible, still overruled by id
252  float value = 0.0;
253 
254  // go over all children in this vector
255  for(size_t c = 0; c < sorted[child_label].size(); c++){
256  value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number);
257  // Value should contain offset from the ideal position
258  // Is -1 if it goes above threshold
259  if(value < best_value && value != -1.0){
260  best_child_id = sorted[child_label][c].id;
261  best_child_lid = c;
262  best_value = value;
263  }
264  }
265  assert(parent_label < sorted.size());
266  assert(p < sorted[parent_label].size());
267  assert(child_label < (int) sorted.size());
268  //Set the correct child in the parent
269  sorted[parent_label][p].child_id[child_number] = best_child_id;
270  sorted[parent_label][p].child_lid[child_number] = best_child_lid;
271  sorted[parent_label][p].child_dist[child_number] = best_value;
272  sorted[parent_label][p].child_label[child_number] = child_label;
273  }
274  return 0;
275  }
276 
277  /**
278  * @brief This function evaluates an entire row of parent segments for the best child segments
279  * @param[in] sorted this is the array of all blobs
280  * @param[in] parent_label this is the part label that indicates the row
281  * @param[in] child_label this is the part label that indicates the childs needed to be investigated
282  * @param[in] child_number the number of this child in the parent, some parents have multiple childs
283  * @param person_attribs
284  * @return zero if successful
285  * @todo once we have good evaluation function reconsider best_value
286  **/
287  inline int
288  evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
289  unsigned int parent_label,
290  int child_label,
291  int child_number,
292  PersonAttribs::Ptr person_attribs)
293  {
294  // Check the input data
295  assert(parent_label < NUM_PARTS);
296  assert(child_label >= 0);
297  assert(child_number >= 0);
298  assert(child_number < MAX_CHILD);
299 
300  if(sorted[parent_label].size() == 0){
301  return 0; //if my size is 0, this is solved by my parent in his iteration
302  }
303  if(sorted[child_label].size() == 0){
304  noChildBlobVector(sorted, parent_label, child_number);
305  return 0;
306  }
307  // go over all parents in this vector
308  for(size_t p = 0; p < sorted[parent_label].size(); p++){
309  float best_value = std::numeric_limits<float>::max();
310  int best_child_id = NO_CHILD;
311  int best_child_lid = 0; // this must be as low as possible, still overruled by id
312  float value = 0.0;
313 
314  // go over all children in this vector
315  for(size_t c = 0; c < sorted[child_label].size(); c++){
316  value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number, person_attribs);
317  // Value should contain offset from the ideal position
318  // Is -1 if it goes above threshold
319  if(value < best_value && value != -1.0){
320  best_child_id = sorted[child_label][c].id;
321  best_child_lid = c;
322  best_value = value;
323  }
324  }
325  assert(parent_label < sorted.size());
326  assert(p < sorted[parent_label].size());
327  assert(child_label < (int) sorted.size());
328  //Set the correct child in the parent
329  sorted[parent_label][p].child_id[child_number] = best_child_id;
330  sorted[parent_label][p].child_lid[child_number] = best_child_lid;
331  sorted[parent_label][p].child_dist[child_number] = best_value;
332  sorted[parent_label][p].child_label[child_number] = child_label;
333  }
334  return 0;
335  }
336 
337  /**
338  * @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
339  * @param[in] sorted a matrix with all found good blobs arranged according to label and order
340  * @return zero if everything went well, negative on an error
341  * @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
342  * @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
343  */
344  inline int
345  buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted)
346  {
347  PCL_VERBOSE("[pcl::gpu::people::buildRelations] : (I) : buildRelations : regular version\n");
348  if(sorted.size() == 0){
349  std::cout << "(E) : Damn you, you gave me an empty matrix!" << std::endl;
350  return (-1);
351  }
352  // Iterate over all parts
353  for(size_t p = 0; p < sorted.size(); p ++)
354  {
355  switch(p){
356  // These are multinodes and end nodes ///
357  case Neck:
358  evaluateBlobVector(sorted, p, FaceRB, 0);
359  evaluateBlobVector(sorted, p, FaceLB, 1);
360  evaluateBlobVector(sorted, p, Rchest, 2);
361  evaluateBlobVector(sorted, p, Lchest, 3);
362  break;
363  case 0: // this is the Lfoot
364  case 4: // this is the Rfoot
365  case 14: // this is the Rhand
366  case 18: // this is the Lhand
367  case 21: // this is the FaceLT
368  case 22: // this is the FaceRT
369  leafBlobVector(sorted, p); //fill in the children of leafs
370  break;
371  case 23: // this is the Rchest
372  evaluateBlobVector(sorted, p, 11, 0); //Child 0 is Rarm
373  evaluateBlobVector(sorted, p, 8, 1); //Child 1 is Rhips
374  break;
375  case 24: // this is the Lchest
376  evaluateBlobVector(sorted, p, 15, 0); //Child 0 is Larm
377  evaluateBlobVector(sorted, p, 9, 1); //Child 1 is Lhips
378  break;
379  // FROM HERE ALL THE REGULAR MIDDLE NODES ///
380  case 1: //this is the Lleg
381  evaluateBlobVector(sorted,p, 0, 0); //Child 0 is Lfeet
382  break;
383  case 2: //this is the Lknee
384  evaluateBlobVector(sorted,p, 1, 0); //Child 0 is Lleg
385  break;
386  case 3: //this is the Lthigh
387  evaluateBlobVector(sorted,p, 2, 0); //Child 0 is Lknee
388  break;
389  case 5: //this is the Rleg
390  evaluateBlobVector(sorted,p, 4, 0); //Child Rfoot
391  break;
392  case 6: //this is the Rknee
393  evaluateBlobVector(sorted,p, 5, 0); //Child Rleg
394  break;
395  case 7: //this is the Rthigh
396  evaluateBlobVector(sorted,p, 6, 0); //Child Rknee
397  break;
398  case 8: //this is the Rhips
399  evaluateBlobVector(sorted,p, 7, 0); //Child Rthigh
400  break;
401  case 9: //this is the Lhips
402  evaluateBlobVector(sorted,p, 3, 0); //Child Lthigh
403  break;
404  case Rarm:
405  evaluateBlobVector(sorted,p, Relbow, 0);
406  if(!hasThisLabelChildren(sorted, Rarm, 0))
407  evaluateBlobVector(sorted,p, Rforearm, 0);
408  break;
409  case 12: //this is the Relbow
410  evaluateBlobVector(sorted,p, 13, 0); //Child Rforearm
411  break;
412  case 13: //this is the Rforearm
413  evaluateBlobVector(sorted,p, 14, 0); //Child Rhand
414  break;
415  case Larm:
416  evaluateBlobVector(sorted,p, Lelbow, 0);
417  if(!hasThisLabelChildren(sorted, Larm, 0))
418  evaluateBlobVector(sorted,p, Lforearm, 0);
419  break;
420  case 16: //this is the Lelbow
421  evaluateBlobVector(sorted,p, 17, 0); //Child Lforearm
422  break;
423  case 17: //this is the Lforearm
424  evaluateBlobVector(sorted,p, 18, 0); //Child Lhand
425  break;
426  case 19: //this is the FaceLB
427  evaluateBlobVector(sorted,p, 21, 0); //Child FaceLT
428  break;
429  case 20: //this is the FaceRB
430  evaluateBlobVector(sorted,p, 22, 0); //Child FaceRT
431  break;
432  default:
433  break;
434  }
435  }
436  return 0;
437  }
438 
439  /**
440  * @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
441  * @param[in] sorted a matrix with all found good blobs arranged according to label and order
442  * @param person_attribs
443  * @return zero if everything went well, negative on an error
444  * @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
445  * @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
446  */
447  inline int
448  buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted,
449  PersonAttribs::Ptr person_attribs)
450  {
451  PCL_DEBUG("[pcl::gpu::people::buildRelations] : (D) : person specific version\n");
452  if(sorted.size() == 0){
453  PCL_ERROR("[pcl::gpu::people::buildRelations] : (E) : Damn you, you gave me an empty matrix!\n");
454  return (-1);
455  }
456  // Iterate over all parts
457  for(size_t p = 0; p < sorted.size(); p ++)
458  {
459  switch(p){
460  // These are multinodes and end nodes ///
461  case Neck:
462  evaluateBlobVector(sorted, p, FaceRB, 0, person_attribs);
463  evaluateBlobVector(sorted, p, FaceLB, 1, person_attribs);
464  evaluateBlobVector(sorted, p, Rchest, 2, person_attribs);
465  evaluateBlobVector(sorted, p, Lchest, 3, person_attribs);
466  break;
467  case 0: // this is the Lfoot
468  case 4: // this is the Rfoot
469  case 14: // this is the Rhand
470  case 18: // this is the Lhand
471  case 21: // this is the FaceLT
472  case 22: // this is the FaceRT
473  leafBlobVector(sorted, p); //fill in the children of leafs
474  break;
475  case 23: // this is the Rchest
476  evaluateBlobVector(sorted, p, 11, 0, person_attribs); //Child 0 is Rarm
477  evaluateBlobVector(sorted, p, 8, 1, person_attribs); //Child 1 is Rhips
478  break;
479  case 24: // this is the Lchest
480  evaluateBlobVector(sorted, p, 15, 0, person_attribs); //Child 0 is Larm
481  evaluateBlobVector(sorted, p, 9, 1, person_attribs); //Child 1 is Lhips
482  break;
483  // FROM HERE ALL THE REGULAR MIDDLE NODES ///
484  case 1: //this is the Lleg
485  evaluateBlobVector(sorted,p, 0, 0, person_attribs); //Child 0 is Lfeet
486  break;
487  case 2: //this is the Lknee
488  evaluateBlobVector(sorted,p, 1, 0, person_attribs); //Child 0 is Lleg
489  break;
490  case 3: //this is the Lthigh
491  evaluateBlobVector(sorted,p, 2, 0, person_attribs); //Child 0 is Lknee
492  break;
493  case 5: //this is the Rleg
494  evaluateBlobVector(sorted,p, 4, 0, person_attribs); //Child Rfoot
495  break;
496  case 6: //this is the Rknee
497  evaluateBlobVector(sorted,p, 5, 0, person_attribs); //Child Rleg
498  break;
499  case 7: //this is the Rthigh
500  evaluateBlobVector(sorted,p, 6, 0, person_attribs); //Child Rknee
501  break;
502  case 8: //this is the Rhips
503  evaluateBlobVector(sorted,p, 7, 0, person_attribs); //Child Rthigh
504  break;
505  case 9: //this is the Lhips
506  evaluateBlobVector(sorted,p, 3, 0, person_attribs); //Child Lthigh
507  break;
508  case Rarm:
509  evaluateBlobVector(sorted,p, Relbow, 0, person_attribs);
510  if(!hasThisLabelChildren(sorted, Rarm, 0))
511  evaluateBlobVector(sorted,p, Rforearm, 0, person_attribs);
512  break;
513  case 12: //this is the Relbow
514  evaluateBlobVector(sorted,p, 13, 0, person_attribs); //Child Rforearm
515  break;
516  case 13: //this is the Rforearm
517  evaluateBlobVector(sorted,p, 14, 0, person_attribs); //Child Rhand
518  break;
519  case Larm:
520  evaluateBlobVector(sorted,p, Lelbow, 0, person_attribs);
521  if(!hasThisLabelChildren(sorted, Larm, 0))
522  evaluateBlobVector(sorted,p, Lforearm, 0, person_attribs);
523  break;
524  case 16: //this is the Lelbow
525  evaluateBlobVector(sorted,p, 17, 0, person_attribs); //Child Lforearm
526  break;
527  case 17: //this is the Lforearm
528  evaluateBlobVector(sorted,p, 18, 0, person_attribs); //Child Lhand
529  break;
530  case 19: //this is the FaceLB
531  evaluateBlobVector(sorted,p, 21, 0, person_attribs); //Child FaceLT
532  break;
533  case 20: //this is the FaceRB
534  evaluateBlobVector(sorted,p, 22, 0, person_attribs); //Child FaceRT
535  break;
536  default:
537  break;
538  }
539  }
540  return 0;
541  }
542 
543 
544 
545  inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
546  Tree2& tree,
547  int part_label,
548  int part_lid)
549  {
550  int nr_children = LUT_nr_children[part_label];
551  tree.nr_parts++;
552  tree.parts_lid[part_label] = part_lid;
553 
554  const Blob2& blob = sorted[part_label][part_lid];
555 
556  // iterate over the number of pixels that are part of this label
557  const std::vector<int>& indices = blob.indices.indices;
558  tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
559 
560  if(nr_children == 0)
561  return 0;
562 
563  // iterate over all possible children
564  for(int i = 0; i < nr_children; i++)
565  {
566  // check if this child has a valid child_id, leaf test should be redundant
567  if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
568  {
569  tree.total_dist_error += blob.child_dist[i];
570  browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
571  }
572  }
573  return 0;
574  }
575 
576  inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
577  Tree2& tree,
578  int part_label,
579  int part_lid,
580  PersonAttribs::Ptr person_attribs)
581  {
582  int nr_children = person_attribs->nr_of_children_[part_label];
583  tree.nr_parts++;
584  tree.parts_lid[part_label] = part_lid;
585 
586  const Blob2& blob = sorted[part_label][part_lid];
587 
588  // iterate over the number of pixels that are part of this label
589  const std::vector<int>& indices = blob.indices.indices;
590  tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
591 
592  if(nr_children == 0)
593  return 0;
594 
595  // iterate over all possible children
596  for(int i = 0; i < nr_children; i++)
597  {
598  // check if this child has a valid child_id, leaf test should be redundant
599  if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
600  {
601  tree.total_dist_error += blob.child_dist[i];
602  browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
603  }
604  }
605  return 0;
606  }
607 
608  inline int buildTree ( const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
609  const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
610  part_t part_label,
611  int part_lid,
612  Tree2& tree)
613  {
614  if(sorted.size() <= 0)
615  {
616  std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
617  return -1;
618  }
619  tree.label = part_label;
620  tree.lid = part_lid;
621  tree.total_dist_error = 0;
622  tree.nr_parts = 0;
623 
624  browseTree(sorted, tree, part_label, part_lid);
625 
626  pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
627  pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
628  pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
629 
630  pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
631 
632  tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
633 
634  return 0;
635  }
636 
637  inline int buildTree ( const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
638  const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
639  part_t part_label,
640  int part_lid,
641  Tree2& tree,
642  PersonAttribs::Ptr person_attribs)
643  {
644  if(sorted.size() <= 0)
645  {
646  std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
647  return -1;
648  }
649  tree.label = part_label;
650  tree.lid = part_lid;
651  tree.total_dist_error = 0;
652  tree.nr_parts = 0;
653 
654  browseTree(sorted, tree, part_label, part_lid, person_attribs);
655 
656  pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
657  pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
658  pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
659 
660  pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
661 
662  tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
663 
664  return 0;
665  }
666 
667  } //end namespace people
668  } // end namespace gpu
669 } // end namespace pcl
670 #endif
static const float LUT_ideal_length[][4]
This LUT contains the ideal length between this part and his children.
Definition: label_common.h:156
Eigen::Vector4f min
Definition: label_tree.h:98
Eigen::Matrix3f cov
Definition: label_tree.h:93
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
part_t
Our code is forseen to use maximal use 32 labels.
Definition: label_common.h:76
int child_id[MAX_CHILD]
Definition: label_blob2.h:70
std::vector< int > indices
Definition: PointIndices.h:19
Eigen::Matrix3f eigenvect
Definition: label_tree.h:95
int buildRelations(std::vector< std::vector< Blob2, Eigen::aligned_allocator< pcl::gpu::people::Blob2 > > > &sorted)
This function goes over the sorted matrix and fills in the optimal parent and child relations...
Definition: label_tree.h:345
char child_label[MAX_CHILD]
Definition: label_blob2.h:73
Eigen::Vector3f eigenval
Definition: label_tree.h:94
std::ostream & operator<<(std::ostream &os, const Blob2 &b)
Definition: label_blob2.h:80
pcl::PointIndices indices
Definition: label_blob2.h:75
int noChildBlobVector(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, int label, int child_number)
This function sets the specific child of the vector to no child, meaning that there are no such child...
Definition: label_tree.h:144
bool hasThisLabelChildren(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, part_t label, int child_number)
This function test if children were found for this label.
Definition: label_tree.h:161
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 parts_lid[NUM_PARTS]
Definition: label_tree.h:88
int browseTree(const std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, Tree2 &tree, int part_label, int part_lid)
Definition: label_tree.h:545
int child_lid[MAX_CHILD]
Definition: label_blob2.h:71
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
static const float LUT_max_length_offset[][4]
This LUT contains the max length between this part and his children.
Definition: label_common.h:188
int leafBlobVector(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, int label)
This function sets the children of the leaf nodes to leaf, meaning that we came to the correct end...
Definition: label_tree.h:123
int buildTree(const std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, const pcl::PointCloud< pcl::PointXYZ > &cloud_in, part_t part_label, int part_lid, Tree2 &tree)
Definition: label_tree.h:608
static const unsigned int LUT_nr_children[]
This LUT contains the number of children for each parent.
Definition: label_common.h:220
float evaluateBlobs(Blob2 &parent, Blob2 &child, int child_nr)
This is the evaluation function used to compare two blobs.
Definition: label_tree.h:182
Eigen::Vector4f mean
Definition: label_tree.h:92
pcl::PointIndices indices
Definition: label_tree.h:97
Eigen::Vector4f max
Definition: label_tree.h:99
boost::shared_ptr< PersonAttribs > Ptr
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
int evaluateBlobVector(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, unsigned int parent_label, int child_label, int child_number)
This function evaluates an entire row of parent segments for the best child segments.
Definition: label_tree.h:229
float child_dist[MAX_CHILD]
Definition: label_blob2.h:72
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
This structure contains all parameters to describe the segmented tree.
Definition: label_tree.h:75
Eigen::Vector4f mean
Definition: label_blob2.h:64
This structure contains all parameters to describe blobs and their parent/child relations.
Definition: label_blob2.h:58