Loading...
Searching...
No Matches
label_tree.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id: $
* @author Koen Buys
* @file tree.h
* @brief This file contains the function prototypes for the tree building functions.
*/
#pragma once
// our headers
#include "pcl/gpu/people/label_blob2.h" //this one defines the blob structure
#include "pcl/gpu/people/label_common.h" //this one defines the LUT's
#include "pcl/gpu/people/person_attribs.h"
// std
#include <vector>
#include <iostream>
#include <algorithm>
#include <stdexcept>
// PCL specific includes
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
namespace pcl
{
namespace gpu
{
namespace people
{
/**
* @brief This structure contains all parameters to describe the segmented tree
*/
struct Tree2
{
//Inline constructor
Tree2() : id(NO_CHILD), lid(NO_CHILD), nr_parts(0)
{
for(int &part : parts_lid)
part = NO_CHILD;
}
int id; // specific identification number of this tree
part_t label; // labels which part the root of this tree is
int lid; // label id, which number of this type of part is this
int nr_parts; // the number of parts in this tree
int parts_lid[NUM_PARTS]; // Indicate the used parts
float total_dist_error; // sum of all distance errors
float norm_dist_error; // total_dist_error/nr_parts
Eigen::Vector4f mean; // mean in xyz
Eigen::Matrix3f cov; // covariance in 3x3 matrix
Eigen::Vector3f eigenval; // eigenvalue of blob
Eigen::Matrix3f eigenvect; // eigenvector of blob
pcl::PointIndices indices; // The indices of the pointcloud
Eigen::Vector4f min; // The min of the bounding box
Eigen::Vector4f max; // The max of the bounding box
};
inline std::ostream& operator << (std::ostream& os, const Tree2& t)
{
os << " Tree2 id " << t.id << " label " << t.label << " lid " << t.lid << " nr_parts " << t.nr_parts << std::endl;
os << " total_dist_error " << t.total_dist_error << " norm_dist_error " << t.norm_dist_error << std::endl;
os << " mean " << t.mean(0) << " , " << t.mean(1) << " , " << t.mean(2) << " , " << t.mean(3) << std::endl;
os << " cov " << std::endl << t.cov << std::endl;
os << " eigenval " << t.eigenval(0) << " , " << t.eigenval(1) << " , " << t.eigenval(2) << std::endl;
os << " eigenvect " << std::endl << t.eigenvect << std::endl;
os << " min " << t.min(0) << " , " << t.min(1) << " , " << t.min(2) << " , " << t.min(3) << std::endl;
os << " max " << t.max(0) << " , " << t.max(1) << " , " << t.max(2) << " , " << t.max(3) << std::endl;
os << " indices length " << t.indices.indices.size() << std::endl;
return (os);
}
/**
* @brief This function sets the children of the leaf nodes to leaf, meaning that we came to the correct end
* @param[in] sorted The matrix of all blobs
* @param[in] label The label of which all children are to be set as leafs
* @return Zero if everything went well
**/
inline int
leafBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
int label )
{
if(sorted[label].empty ())
return 0;
for(auto &blob : sorted[label])
{
for(int j = 0; j < MAX_CHILD; j++)
blob.child_id[j] = LEAF;
}
return 0;
}
/**
* @brief This function sets the specific child of the vector to no child, meaning that there are no such children
* @param[in] sorted The matrix of all blobs
* @param[in] label The label of which the child must be set to NO_CHILD
* @param[in] child_number The index of the respective child that must be set
* @return Zero if everything went well
**/
inline int
noChildBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
int label,
int child_number)
{
if(sorted[label].empty ())
return 0;
for(auto &blob : sorted[label]){
blob.child_id[child_number] = NO_CHILD;
}
return 0;
}
/**
* @brief This function test if children were found for this label
* @return True if this label has valid children
**/
inline bool
hasThisLabelChildren ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
part_t label,
int child_number)
{
if(sorted[label].empty ())
return false;
for(const auto &blob : sorted[label])
if((blob.child_id[child_number] != NO_CHILD) && (blob.child_id[child_number] != LEAF))
return true;
return false;
}
/**
* @brief This is the evaluation function used to compare two blobs
* @param[in] parent pointer to the parent blob
* @param[in] child pointer to the child blob
* @param[in] child_nr the number of the child
* @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
* @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
**/
inline float
evaluateBlobs (Blob2& parent, Blob2& child, int child_nr)
{
float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
pow(parent.mean(1) - child.mean(1), 2) +
pow(parent.mean(2) - child.mean(2), 2));
float offset = std::fabs(LUT_ideal_length[(int)parent.label][child_nr] - root);
if(offset > LUT_max_length_offset[(int)parent.label][child_nr])
return -1.0;
return offset;
}
/**
* @brief This is the evaluation function used to compare two blobs
* @param[in] parent pointer to the parent blob
* @param[in] child pointer to the child blob
* @param[in] child_nr the number of the child
* @param person_attribs
* @return it returns the distance error from the ideal parent child distance, it returns -1.0 if it goes over threshold
* @todo what if child is second link in stead of first link (ea forearm in stead of elbow for arm)
**/
inline float
evaluateBlobs (Blob2& parent,
Blob2& child,
int child_nr,
PersonAttribs::Ptr person_attribs)
{
float root = sqrt(pow(parent.mean(0) - child.mean(0), 2) +
pow(parent.mean(1) - child.mean(1), 2) +
pow(parent.mean(2) - child.mean(2), 2));
float offset = std::fabs(person_attribs->part_ideal_length_[(int)parent.label][child_nr] - root);
if(offset > person_attribs->max_length_offset_[(int)parent.label][child_nr])
return -1.0;
return offset;
}
/**
* @brief This function evaluates an entire row of parent segments for the best child segments
* @param[in] sorted this is the array of all blobs
* @param[in] parent_label this is the part label that indicates the row
* @param[in] child_label this is the part label that indicates the childs needed to be investigated
* @param[in] child_number the number of this child in the parent, some parents have multiple childs
* @return zero if successful
* @todo once we have good evaluation function reconsider best_value
**/
inline int
evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
unsigned int parent_label,
int child_label,
int child_number)
{
// Check the input data
assert(parent_label < NUM_PARTS);
assert(child_label >= 0);
assert(child_number >= 0);
assert(child_number < MAX_CHILD);
if(sorted[parent_label].empty ()){
return 0; //if my size is 0, this is solved by my parent in his iteration
}
if(sorted[child_label].empty ()){
noChildBlobVector(sorted, parent_label, child_number);
return 0;
}
// go over all parents in this vector
for(std::size_t p = 0; p < sorted[parent_label].size(); p++){
float best_value = std::numeric_limits<float>::max();
int best_child_id = NO_CHILD;
int best_child_lid = 0; // this must be as low as possible, still overruled by id
float value = 0.0;
// go over all children in this vector
for(std::size_t c = 0; c < sorted[child_label].size(); c++){
value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number);
// Value should contain offset from the ideal position
// Is -1 if it goes above threshold
if(value < best_value && value != -1.0){
best_child_id = sorted[child_label][c].id;
best_child_lid = c;
best_value = value;
}
}
assert(parent_label < sorted.size());
assert(p < sorted[parent_label].size());
assert(child_label < (int) sorted.size());
//Set the correct child in the parent
sorted[parent_label][p].child_id[child_number] = best_child_id;
sorted[parent_label][p].child_lid[child_number] = best_child_lid;
sorted[parent_label][p].child_dist[child_number] = best_value;
sorted[parent_label][p].child_label[child_number] = child_label;
}
return 0;
}
/**
* @brief This function evaluates an entire row of parent segments for the best child segments
* @param[in] sorted this is the array of all blobs
* @param[in] parent_label this is the part label that indicates the row
* @param[in] child_label this is the part label that indicates the childs needed to be investigated
* @param[in] child_number the number of this child in the parent, some parents have multiple childs
* @param person_attribs
* @return zero if successful
* @todo once we have good evaluation function reconsider best_value
**/
inline int
evaluateBlobVector( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
unsigned int parent_label,
int child_label,
int child_number,
PersonAttribs::Ptr person_attribs)
{
// Check the input data
assert(parent_label < NUM_PARTS);
assert(child_label >= 0);
assert(child_number >= 0);
assert(child_number < MAX_CHILD);
if(sorted[parent_label].empty ()){
return 0; //if my size is 0, this is solved by my parent in his iteration
}
if(sorted[child_label].empty ()){
noChildBlobVector(sorted, parent_label, child_number);
return 0;
}
// go over all parents in this vector
for(std::size_t p = 0; p < sorted[parent_label].size(); p++){
float best_value = std::numeric_limits<float>::max();
int best_child_id = NO_CHILD;
int best_child_lid = 0; // this must be as low as possible, still overruled by id
float value = 0.0;
// go over all children in this vector
for(std::size_t c = 0; c < sorted[child_label].size(); c++){
value = evaluateBlobs(sorted[parent_label][p], sorted[child_label][c], child_number, person_attribs);
// Value should contain offset from the ideal position
// Is -1 if it goes above threshold
if(value < best_value && value != -1.0){
best_child_id = sorted[child_label][c].id;
best_child_lid = c;
best_value = value;
}
}
assert(parent_label < sorted.size());
assert(p < sorted[parent_label].size());
assert(child_label < (int) sorted.size());
//Set the correct child in the parent
sorted[parent_label][p].child_id[child_number] = best_child_id;
sorted[parent_label][p].child_lid[child_number] = best_child_lid;
sorted[parent_label][p].child_dist[child_number] = best_value;
sorted[parent_label][p].child_label[child_number] = child_label;
}
return 0;
}
/**
* @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
* @param[in] sorted a matrix with all found good blobs arranged according to label and order
* @return zero if everything went well, negative on an error
* @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
* @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
*/
inline int
buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted)
{
PCL_VERBOSE("[pcl::gpu::people::buildRelations] : (I) : buildRelations : regular version\n");
if(sorted.empty ()){
std::cout << "(E) : Damn you, you gave me an empty matrix!" << std::endl;
return (-1);
}
// Iterate over all parts
for(std::size_t p = 0; p < sorted.size(); p ++)
{
switch(p){
// These are multinodes and end nodes ///
case Neck:
evaluateBlobVector(sorted, p, FaceRB, 0);
evaluateBlobVector(sorted, p, FaceLB, 1);
evaluateBlobVector(sorted, p, Rchest, 2);
evaluateBlobVector(sorted, p, Lchest, 3);
break;
case 0: // this is the Lfoot
case 4: // this is the Rfoot
case 14: // this is the Rhand
case 18: // this is the Lhand
case 21: // this is the FaceLT
case 22: // this is the FaceRT
leafBlobVector(sorted, p); //fill in the children of leafs
break;
case 23: // this is the Rchest
evaluateBlobVector(sorted, p, 11, 0); //Child 0 is Rarm
evaluateBlobVector(sorted, p, 8, 1); //Child 1 is Rhips
break;
case 24: // this is the Lchest
evaluateBlobVector(sorted, p, 15, 0); //Child 0 is Larm
evaluateBlobVector(sorted, p, 9, 1); //Child 1 is Lhips
break;
// FROM HERE ALL THE REGULAR MIDDLE NODES ///
case 1: //this is the Lleg
evaluateBlobVector(sorted,p, 0, 0); //Child 0 is Lfeet
break;
case 2: //this is the Lknee
evaluateBlobVector(sorted,p, 1, 0); //Child 0 is Lleg
break;
case 3: //this is the Lthigh
evaluateBlobVector(sorted,p, 2, 0); //Child 0 is Lknee
break;
case 5: //this is the Rleg
evaluateBlobVector(sorted,p, 4, 0); //Child Rfoot
break;
case 6: //this is the Rknee
evaluateBlobVector(sorted,p, 5, 0); //Child Rleg
break;
case 7: //this is the Rthigh
evaluateBlobVector(sorted,p, 6, 0); //Child Rknee
break;
case 8: //this is the Rhips
evaluateBlobVector(sorted,p, 7, 0); //Child Rthigh
break;
case 9: //this is the Lhips
evaluateBlobVector(sorted,p, 3, 0); //Child Lthigh
break;
case Rarm:
evaluateBlobVector(sorted,p, Relbow, 0);
if(!hasThisLabelChildren(sorted, Rarm, 0))
evaluateBlobVector(sorted,p, Rforearm, 0);
break;
case 12: //this is the Relbow
evaluateBlobVector(sorted,p, 13, 0); //Child Rforearm
break;
case 13: //this is the Rforearm
evaluateBlobVector(sorted,p, 14, 0); //Child Rhand
break;
case Larm:
evaluateBlobVector(sorted,p, Lelbow, 0);
if(!hasThisLabelChildren(sorted, Larm, 0))
evaluateBlobVector(sorted,p, Lforearm, 0);
break;
case 16: //this is the Lelbow
evaluateBlobVector(sorted,p, 17, 0); //Child Lforearm
break;
case 17: //this is the Lforearm
evaluateBlobVector(sorted,p, 18, 0); //Child Lhand
break;
case 19: //this is the FaceLB
evaluateBlobVector(sorted,p, 21, 0); //Child FaceLT
break;
case 20: //this is the FaceRB
evaluateBlobVector(sorted,p, 22, 0); //Child FaceRT
break;
default:
break;
}
}
return 0;
}
/**
* @brief This function goes over the sorted matrix and fills in the optimal parent and child relations
* @param[in] sorted a matrix with all found good blobs arranged according to label and order
* @param person_attribs
* @return zero if everything went well, negative on an error
* @todo This function also fixes the kinematic chain, we should implement this in a xml or LUT
* @todo look if we can't get a more efficient implementation (iterator together with sortBlobs perhaps?)
*/
inline int
buildRelations( std::vector<std::vector<Blob2, Eigen::aligned_allocator<pcl::gpu::people::Blob2> > >& sorted,
PersonAttribs::Ptr person_attribs)
{
PCL_DEBUG("[pcl::gpu::people::buildRelations] : (D) : person specific version\n");
if(sorted.empty ()){
PCL_ERROR("[pcl::gpu::people::buildRelations] : (E) : Damn you, you gave me an empty matrix!\n");
return (-1);
}
// Iterate over all parts
for(std::size_t p = 0; p < sorted.size(); p ++)
{
switch(p){
// These are multinodes and end nodes ///
case Neck:
evaluateBlobVector(sorted, p, FaceRB, 0, person_attribs);
evaluateBlobVector(sorted, p, FaceLB, 1, person_attribs);
evaluateBlobVector(sorted, p, Rchest, 2, person_attribs);
evaluateBlobVector(sorted, p, Lchest, 3, person_attribs);
break;
case 0: // this is the Lfoot
case 4: // this is the Rfoot
case 14: // this is the Rhand
case 18: // this is the Lhand
case 21: // this is the FaceLT
case 22: // this is the FaceRT
leafBlobVector(sorted, p); //fill in the children of leafs
break;
case 23: // this is the Rchest
evaluateBlobVector(sorted, p, 11, 0, person_attribs); //Child 0 is Rarm
evaluateBlobVector(sorted, p, 8, 1, person_attribs); //Child 1 is Rhips
break;
case 24: // this is the Lchest
evaluateBlobVector(sorted, p, 15, 0, person_attribs); //Child 0 is Larm
evaluateBlobVector(sorted, p, 9, 1, person_attribs); //Child 1 is Lhips
break;
// FROM HERE ALL THE REGULAR MIDDLE NODES ///
case 1: //this is the Lleg
evaluateBlobVector(sorted,p, 0, 0, person_attribs); //Child 0 is Lfeet
break;
case 2: //this is the Lknee
evaluateBlobVector(sorted,p, 1, 0, person_attribs); //Child 0 is Lleg
break;
case 3: //this is the Lthigh
evaluateBlobVector(sorted,p, 2, 0, person_attribs); //Child 0 is Lknee
break;
case 5: //this is the Rleg
evaluateBlobVector(sorted,p, 4, 0, person_attribs); //Child Rfoot
break;
case 6: //this is the Rknee
evaluateBlobVector(sorted,p, 5, 0, person_attribs); //Child Rleg
break;
case 7: //this is the Rthigh
evaluateBlobVector(sorted,p, 6, 0, person_attribs); //Child Rknee
break;
case 8: //this is the Rhips
evaluateBlobVector(sorted,p, 7, 0, person_attribs); //Child Rthigh
break;
case 9: //this is the Lhips
evaluateBlobVector(sorted,p, 3, 0, person_attribs); //Child Lthigh
break;
case Rarm:
evaluateBlobVector(sorted,p, Relbow, 0, person_attribs);
if(!hasThisLabelChildren(sorted, Rarm, 0))
evaluateBlobVector(sorted,p, Rforearm, 0, person_attribs);
break;
case 12: //this is the Relbow
evaluateBlobVector(sorted,p, 13, 0, person_attribs); //Child Rforearm
break;
case 13: //this is the Rforearm
evaluateBlobVector(sorted,p, 14, 0, person_attribs); //Child Rhand
break;
case Larm:
evaluateBlobVector(sorted,p, Lelbow, 0, person_attribs);
if(!hasThisLabelChildren(sorted, Larm, 0))
evaluateBlobVector(sorted,p, Lforearm, 0, person_attribs);
break;
case 16: //this is the Lelbow
evaluateBlobVector(sorted,p, 17, 0, person_attribs); //Child Lforearm
break;
case 17: //this is the Lforearm
evaluateBlobVector(sorted,p, 18, 0, person_attribs); //Child Lhand
break;
case 19: //this is the FaceLB
evaluateBlobVector(sorted,p, 21, 0, person_attribs); //Child FaceLT
break;
case 20: //this is the FaceRB
evaluateBlobVector(sorted,p, 22, 0, person_attribs); //Child FaceRT
break;
default:
break;
}
}
return 0;
}
inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
Tree2& tree,
int part_label,
int part_lid)
{
int nr_children = LUT_nr_children[part_label];
tree.nr_parts++;
tree.parts_lid[part_label] = part_lid;
const Blob2& blob = sorted[part_label][part_lid];
// iterate over the number of pixels that are part of this label
const auto& indices = blob.indices.indices;
tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
if(nr_children == 0)
return 0;
// iterate over all possible children
for(int i = 0; i < nr_children; i++)
{
// check if this child has a valid child_id, leaf test should be redundant
if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
{
tree.total_dist_error += blob.child_dist[i];
browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
}
}
return 0;
}
inline int browseTree (const std::vector<std::vector <Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
Tree2& tree,
int part_label,
int part_lid,
PersonAttribs::Ptr person_attribs)
{
int nr_children = person_attribs->nr_of_children_[part_label];
tree.nr_parts++;
tree.parts_lid[part_label] = part_lid;
const Blob2& blob = sorted[part_label][part_lid];
// iterate over the number of pixels that are part of this label
const auto& indices = blob.indices.indices;
tree.indices.indices.insert(tree.indices.indices.end(), indices.begin(), indices.end());
if(nr_children == 0)
return 0;
// iterate over all possible children
for(int i = 0; i < nr_children; i++)
{
// check if this child has a valid child_id, leaf test should be redundant
if(blob.child_id[i] != NO_CHILD && blob.child_id[i] != LEAF)
{
tree.total_dist_error += blob.child_dist[i];
browseTree( sorted, tree, blob.child_label[i], blob.child_lid[i]);
}
}
return 0;
}
inline 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)
{
if(sorted.empty ())
{
std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
return -1;
}
tree.label = part_label;
tree.lid = part_lid;
tree.total_dist_error = 0;
tree.nr_parts = 0;
browseTree(sorted, tree, part_label, part_lid);
pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
return 0;
}
inline 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,
PersonAttribs::Ptr person_attribs)
{
if(sorted.empty ())
{
std::cout << "(E) : buildTree(): hey man, don't fool me, you gave me an empty blob matrix" << std::endl;
return -1;
}
tree.label = part_label;
tree.lid = part_lid;
tree.total_dist_error = 0;
tree.nr_parts = 0;
browseTree(sorted, tree, part_label, part_lid, person_attribs);
pcl::getMinMax3D(cloud_in, tree.indices, tree.min, tree.max);
pcl::compute3DCentroid(cloud_in, tree.indices, tree.mean);
pcl::computeCovarianceMatrixNormalized(cloud_in, tree.indices, tree.mean, tree.cov);
pcl::eigen33(tree.cov, tree.eigenvect, tree.eigenval);
tree.norm_dist_error = tree.total_dist_error/tree.nr_parts;
return 0;
}
} //end namespace people
} // end namespace gpu
} // end namespace pcl
centroid.h
Define methods for centroid estimation and covariance matrix calculus.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::gpu::people::PersonAttribs::Ptr
shared_ptr< PersonAttribs > Ptr
Definition person_attribs.h:19
common.h
Define standard C methods and C++ classes that are common to all methods.
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::getMinMax3D
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
pcl::computeCovarianceMatrixNormalized
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:269
pcl::eigen33
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:295
pcl::compute3DCentroid
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:57
pcl::gpu::people
Definition bodyparts_detector.h:63
pcl::gpu::people::MAX_CHILD
@ MAX_CHILD
Definition label_common.h:61
pcl::gpu::people::NUM_PARTS
@ NUM_PARTS
Definition label_common.h:60
pcl::gpu::people::evaluateBlobs
float evaluateBlobs(Blob2 &parent, Blob2 &child, int child_nr)
This is the evaluation function used to compare two blobs.
Definition label_tree.h:181
pcl::gpu::people::LUT_max_length_offset
static const float LUT_max_length_offset[][4]
This LUT contains the max length between this part and his children.
Definition label_common.h:187
pcl::gpu::people::leafBlobVector
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
pcl::gpu::people::buildTree
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
pcl::gpu::people::noChildBlobVector
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
pcl::gpu::people::LUT_nr_children
static const unsigned int LUT_nr_children[]
This LUT contains the number of children for each parent.
Definition label_common.h:219
pcl::gpu::people::LEAF
@ LEAF
Definition label_common.h:55
pcl::gpu::people::NO_CHILD
@ NO_CHILD
Definition label_common.h:54
pcl::gpu::people::LUT_ideal_length
static const float LUT_ideal_length[][4]
This LUT contains the ideal length between this part and his children.
Definition label_common.h:155
pcl::gpu::people::evaluateBlobVector
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
pcl::gpu::people::operator<<
std::ostream & operator<<(std::ostream &os, const Blob2 &b)
Definition label_blob2.h:79
pcl::gpu::people::browseTree
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
pcl::gpu::people::part_t
part_t
Our code is foreseen to use maximal use 32 labels.
Definition label_common.h:76
pcl::gpu::people::Rarm
@ Rarm
Definition label_common.h:88
pcl::gpu::people::Lchest
@ Lchest
Definition label_common.h:101
pcl::gpu::people::FaceRB
@ FaceRB
Definition label_common.h:97
pcl::gpu::people::Rforearm
@ Rforearm
Definition label_common.h:90
pcl::gpu::people::Rchest
@ Rchest
Definition label_common.h:100
pcl::gpu::people::Larm
@ Larm
Definition label_common.h:92
pcl::gpu::people::Neck
@ Neck
Definition label_common.h:87
pcl::gpu::people::FaceLB
@ FaceLB
Definition label_common.h:96
pcl::gpu::people::Lforearm
@ Lforearm
Definition label_common.h:94
pcl::gpu::people::Lelbow
@ Lelbow
Definition label_common.h:93
pcl::gpu::people::Relbow
@ Relbow
Definition label_common.h:89
pcl::gpu::people::buildRelations
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
pcl::gpu::people::hasThisLabelChildren
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
pcl::gpu
Definition device_array.h:45
pcl
Definition convolution.h:46
pcl::PointIndices
Definition PointIndices.h:12
pcl::PointIndices::indices
Indices indices
Definition PointIndices.h:20
pcl::gpu::people::Blob2
This structure contains all parameters to describe blobs and their parent/child relations.
Definition label_blob2.h:58
pcl::gpu::people::Blob2::child_label
char child_label[MAX_CHILD]
Definition label_blob2.h:72
pcl::gpu::people::Blob2::mean
Eigen::Vector4f mean
Definition label_blob2.h:63
pcl::gpu::people::Blob2::label
part_t label
Definition label_blob2.h:60
pcl::gpu::people::Blob2::indices
pcl::PointIndices indices
Definition label_blob2.h:74
pcl::gpu::people::Blob2::child_id
int child_id[MAX_CHILD]
Definition label_blob2.h:69
pcl::gpu::people::Blob2::child_dist
float child_dist[MAX_CHILD]
Definition label_blob2.h:71
pcl::gpu::people::Blob2::child_lid
int child_lid[MAX_CHILD]
Definition label_blob2.h:70
pcl::gpu::people::Tree2
This structure contains all parameters to describe the segmented tree.
Definition label_tree.h:75
pcl::gpu::people::Tree2::id
int id
Definition label_tree.h:83
pcl::gpu::people::Tree2::cov
Eigen::Matrix3f cov
Definition label_tree.h:92
pcl::gpu::people::Tree2::total_dist_error
float total_dist_error
Definition label_tree.h:88
pcl::gpu::people::Tree2::eigenvect
Eigen::Matrix3f eigenvect
Definition label_tree.h:94
pcl::gpu::people::Tree2::Tree2
Tree2()
Definition label_tree.h:77
pcl::gpu::people::Tree2::eigenval
Eigen::Vector3f eigenval
Definition label_tree.h:93
pcl::gpu::people::Tree2::indices
pcl::PointIndices indices
Definition label_tree.h:96
pcl::gpu::people::Tree2::mean
Eigen::Vector4f mean
Definition label_tree.h:91
pcl::gpu::people::Tree2::lid
int lid
Definition label_tree.h:85
pcl::gpu::people::Tree2::label
part_t label
Definition label_tree.h:84
pcl::gpu::people::Tree2::norm_dist_error
float norm_dist_error
Definition label_tree.h:89
pcl::gpu::people::Tree2::max
Eigen::Vector4f max
Definition label_tree.h:98
pcl::gpu::people::Tree2::min
Eigen::Vector4f min
Definition label_tree.h:97
pcl::gpu::people::Tree2::nr_parts
int nr_parts
Definition label_tree.h:86
pcl::gpu::people::Tree2::parts_lid
int parts_lid[NUM_PARTS]
Definition label_tree.h:87