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