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;
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;
125 if(
sorted[label].empty ())
147 if(
sorted[label].empty ())
164 if(
sorted[label].empty ())
240 if(
sorted[child_label].empty ()){
246 float best_value = std::numeric_limits<float>::max();
300 if(
sorted[child_label].empty ()){
306 float best_value = std::numeric_limits<float>::max();
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 ++)
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 ++)
554 const auto& indices =
blob.indices.indices;
586 const auto& indices =
blob.indices.indices;
613 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
643 std::cout <<
"(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
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.
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.
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...
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.
float evaluateBlobs(Blob2 &parent, Blob2 &child, int child_nr)
This is the evaluation function used to compare two blobs.
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)
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...
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.
std::ostream & operator<<(std::ostream &os, const Blob2 &b)
int browseTree(const std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, Tree2 &tree, int part_label, int part_lid)
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.
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.
This structure contains all parameters to describe blobs and their parent/child relations.
This structure contains all parameters to describe the segmented tree.
Eigen::Matrix3f eigenvect
pcl::PointIndices indices