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++){
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);
static const float LUT_ideal_length[][4]
This LUT contains the ideal length between this part and his children.
This file defines compatibility wrappers for low level I/O functions.
part_t
Our code is forseen to use maximal use 32 labels.
std::vector< int > indices
Eigen::Matrix3f eigenvect
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...
Define standard C methods and C++ classes that are common to all methods.
char child_label[MAX_CHILD]
std::ostream & operator<<(std::ostream &os, const Blob2 &b)
pcl::PointIndices indices
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...
Defines all the PCL implemented PointT point type structures.
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.
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...
int browseTree(const std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, Tree2 &tree, int part_label, int part_lid)
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...
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...
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)
static const unsigned int LUT_nr_children[]
This LUT contains the number of children for each parent.
float evaluateBlobs(Blob2 &parent, Blob2 &child, int child_nr)
This is the evaluation function used to compare two blobs.
pcl::PointIndices indices
boost::shared_ptr< PersonAttribs > Ptr
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
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.
float child_dist[MAX_CHILD]
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
This structure contains all parameters to describe the segmented tree.
Define methods for centroid estimation and covariance matrix calculus.
This structure contains all parameters to describe blobs and their parent/child relations.