Point Cloud Library (PCL)  1.8.1-dev
head_based_subcluster.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * head_based_subcluster.hpp
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #ifndef PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
42 #define PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
43 
44 #include <pcl/people/head_based_subcluster.h>
45 
46 template <typename PointT>
48 {
49  // set default values for optional parameters:
50  vertical_ = false;
51  head_centroid_ = true;
52  min_height_ = 1.3;
53  max_height_ = 2.3;
54  min_points_ = 30;
55  max_points_ = 5000;
56  heads_minimum_distance_ = 0.3;
57 
58  // set flag values for mandatory parameters:
59  sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
60 }
61 
62 template <typename PointT> void
64 {
65  cloud_ = cloud;
66 }
67 
68 template <typename PointT> void
70 {
71  ground_coeffs_ = ground_coeffs;
72  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
73 }
74 
75 template <typename PointT> void
76 pcl::people::HeadBasedSubclustering<PointT>::setInitialClusters (std::vector<pcl::PointIndices>& cluster_indices)
77 {
78  cluster_indices_ = cluster_indices;
79 }
80 
81 template <typename PointT> void
83 {
84  vertical_ = vertical;
85 }
86 
87 template <typename PointT> void
89 {
90  min_height_ = min_height;
91  max_height_ = max_height;
92 }
93 
94 template <typename PointT> void
96 {
97  min_points_ = min_points;
98  max_points_ = max_points;
99 }
100 
101 template <typename PointT> void
103 {
104  heads_minimum_distance_= heads_minimum_distance;
105 }
106 
107 template <typename PointT> void
109 {
110  head_centroid_ = head_centroid;
111 }
112 
113 template <typename PointT> void
115 {
116  min_height = min_height_;
117  max_height = max_height_;
118 }
119 
120 template <typename PointT> void
122 {
123  min_points = min_points_;
124  max_points = max_points_;
125 }
126 
127 template <typename PointT> float
129 {
130  return (heads_minimum_distance_);
131 }
132 
133 template <typename PointT> void
135  std::vector<pcl::people::PersonCluster<PointT> >& output_clusters)
136 {
137  float min_distance_between_cluster_centers = 0.4; // meters
138  float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed)
139  Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed)
140  std::vector <std::vector<int> > connected_clusters;
141  connected_clusters.resize(input_clusters.size());
142  std::vector<bool> used_clusters; // 0 in correspondence of clusters remained to process, 1 for already used clusters
143  used_clusters.resize(input_clusters.size());
144  for(unsigned int i = 0; i < input_clusters.size(); i++) // for every cluster
145  {
146  Eigen::Vector3f theoretical_center = input_clusters[i].getTCenter();
147  float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground
148  Eigen::Vector3f current_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane
149  for(unsigned int j = i+1; j < input_clusters.size(); j++) // for every remaining cluster
150  {
151  theoretical_center = input_clusters[j].getTCenter();
152  float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground
153  Eigen::Vector3f new_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane
154  if (((new_cluster_center_projection - current_cluster_center_projection).norm()) < min_distance_between_cluster_centers)
155  {
156  connected_clusters[i].push_back(j);
157  }
158  }
159  }
160 
161  for(unsigned int i = 0; i < connected_clusters.size(); i++) // for every cluster
162  {
163  if (!used_clusters[i]) // if this cluster has not been used yet
164  {
165  used_clusters[i] = true;
166  if (connected_clusters[i].empty()) // no other clusters to merge
167  {
168  output_clusters.push_back(input_clusters[i]);
169  }
170  else
171  {
172  // Copy cluster points into new cluster:
173  pcl::PointIndices point_indices;
174  point_indices = input_clusters[i].getIndices();
175  for(unsigned int j = 0; j < connected_clusters[i].size(); j++)
176  {
177  if (!used_clusters[connected_clusters[i][j]]) // if this cluster has not been used yet
178  {
179  used_clusters[connected_clusters[i][j]] = true;
180  for(std::vector<int>::const_iterator points_iterator = input_clusters[connected_clusters[i][j]].getIndices().indices.begin();
181  points_iterator != input_clusters[connected_clusters[i][j]].getIndices().indices.end(); points_iterator++)
182  {
183  point_indices.indices.push_back(*points_iterator);
184  }
185  }
186  }
187  pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
188  output_clusters.push_back(cluster);
189  }
190  }
191  }
192  }
193 
194 template <typename PointT> void
196  std::vector<int>& maxima_cloud_indices, std::vector<pcl::people::PersonCluster<PointT> >& subclusters)
197 {
198  // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
199  float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed)
200  Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed)
201  Eigen::Matrix3Xf maxima_projected(3,maxima_number); // matrix containing the projection of maxima onto the ground plane
202  Eigen::VectorXi subclusters_number_of_points(maxima_number); // subclusters number of points
203  std::vector <std::vector <int> > sub_clusters_indices; // vector of vectors with the cluster indices for every maximum
204  sub_clusters_indices.resize(maxima_number); // resize to number of maxima
205 
206  // Project maxima on the ground plane:
207  for(int i = 0; i < maxima_number; i++) // for every maximum
208  {
209  PointT* current_point = &cloud_->points[maxima_cloud_indices[i]]; // current maximum point cloud point
210  Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
211  float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
212  maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane
213  subclusters_number_of_points(i) = 0; // intialize number of points
214  }
215 
216  // Associate cluster points to one of the maximum:
217  for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); points_iterator++)
218  {
219  PointT* current_point = &cloud_->points[*points_iterator]; // current point cloud point
220  Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
221  float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
222  p_current_eigen = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane
223 
224  int i = 0;
225  bool correspondence_detected = false;
226  while ((!correspondence_detected) && (i < maxima_number))
227  {
228  if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
229  {
230  correspondence_detected = true;
231  sub_clusters_indices[i].push_back(*points_iterator);
232  subclusters_number_of_points(i)++;
233  }
234  else
235  i++;
236  }
237  }
238 
239  // Create a subcluster if the number of points associated to a maximum is over a threshold:
240  for(int i = 0; i < maxima_number; i++) // for every maximum
241  {
242  if (subclusters_number_of_points(i) > min_points_)
243  {
244  pcl::PointIndices point_indices;
245  point_indices.indices = sub_clusters_indices[i]; // indices associated to the i-th maximum
246 
247  pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
248  subclusters.push_back(cluster);
249  //std::cout << "Cluster number of points: " << subclusters_number_of_points(i) << std::endl;
250  }
251  }
252 }
253 
254 template <typename PointT> void
256 {
257  // Check if all mandatory variables have been set:
258  if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
259  {
260  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n");
261  return;
262  }
263  if (cluster_indices_.size() == 0)
264  {
265  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
266  return;
267  }
268  if (cloud_ == NULL)
269  {
270  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n");
271  return;
272  }
273 
274  // Person clusters creation from clusters indices:
275  for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it)
276  {
277  pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation
278  clusters.push_back(cluster);
279  }
280 
281  // Remove clusters with too high height from the ground plane:
282  std::vector<pcl::people::PersonCluster<PointT> > new_clusters;
283  for(unsigned int i = 0; i < clusters.size(); i++) // for every cluster
284  {
285  if (clusters[i].getHeight() <= max_height_)
286  new_clusters.push_back(clusters[i]);
287  }
288  clusters = new_clusters;
289  new_clusters.clear();
290 
291  // Merge clusters close in floor coordinates:
292  mergeClustersCloseInFloorCoordinates(clusters, new_clusters);
293  clusters = new_clusters;
294 
295  std::vector<pcl::people::PersonCluster<PointT> > subclusters;
296  int cluster_min_points_sub = int(float(min_points_) * 1.5);
297  // int cluster_max_points_sub = max_points_;
298 
299  // create HeightMap2D object:
300  pcl::people::HeightMap2D<PointT> height_map_obj;
301  height_map_obj.setGround(ground_coeffs_);
302  height_map_obj.setInputCloud(cloud_);
303  height_map_obj.setSensorPortraitOrientation(vertical_);
304  height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_);
305  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster
306  {
307  float height = it->getHeight();
308  int number_of_points = it->getNumberPoints();
309  if(height > min_height_ && height < max_height_)
310  {
311  if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub)
312  {
313  // Compute height map associated to the current cluster and its local maxima (heads):
314  height_map_obj.compute(*it);
315  if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum
316  {
317  // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
318  createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters);
319  }
320  else
321  { // Only one maximum --> copy original cluster:
322  subclusters.push_back(*it);
323  }
324  }
325  else
326  {
327  // Cluster properties not good for sub-clustering --> copy original cluster:
328  subclusters.push_back(*it);
329  }
330  }
331  }
332  clusters = subclusters; // substitute clusters with subclusters
333 }
334 
335 template <typename PointT>
337 {
338  // TODO Auto-generated destructor stub
339 }
340 #endif /* PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ */
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
void getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
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.
std::vector< int > indices
Definition: PointIndices.h:19
void setHeadCentroid(bool head_centroid)
Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole bo...
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
PersonCluster represents a class for representing information about a cluster containing a person...
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void mergeClustersCloseInFloorCoordinates(std::vector< pcl::people::PersonCluster< PointT > > &input_clusters, std::vector< pcl::people::PersonCluster< PointT > > &output_clusters)
Merge clusters close in floor coordinates.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
boost::shared_ptr< PointCloud > PointCloudPtr
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method...
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
HeightMap2D represents a class for creating a 2D height map from a point cloud and searching for its ...
Definition: height_map_2d.h:55
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.