Loading...
Searching...
No Matches
label_segment.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 segment.h
* @brief This file contains the function prototypes for the segmentation functions
*/
#pragma once
// our headers
#include "pcl/gpu/people/label_blob2.h"
#include "pcl/gpu/people/label_common.h"
// std
#include <vector>
// opencv drawing stuff
//#include <opencv2/highgui/highgui.hpp>
// PCL specific includes
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/PointIndices.h>
#include <pcl/common/time.h>
namespace pcl
{
namespace gpu
{
namespace people
{
namespace label_skeleton
{
/*
* \brief this function smooths the label image based on label and depth
* \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
* \param[in] dmap the cvMat with the depths, must be CV_16U in mm
* \param[out] lmap_out the smoothed output labelmap as cvMat
* \param[in] patch_size make the patch size for smoothing
* \param[in] depthThres the z-distance threshold
* \todo add a Gaussian contribution function to depth and vote
**/
//inline void smoothLabelImage ( cv::Mat& lmap_in,
// cv::Mat& dmap,
// cv::Mat& lmap_out,
// unsigned int patch_size,
// unsigned int depthThres)
//{
// // check depth
// assert(lmap_in.depth() == CV_8UC1);
// assert(dmap.depth() == CV_16U);
// assert(lmap_out.depth() == CV_8UC1);
// // check size
// assert(lmap_in.rows == dmap.rows);
// assert(lmap_in.cols == dmap.cols);
// assert(lmap_out.rows == dmap.rows);
// assert(lmap_out.cols == dmap.cols);
// unsigned int half_patch = static_cast<int> (patch_size/2);
// // iterate over the height of the image (from 2 till 478)
// for(unsigned int h = (0 + half_patch); h < (lmap_in.rows - half_patch); h++)
// {
// // iterate over the width of the image (from 2 till 638)
// for(unsigned int w = (0 + half_patch); w < (lmap_in.cols - half_patch); w++)
// {
// short depth = dmap.at<short>(h, w);
// unsigned int votes[NUM_PARTS];
// //this should be unneeded but to test
// for(int j = 0 ; j< NUM_PARTS; j++)
// votes[j] = 0;
// // iterate over the size of the patch in the height
// for(unsigned int h_l = (h - half_patch); h_l <= (h + half_patch); h_l++)
// {
// // iterate over the size of the patch in the width
// for(unsigned int w_l = (w - half_patch); w_l <= (w + half_patch); w_l++)
// {
// // get the depth of this part of the patch
// short depth_l = dmap.at<short>(h_l,w_l);
// // evaluate the difference to the centroid
// if(std::abs(depth - depth_l) < static_cast<int> (depthThres))
// {
// char label = lmap_in.at<char>(h_l,w_l);
// if(label >= 0 && label < NUM_PARTS)
// votes[static_cast<unsigned int> (label)]++;
// else
// std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
// }
// }
// }
// unsigned int max = 0;
// char label = lmap_in.at<char>(h,w);
// // iterate over the bin to find the max
// for(char i=0; i<NUM_PARTS; i++)
// {
// if(votes[static_cast<int> (i)] > max)
// {
// max = votes[static_cast<int> (i)];
// label = i;
// }
// }
// lmap_out.at<char>(h,w) = label;
// }
// }
//}
/*
* \brief this function smooths the label image based on label and depth
* \param[in] lmap_in the cvMat with the labels, must be CV_8UC1
* \param[in] dmap the cvMat with the depths, must be CV_16U in mm
* \param[out] lmap_out the smoothed output labelmap as cvMat
* \todo make the patch size a parameter
* \todo make the z-distance a parameter
* \todo add a Gaussian contribution function to depth and vote
**/
//inline void smoothLabelImage2 ( cv::Mat& lmap_in,
// cv::Mat& dmap,
// cv::Mat& lmap_out)
//{
// // check depth
// assert(lmap_in.depth() == CV_8UC1);
// assert(dmap.depth() == CV_16U);
// assert(lmap_out.depth() == CV_8UC1);
// // check size
// assert(lmap_in.rows == dmap.rows);
// assert(lmap_in.cols == dmap.cols);
// assert(lmap_out.rows == dmap.rows);
// assert(lmap_out.cols == dmap.cols);
// //unsigned int patch_size = 5;
// unsigned int half_patch = 2;
// unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
// // iterate over the height of the image (from 2 till 478)
// unsigned int endrow = (lmap_in.rows - half_patch);
// unsigned int endcol = (lmap_in.cols - half_patch);
// for(unsigned int h = (0 + half_patch); h < endrow; h++)
// {
// unsigned int endheight = (h + half_patch);
// // iterate over the width of the image (from 2 till 638)
// for(unsigned int w = (0 + half_patch); w < endcol; w++)
// {
// unsigned int endwidth = (w + half_patch);
// short depth = dmap.at<short>(h, w);
// unsigned int votes[NUM_PARTS];
// //this should be unneeded but to test
// for(int j = 0 ; j< NUM_PARTS; j++)
// votes[j] = 0;
// // iterate over the size of the patch in the height
// for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
// {
// // iterate over the size of the patch in the width
// for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
// {
// // get the depth of this part of the patch
// short depth_l = dmap.at<short>(h_l,w_l);
// // evaluate the difference to the centroid
// if(std::abs(depth - depth_l) < static_cast<int> (depthThres))
// {
// char label = lmap_in.at<char>(h_l,w_l);
// if(label >= 0 && label < NUM_PARTS)
// votes[static_cast<unsigned int>(label)]++;
// else
// std::cout << "(E) : smoothLabelImage(): I've read a label that is non valid" << std::endl;
// }
// }
// }
// unsigned int max = 0;
// char label = lmap_in.at<char>(h,w);
// // iterate over the bin to find the max
// for(char i=0; i<NUM_PARTS; i++)
// {
// if(votes[static_cast<unsigned int>(i)] > max)
// {
// max = votes[static_cast<unsigned int>(i)];
// label = i;
// }
// }
// lmap_out.at<char>(h,w) = label;
// }
// }
//}
/**
* @brief this function smooths the label image based on label and depth
* @param[in] lmap_in the cvMat with the labels, must be CV_8UC1
* @param[in] dmap the cvMat with the depths, must be CV_16U in mm
* @param[out] lmap_out the smoothed output labelmap as cvMat
* @todo make the patch size a parameter
* @todo make the z-distance a parameter
* @todo add a Gaussian contribution function to depth and vote
**/
inline void smoothLabelImage ( cv::Mat& lmap_in,
cv::Mat& dmap,
cv::Mat& lmap_out)
{
// check depth
assert(lmap_in.depth() == CV_8UC1);
assert(dmap.depth() == CV_16U);
assert(lmap_out.depth() == CV_8UC1);
// check size
assert(lmap_in.rows == dmap.rows);
assert(lmap_in.cols == dmap.cols);
assert(lmap_out.rows == dmap.rows);
assert(lmap_out.cols == dmap.cols);
//unsigned int patch_size = 5;
unsigned int half_patch = 2;
unsigned int depthThres = 300; // Think this is in mm, verify this!!!!!
// iterate over the height of the image (from 2 till 478)
unsigned int endrow = (lmap_in.rows - half_patch);
unsigned int endcol = (lmap_in.cols - half_patch);
unsigned int votes[NUM_PARTS];
unsigned int endheight, endwidth;
const short* drow;
char *loutrow;
short depth;
const short* drow_offset;
const char* lrow_offset;
short depth_l;
char label;
for(unsigned int h = (0 + half_patch); h < endrow; h++)
{
endheight = (h + half_patch);
drow = dmap.ptr<short>(h);
loutrow = lmap_out.ptr<char>(h);
// iterate over the width of the image (from 2 till 638)
for(unsigned int w = (0 + half_patch); w < endcol; w++)
{
endwidth = (w + half_patch);
depth = drow[w];
// reset votes
for(int j = 0 ; j< NUM_PARTS; j++)
votes[j] = 0;
// iterate over the size of the patch in the height
for(unsigned int h_l = (h - half_patch); h_l <= endheight; h_l++)
{
drow_offset = dmap.ptr<short>(h_l);
lrow_offset = lmap_in.ptr<char>(h_l);
// iterate over the size of the patch in the width
for(unsigned int w_l = (w - half_patch); w_l <= endwidth; w_l++)
{
// get the depth of this part of the patch
depth_l = drow_offset[w_l];
// evaluate the difference to the centroid
if(std::abs(depth - depth_l) < static_cast<int> (depthThres))
{
label = lrow_offset[w_l];
votes[static_cast<unsigned int>(label)]++;
}
}
}
unsigned int max = 0;
// iterate over the bin to find the max
for(char i=0; i<NUM_PARTS; i++)
{
if(votes[static_cast<unsigned int>(i)] > max)
{
max = votes[static_cast<unsigned int>(i)];
loutrow[w] = i;
}
}
}
}
}
/**
* @brief This function takes a number of indices with label and sorts them in the blob matrix
* @param[in] cloud_in the original input pointcloud from which everything was calculated
* @param[in] sizeThres the minimal size needed to be considered as a valid blob
* @param[out] sorted the matrix in which every blob will be pushed back
* @param[in] indices the matrix of PointIndices
* @todo implement the eigenvalue evaluation again
* @todo do we still need sizeThres?
**/
inline void sortIndicesToBlob2 ( const pcl::PointCloud<pcl::PointXYZ>& cloud_in,
unsigned int sizeThres,
std::vector< std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted,
std::vector< std::vector<pcl::PointIndices> >& indices)
{
assert(sorted.size () == indices.size ());
unsigned int id = 0;
// Iterate over all labels
for(unsigned int lab = 0; lab < indices.size(); ++lab)
{
unsigned int lid = 0;
// Iterate over all blobs of this label
for(unsigned int l = 0; l < indices[lab].size(); l++)
{
// If the blob has enough pixels
if(indices[lab][l].indices.size() > sizeThres)
{
Blob2 b;
b.indices = indices[lab][l];
pcl::compute3DCentroid(cloud_in, b.indices, b.mean);
#ifdef NEED_STATS
pcl::computeCovarianceMatrixNormalized(cloud_in, b.indices, b.mean, b.cov);
pcl::getMinMax3D(cloud_in, b.indices, b.min, b.max);
pcl::eigen33(b.cov, b.eigenvect, b.eigenval);
#endif
// Check if it is a valid blob
//if((b.eigenval(0) < LUT_max_part_size[(int) lab]) && (b.mean(2) != 0))
if((b.mean(2) != 0))
{
b.id = id;
id++;
b.label = static_cast<part_t> (lab);
b.lid = lid;
lid++;
sorted[lab].push_back(b);
}
}
}
}
}
/**
* @brief This function is a stupid helper function to debug easilier, it print out how the matrix was sorted
* @param[in] sorted the matrix of blobs
* @return Zero if everything went well
**/
inline int giveSortedBlobsInfo ( std::vector<std::vector<Blob2, Eigen::aligned_allocator<Blob2> > >& sorted)
{
std::cout << "(I) : giveSortedBlobsInfo(): Size of outer vector: " << sorted.size() << std::endl;
for(unsigned int i = 0; i < sorted.size(); i++)
{
std::cout << "(I) : giveSortedBlobsInfo(): Found " << sorted[i].size() << " parts of type " << i << std::endl;
std::cout << "(I) : giveSortedBlobsInfo(): indices : ";
for(unsigned int j = 0; j < sorted[i].size(); j++)
{
std::cout << " id:" << sorted[i][j].id << " lid:" << sorted[i][j].lid;
}
std::cout << std::endl;
}
return 0;
}
} // end namespace label_skeleton
} // 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:173
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.
time.h
Define methods for measuring time spent in code blocks.
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::label_skeleton::giveSortedBlobsInfo
int giveSortedBlobsInfo(std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted)
This function is a stupid helper function to debug easilier, it print out how the matrix was sorted.
Definition label_segment.h:380
pcl::gpu::people::label_skeleton::smoothLabelImage
void smoothLabelImage(cv::Mat &lmap_in, cv::Mat &dmap, cv::Mat &lmap_out)
this function smooths the label image based on label and depth
Definition label_segment.h:240
pcl::gpu::people::label_skeleton::sortIndicesToBlob2
void sortIndicesToBlob2(const pcl::PointCloud< pcl::PointXYZ > &cloud_in, unsigned int sizeThres, std::vector< std::vector< Blob2, Eigen::aligned_allocator< Blob2 > > > &sorted, std::vector< std::vector< pcl::PointIndices > > &indices)
This function takes a number of indices with label and sorts them in the blob matrix.
Definition label_segment.h:331
pcl::gpu::people::NUM_PARTS
@ NUM_PARTS
Definition label_common.h:60
pcl::gpu::people::part_t
part_t
Our code is forseen to use maximal use 32 labels.
Definition label_common.h:76
pcl
Definition convolution.h:46
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::eigenvect
Eigen::Matrix3f eigenvect
Definition label_blob2.h:66
pcl::gpu::people::Blob2::mean
Eigen::Vector4f mean
Definition label_blob2.h:63
pcl::gpu::people::Blob2::cov
Eigen::Matrix3f cov
Definition label_blob2.h:64
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::eigenval
Eigen::Vector3f eigenval
Definition label_blob2.h:65
pcl::gpu::people::Blob2::min
Eigen::Vector4f min
Definition label_blob2.h:75
pcl::gpu::people::Blob2::max
Eigen::Vector4f max
Definition label_blob2.h:76
pcl::gpu::people::Blob2::lid
int lid
Definition label_blob2.h:61
pcl::gpu::people::Blob2::id
int id
Definition label_blob2.h:59