Point Cloud Library (PCL)  1.14.1
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(const auto & cluster_index : cluster_indices_)
274  {
275  pcl::people::PersonCluster<PointT> cluster(cloud_, cluster_index, 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 = static_cast<int>(static_cast<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(auto 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_ */
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.
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.
virtual ~HeadBasedSubclustering()
Destructor.
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:54
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.