Point Cloud Library (PCL)  1.12.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(std::size_t 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(std::size_t 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(std::size_t 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(const int &cluster : connected_clusters[i])
176  {
177  if (!used_clusters[cluster]) // if this cluster has not been used yet
178  {
179  used_clusters[cluster] = true;
180  for(const auto& cluster_idx : input_clusters[cluster].getIndices().indices)
181  {
182  point_indices.indices.push_back(cluster_idx);
183  }
184  }
185  }
186  pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
187  output_clusters.push_back(cluster);
188  }
189  }
190  }
191  }
192 
193 template <typename PointT> void
195  std::vector<int>& maxima_cloud_indices, std::vector<pcl::people::PersonCluster<PointT> >& subclusters)
196 {
197  // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
198  float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed)
199  Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed)
200  Eigen::Matrix3Xf maxima_projected(3,maxima_number); // matrix containing the projection of maxima onto the ground plane
201  Eigen::VectorXi subclusters_number_of_points(maxima_number); // subclusters number of points
202  std::vector <pcl::Indices> sub_clusters_indices; // vector of vectors with the cluster indices for every maximum
203  sub_clusters_indices.resize(maxima_number); // resize to number of maxima
204 
205  // Project maxima on the ground plane:
206  for(int i = 0; i < maxima_number; i++) // for every maximum
207  {
208  PointT* current_point = &(*cloud_)[maxima_cloud_indices[i]]; // current maximum point cloud point
209  Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen
210  float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
211  maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane
212  subclusters_number_of_points(i) = 0; // initialize number of points
213  }
214 
215  // Associate cluster points to one of the maximum:
216  for(const auto& cluster_idx : cluster.getIndices().indices)
217  {
218  Eigen::Vector3f p_current_eigen((*cloud_)[cluster_idx].x, (*cloud_)[cluster_idx].y, (*cloud_)[cluster_idx].z); // conversion to eigen
219  float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground
220  p_current_eigen -= head_ground_coeffs * t; // projection of the point on the groundplane
221 
222  int i = 0;
223  bool correspondence_detected = false;
224  while ((!correspondence_detected) && (i < maxima_number))
225  {
226  if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
227  {
228  correspondence_detected = true;
229  sub_clusters_indices[i].push_back(cluster_idx);
230  subclusters_number_of_points(i)++;
231  }
232  else
233  i++;
234  }
235  }
236 
237  // Create a subcluster if the number of points associated to a maximum is over a threshold:
238  for(int i = 0; i < maxima_number; i++) // for every maximum
239  {
240  if (subclusters_number_of_points(i) > min_points_)
241  {
242  pcl::PointIndices point_indices;
243  point_indices.indices = sub_clusters_indices[i]; // indices associated to the i-th maximum
244 
245  pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
246  subclusters.push_back(cluster);
247  //std::cout << "Cluster number of points: " << subclusters_number_of_points(i) << std::endl;
248  }
249  }
250 }
251 
252 template <typename PointT> void
254 {
255  // Check if all mandatory variables have been set:
256  if (std::isnan(sqrt_ground_coeffs_))
257  {
258  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n");
259  return;
260  }
261  if (cluster_indices_.empty ())
262  {
263  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
264  return;
265  }
266  if (cloud_ == nullptr)
267  {
268  PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n");
269  return;
270  }
271 
272  // Person clusters creation from clusters indices:
273  for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it)
274  {
275  pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation
276  clusters.push_back(cluster);
277  }
278 
279  // Remove clusters with too high height from the ground plane:
280  std::vector<pcl::people::PersonCluster<PointT> > new_clusters;
281  for(std::size_t i = 0; i < clusters.size(); i++) // for every cluster
282  {
283  if (clusters[i].getHeight() <= max_height_)
284  new_clusters.push_back(clusters[i]);
285  }
286  clusters = new_clusters;
287  new_clusters.clear();
288 
289  // Merge clusters close in floor coordinates:
290  mergeClustersCloseInFloorCoordinates(clusters, new_clusters);
291  clusters = new_clusters;
292 
293  std::vector<pcl::people::PersonCluster<PointT> > subclusters;
294  int cluster_min_points_sub = int(float(min_points_) * 1.5);
295  // int cluster_max_points_sub = max_points_;
296 
297  // create HeightMap2D object:
298  pcl::people::HeightMap2D<PointT> height_map_obj;
299  height_map_obj.setGround(ground_coeffs_);
300  height_map_obj.setInputCloud(cloud_);
301  height_map_obj.setSensorPortraitOrientation(vertical_);
302  height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_);
303  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster
304  {
305  float height = it->getHeight();
306  int number_of_points = it->getNumberPoints();
307  if(height > min_height_ && height < max_height_)
308  {
309  if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub)
310  {
311  // Compute height map associated to the current cluster and its local maxima (heads):
312  height_map_obj.compute(*it);
313  if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum
314  {
315  // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
316  createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters);
317  }
318  else
319  { // Only one maximum --> copy original cluster:
320  subclusters.push_back(*it);
321  }
322  }
323  else
324  {
325  // Cluster properties not good for sub-clustering --> copy original cluster:
326  subclusters.push_back(*it);
327  }
328  }
329  }
330  clusters = subclusters; // substitute clusters with subclusters
331 }
332 
333 template <typename PointT>
335 #endif /* PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ */
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
virtual ~HeadBasedSubclustering()
Destructor.
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 subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
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 getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
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.
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
HeightMap2D represents a class for creating a 2D height map from a point cloud and searching for its ...
Definition: height_map_2d.h:58
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
PersonCluster represents a class for representing information about a cluster containing a person.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
A point structure representing Euclidean xyz coordinates, and the RGB color.