Point Cloud Library (PCL) 1.13.0
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
46template <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
62template <typename PointT> void
64{
65 cloud_ = cloud;
66}
67
68template <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
75template <typename PointT> void
76pcl::people::HeadBasedSubclustering<PointT>::setInitialClusters (std::vector<pcl::PointIndices>& cluster_indices)
77{
78 cluster_indices_ = cluster_indices;
79}
80
81template <typename PointT> void
83{
84 vertical_ = vertical;
85}
86
87template <typename PointT> void
89{
90 min_height_ = min_height;
91 max_height_ = max_height;
92}
93
94template <typename PointT> void
96{
97 min_points_ = min_points;
98 max_points_ = max_points;
99}
100
101template <typename PointT> void
103{
104 heads_minimum_distance_= heads_minimum_distance;
105}
106
107template <typename PointT> void
109{
110 head_centroid_ = head_centroid;
111}
112
113template <typename PointT> void
115{
116 min_height = min_height_;
117 max_height = max_height_;
118}
119
120template <typename PointT> void
122{
123 min_points = min_points_;
124 max_points = max_points_;
125}
126
127template <typename PointT> float
129{
130 return (heads_minimum_distance_);
131}
132
133template <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
193template <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
252template <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:
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
333template <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.