42 #ifndef PCL_GPU_PEOPLE_LABEL_TREE_H_
43 #define PCL_GPU_PEOPLE_LABEL_TREE_H_
46 #include "pcl/gpu/people/label_blob2.h"
47 #include "pcl/gpu/people/label_common.h"
48 #include "pcl/gpu/people/person_attribs.h"
57 #include <pcl/conversions.h>
58 #include <pcl/point_cloud.h>
60 #include <pcl/console/print.h>
62 #include <pcl/common/eigen.h>
104 os <<
" Tree2 id " << t.
id <<
" label " << t.
label <<
" lid " << t.
lid <<
" nr_parts " << t.
nr_parts << 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;
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;
126 if(sorted[label].size() == 0)
128 for(
size_t i = 0; i < sorted[label].size(); i++)
131 sorted[label][i].child_id[j] =
LEAF;
148 if(sorted[label].size() == 0)
150 for(
size_t i = 0; i < sorted[label].size(); i++){
151 sorted[label][i].child_id[child_number] =
NO_CHILD;
165 if(sorted[label].size() == 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))
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));
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])
230 unsigned int parent_label,
236 assert(child_label >= 0);
237 assert(child_number >= 0);
240 if(sorted[parent_label].size() == 0){
243 if(sorted[child_label].size() == 0){
248 for(
size_t p = 0; p < sorted[parent_label].size(); p++){
249 float best_value = std::numeric_limits<float>::max();
251 int best_child_lid = 0;
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);
259 if(value < best_value && value != -1.0){
260 best_child_id = sorted[child_label][c].id;
265 assert(parent_label < sorted.size());
266 assert(p < sorted[parent_label].size());
267 assert(child_label < (
int) sorted.size());
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;
289 unsigned int parent_label,
296 assert(child_label >= 0);
297 assert(child_number >= 0);
300 if(sorted[parent_label].size() == 0){
303 if(sorted[child_label].size() == 0){
308 for(
size_t p = 0; p < sorted[parent_label].size(); p++){
309 float best_value = std::numeric_limits<float>::max();
311 int best_child_lid = 0;
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);
319 if(value < best_value && value != -1.0){
320 best_child_id = sorted[child_label][c].id;
325 assert(parent_label < sorted.size());
326 assert(p < sorted[parent_label].size());
327 assert(child_label < (
int) sorted.size());
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;
345 buildRelations( std::vector<std::vector<
Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted)
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;
353 for(
size_t p = 0; p < sorted.size(); p ++)
448 buildRelations( std::vector<std::vector<
Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted,
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");
457 for(
size_t p = 0; p < sorted.size(); p ++)
545 inline int browseTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
554 const Blob2& blob = sorted[part_label][part_lid];
564 for(
int i = 0; i < nr_children; i++)
576 inline int browseTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
582 int nr_children = person_attribs->nr_of_children_[part_label];
586 const Blob2& blob = sorted[part_label][part_lid];
596 for(
int i = 0; i < nr_children; i++)
608 inline int buildTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
614 if(sorted.size() <= 0)
616 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
619 tree.
label = part_label;
624 browseTree(sorted, tree, part_label, part_lid);
637 inline int buildTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
644 if(sorted.size() <= 0)
646 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
649 tree.
label = part_label;
654 browseTree(sorted, tree, part_label, part_lid, person_attribs);