Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
65namespace 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 &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
203 Blob2& child,
204 int child_nr,
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);
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 ()){
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();
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;
259 best_value = value;
260 }
261 }
263 assert(p < sorted[parent_label].size());
264 assert(child_label < (int) sorted.size());
265 //Set the correct child in the parent
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,
290 {
291 // Check the input data
292 assert(parent_label < NUM_PARTS);
293 assert(child_label >= 0);
294 assert(child_number >= 0);
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 ()){
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();
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++){
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;
319 best_value = value;
320 }
321 }
323 assert(p < sorted[parent_label].size());
324 assert(child_label < (int) sorted.size());
325 //Set the correct child in the parent
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:
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:
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:
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,
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:
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:
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:
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 {
548 tree.nr_parts++;
550
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,
578 {
579 int nr_children = person_attribs->nr_of_children_[part_label];
580 tree.nr_parts++;
582
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,
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
622
623 pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
626
627 pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
628
630
631 return 0;
632 }
633
634 inline int buildTree ( const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
637 int part_lid,
638 Tree2& tree,
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
652
653 pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
656
657 pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
658
660
661 return 0;
662 }
663
664 } //end namespace people
665 } // end namespace gpu
666} // end namespace pcl
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PersonAttribs > Ptr
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:251
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition eigen.hpp:296
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:56
float evaluateBlobs(Blob2 &parent, Blob2 &child, int child_nr)
This is the evaluation function used to compare two blobs.
Definition label_tree.h:181
static const float LUT_max_length_offset[][4]
This LUT contains the max length between this part and his children.
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:605
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
static const unsigned int LUT_nr_children[]
This LUT contains the number of children for each parent.
static const float LUT_ideal_length[][4]
This LUT contains the ideal length between this part and his children.
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
std::ostream & operator<<(std::ostream &os, const Blob2 &b)
Definition label_blob2.h:79
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
part_t
Our code is forseen to use maximal use 32 labels.
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
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
This structure contains all parameters to describe blobs and their parent/child relations.
Definition label_blob2.h:58
Eigen::Vector4f mean
Definition label_blob2.h:63
This structure contains all parameters to describe the segmented tree.
Definition label_tree.h:75
Eigen::Matrix3f cov
Definition label_tree.h:92
Eigen::Matrix3f eigenvect
Definition label_tree.h:94
Eigen::Vector3f eigenval
Definition label_tree.h:93
pcl::PointIndices indices
Definition label_tree.h:96
Eigen::Vector4f mean
Definition label_tree.h:91
Eigen::Vector4f max
Definition label_tree.h:98
Eigen::Vector4f min
Definition label_tree.h:97
int parts_lid[NUM_PARTS]
Definition label_tree.h:87