Point Cloud Library (PCL)  1.7.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(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_ */