45 #include "pcl/gpu/people/label_blob2.h"
46 #include "pcl/gpu/people/label_common.h"
47 #include "pcl/gpu/people/person_attribs.h"
56 #include <pcl/conversions.h>
57 #include <pcl/point_cloud.h>
59 #include <pcl/console/print.h>
61 #include <pcl/common/eigen.h>
103 os <<
" Tree2 id " << t.
id <<
" label " << t.
label <<
" lid " << t.
lid <<
" nr_parts " << t.
nr_parts << 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;
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;
125 if(sorted[label].empty ())
127 for(
auto &blob : sorted[label])
130 blob.child_id[j] =
LEAF;
147 if(sorted[label].empty ())
149 for(
auto &blob : sorted[label]){
150 blob.child_id[child_number] =
NO_CHILD;
164 if(sorted[label].empty ())
166 for(
const auto &blob : sorted[label])
167 if((blob.child_id[child_number] !=
NO_CHILD) && (blob.child_id[child_number] !=
LEAF))
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));
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])
227 unsigned int parent_label,
233 assert(child_label >= 0);
234 assert(child_number >= 0);
237 if(sorted[parent_label].empty ()){
240 if(sorted[child_label].empty ()){
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;
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);
256 if(value < best_value && value != -1.0){
257 best_child_id = sorted[child_label][c].id;
262 assert(parent_label < sorted.size());
263 assert(p < sorted[parent_label].size());
264 assert(child_label < (
int) sorted.size());
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;
286 unsigned int parent_label,
293 assert(child_label >= 0);
294 assert(child_number >= 0);
297 if(sorted[parent_label].empty ()){
300 if(sorted[child_label].empty ()){
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;
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);
316 if(value < best_value && value != -1.0){
317 best_child_id = sorted[child_label][c].id;
322 assert(parent_label < sorted.size());
323 assert(p < sorted[parent_label].size());
324 assert(child_label < (
int) sorted.size());
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;
342 buildRelations( std::vector<std::vector<
Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted)
344 PCL_VERBOSE(
"[pcl::gpu::people::buildRelations] : (I) : buildRelations : regular version\n");
346 std::cout <<
"(E) : Damn you, you gave me an empty matrix!" << std::endl;
350 for(std::size_t p = 0; p < sorted.size(); p ++)
445 buildRelations( std::vector<std::vector<
Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted,
448 PCL_DEBUG(
"[pcl::gpu::people::buildRelations] : (D) : person specific version\n");
450 PCL_ERROR(
"[pcl::gpu::people::buildRelations] : (E) : Damn you, you gave me an empty matrix!\n");
454 for(std::size_t p = 0; p < sorted.size(); p ++)
542 inline int browseTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
551 const Blob2& blob = sorted[part_label][part_lid];
561 for(
int i = 0; i < nr_children; i++)
573 inline int browseTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
579 int nr_children = person_attribs->nr_of_children_[part_label];
583 const Blob2& blob = sorted[part_label][part_lid];
593 for(
int i = 0; i < nr_children; i++)
605 inline int buildTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
613 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
616 tree.
label = part_label;
621 browseTree(sorted, tree, part_label, part_lid);
634 inline int buildTree (
const std::vector<std::vector <
Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
643 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
646 tree.
label = part_label;
651 browseTree(sorted, tree, part_label, part_lid, person_attribs);