head_based_subcluster.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2013-, Open Perception, 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 the copyright holder(s) 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.
*
* head_based_subcluster.hpp
* Created on: Nov 30, 2012
* Author: Matteo Munaro
*/
#ifndef PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
#define PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
#include <pcl/people/head_based_subcluster.h>
template <typename PointT>
pcl::people::HeadBasedSubclustering<PointT>::HeadBasedSubclustering ()
{
// set default values for optional parameters:
vertical_ = false;
head_centroid_ = true;
min_height_ = 1.3;
max_height_ = 2.3;
min_points_ = 30;
max_points_ = 5000;
heads_minimum_distance_ = 0.3;
// set flag values for mandatory parameters:
sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setInputCloud (PointCloudPtr& cloud)
{
cloud_ = cloud;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setGround (Eigen::VectorXf& ground_coeffs)
{
ground_coeffs_ = ground_coeffs;
sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setInitialClusters (std::vector<pcl::PointIndices>& cluster_indices)
{
cluster_indices_ = cluster_indices;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setSensorPortraitOrientation (bool vertical)
{
vertical_ = vertical;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setHeightLimits (float min_height, float max_height)
{
min_height_ = min_height;
max_height_ = max_height;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setDimensionLimits (int min_points, int max_points)
{
min_points_ = min_points;
max_points_ = max_points;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setMinimumDistanceBetweenHeads (float heads_minimum_distance)
{
heads_minimum_distance_= heads_minimum_distance;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setHeadCentroid (bool head_centroid)
{
head_centroid_ = head_centroid;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::getHeightLimits (float& min_height, float& max_height)
{
min_height = min_height_;
max_height = max_height_;
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::getDimensionLimits (int& min_points, int& max_points)
{
min_points = min_points_;
max_points = max_points_;
}
template <typename PointT> float
pcl::people::HeadBasedSubclustering<PointT>::getMinimumDistanceBetweenHeads ()
{
return (heads_minimum_distance_);
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::mergeClustersCloseInFloorCoordinates (std::vector<pcl::people::PersonCluster<PointT> >& input_clusters,
std::vector<pcl::people::PersonCluster<PointT> >& output_clusters)
{
float min_distance_between_cluster_centers = 0.4; // meters
float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed)
Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed)
std::vector <std::vector<int> > connected_clusters;
connected_clusters.resize(input_clusters.size());
std::vector<bool> used_clusters; // 0 in correspondence of clusters remained to process, 1 for already used clusters
used_clusters.resize(input_clusters.size());
for(unsigned int i = 0; i < input_clusters.size(); i++) // for every cluster
{
Eigen::Vector3f theoretical_center = input_clusters[i].getTCenter();
float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground
Eigen::Vector3f current_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane
for(unsigned int j = i+1; j < input_clusters.size(); j++) // for every remaining cluster
{
theoretical_center = input_clusters[j].getTCenter();
float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground
Eigen::Vector3f new_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane
if (((new_cluster_center_projection - current_cluster_center_projection).norm()) < min_distance_between_cluster_centers)
{
connected_clusters[i].push_back(j);
}
}
}
for(unsigned int i = 0; i < connected_clusters.size(); i++) // for every cluster
{
if (!used_clusters[i]) // if this cluster has not been used yet
{
used_clusters[i] = true;
if (connected_clusters[i].empty()) // no other clusters to merge
{
output_clusters.push_back(input_clusters[i]);
}
else
{
// Copy cluster points into new cluster:
pcl::PointIndices point_indices;
point_indices = input_clusters[i].getIndices();
for(unsigned int j = 0; j < connected_clusters[i].size(); j++)
{
if (!used_clusters[connected_clusters[i][j]]) // if this cluster has not been used yet
{
used_clusters[connected_clusters[i][j]] = true;
for(std::vector<int>::const_iterator points_iterator = input_clusters[connected_clusters[i][j]].getIndices().indices.begin();
points_iterator != input_clusters[connected_clusters[i][j]].getIndices().indices.end(); points_iterator++)
{
point_indices.indices.push_back(*points_iterator);
}
}
}
pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
output_clusters.push_back(cluster);
}
}
}
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::PersonCluster<PointT>& cluster, int maxima_number,
std::vector<int>& maxima_cloud_indices, std::vector<pcl::people::PersonCluster<PointT> >& subclusters)
{
// create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed)
Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed)
Eigen::Matrix3Xf maxima_projected(3,maxima_number); // matrix containing the projection of maxima onto the ground plane
Eigen::VectorXi subclusters_number_of_points(maxima_number); // subclusters number of points
std::vector <std::vector <int> > sub_clusters_indices; // vector of vectors with the cluster indices for every maximum
sub_clusters_indices.resize(maxima_number); // resize to number of maxima
// Project maxima on the ground plane:
for(int i = 0; i < maxima_number; i++) // for every maximum
{
PointT* current_point = &cloud_->points[maxima_cloud_indices[i]]; // current maximum point cloud point
Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane
subclusters_number_of_points(i) = 0; // intialize number of points
}
// Associate cluster points to one of the maximum:
for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); points_iterator++)
{
PointT* current_point = &cloud_->points[*points_iterator]; // current point cloud point
Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
p_current_eigen = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane
int i = 0;
bool correspondence_detected = false;
while ((!correspondence_detected) && (i < maxima_number))
{
if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
{
correspondence_detected = true;
sub_clusters_indices[i].push_back(*points_iterator);
subclusters_number_of_points(i)++;
}
else
i++;
}
}
// Create a subcluster if the number of points associated to a maximum is over a threshold:
for(int i = 0; i < maxima_number; i++) // for every maximum
{
if (subclusters_number_of_points(i) > min_points_)
{
pcl::PointIndices point_indices;
point_indices.indices = sub_clusters_indices[i]; // indices associated to the i-th maximum
pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
subclusters.push_back(cluster);
//std::cout << "Cluster number of points: " << subclusters_number_of_points(i) << std::endl;
}
}
}
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::subcluster (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
// Check if all mandatory variables have been set:
if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n");
return;
}
if (cluster_indices_.size() == 0)
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
return;
}
if (cloud_ == NULL)
{
PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n");
return;
}
// Person clusters creation from clusters indices:
for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it)
{
pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation
clusters.push_back(cluster);
}
// Remove clusters with too high height from the ground plane:
std::vector<pcl::people::PersonCluster<PointT> > new_clusters;
for(unsigned int i = 0; i < clusters.size(); i++) // for every cluster
{
if (clusters[i].getHeight() <= max_height_)
new_clusters.push_back(clusters[i]);
}
clusters = new_clusters;
new_clusters.clear();
// Merge clusters close in floor coordinates:
mergeClustersCloseInFloorCoordinates(clusters, new_clusters);
clusters = new_clusters;
std::vector<pcl::people::PersonCluster<PointT> > subclusters;
int cluster_min_points_sub = int(float(min_points_) * 1.5);
// int cluster_max_points_sub = max_points_;
// create HeightMap2D object:
pcl::people::HeightMap2D<PointT> height_map_obj;
height_map_obj.setGround(ground_coeffs_);
height_map_obj.setInputCloud(cloud_);
height_map_obj.setSensorPortraitOrientation(vertical_);
height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_);
for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster
{
float height = it->getHeight();
int number_of_points = it->getNumberPoints();
if(height > min_height_ && height < max_height_)
{
if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub)
{
// Compute height map associated to the current cluster and its local maxima (heads):
height_map_obj.compute(*it);
if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum
{
// create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters);
}
else
{ // Only one maximum --> copy original cluster:
subclusters.push_back(*it);
}
}
else
{
// Cluster properties not good for sub-clustering --> copy original cluster:
subclusters.push_back(*it);
}
}
}
clusters = subclusters; // substitute clusters with subclusters
}
template <typename PointT>
pcl::people::HeadBasedSubclustering<PointT>::~HeadBasedSubclustering ()
{
// TODO Auto-generated destructor stub
}
#endif /* PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ */
pcl::people::HeightMap2D::getMaximaNumberAfterFiltering
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
Definition: height_map_2d.hpp:297
pcl::people::HeadBasedSubclustering::getMinimumDistanceBetweenHeads
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons&#39; heads.
Definition: head_based_subcluster.hpp:128
pcl::people::HeadBasedSubclustering::setDimensionLimits
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
Definition: head_based_subcluster.hpp:95
pcl::people::HeadBasedSubclustering::getHeightLimits
void getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
Definition: head_based_subcluster.hpp:114
pcl::people::HeadBasedSubclustering::subcluster
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
Definition: head_based_subcluster.hpp:255
pcl::people::HeadBasedSubclustering::setHeightLimits
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
Definition: head_based_subcluster.hpp:88
pcl::people::HeightMap2D::compute
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
Definition: height_map_2d.hpp:59
pcl::people::HeadBasedSubclustering::createSubClusters
void createSubClusters(pcl::people::PersonCluster< PointT > &cluster, int maxima_number_after_filtering, std::vector< int > &maxima_cloud_indices_filtered, std::vector< pcl::people::PersonCluster< PointT > > &subclusters)
Create subclusters centered on the heads position from the current cluster.
Definition: head_based_subcluster.hpp:195
pcl::PointIndices::indices
std::vector< int > indices
Definition: PointIndices.h:19
pcl::people::HeadBasedSubclustering::HeadBasedSubclustering
HeadBasedSubclustering()
Constructor.
Definition: head_based_subcluster.hpp:47
pcl::people::HeadBasedSubclustering::setHeadCentroid
void setHeadCentroid(bool head_centroid)
Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole bo...
Definition: head_based_subcluster.hpp:108
pcl::people::HeadBasedSubclustering::setSensorPortraitOrientation
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
Definition: head_based_subcluster.hpp:82
pcl::PointIndices
Definition: PointIndices.h:12
pcl::people::PersonCluster
PersonCluster represents a class for representing information about a cluster containing a person...
Definition: person_cluster.h:55
pcl::people::HeadBasedSubclustering::setInitialClusters
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
Definition: head_based_subcluster.hpp:76
pcl::people::HeadBasedSubclustering::mergeClustersCloseInFloorCoordinates
void mergeClustersCloseInFloorCoordinates(std::vector< pcl::people::PersonCluster< PointT > > &input_clusters, std::vector< pcl::people::PersonCluster< PointT > > &output_clusters)
Merge clusters close in floor coordinates.
Definition: head_based_subcluster.hpp:134
pcl::people::HeightMap2D::setGround
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
Definition: height_map_2d.hpp:254
pcl::people::HeadBasedSubclustering::PointCloudPtr
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: head_based_subcluster.h:64
pcl::people::HeadBasedSubclustering::~HeadBasedSubclustering
virtual ~HeadBasedSubclustering()
Destructor.
Definition: head_based_subcluster.hpp:336
pcl::people::HeightMap2D::getMaximaCloudIndicesFiltered
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method...
Definition: height_map_2d.hpp:303
pcl::people::HeadBasedSubclustering::setInputCloud
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
Definition: head_based_subcluster.hpp:63
pcl::people::HeightMap2D::setInputCloud
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
Definition: height_map_2d.hpp:248
pcl::people::HeadBasedSubclustering::getDimensionLimits
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
Definition: head_based_subcluster.hpp:121
pcl::people::HeightMap2D::setMinimumDistanceBetweenMaxima
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
Definition: height_map_2d.hpp:267
pcl::people::HeadBasedSubclustering::setGround
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
Definition: head_based_subcluster.hpp:69
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:594
pcl::people::HeightMap2D::setSensorPortraitOrientation
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
Definition: height_map_2d.hpp:273
pcl::people::HeightMap2D
HeightMap2D represents a class for creating a 2D height map from a point cloud and searching for its ...
Definition: height_map_2d.h:55
pcl::people::HeadBasedSubclustering::setMinimumDistanceBetweenHeads
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons&#39; heads.
Definition: head_based_subcluster.hpp:102
pcl::people::PersonCluster::getIndices
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
Definition: person_cluster.hpp:255