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