Point Cloud Library (PCL)  1.7.1
ground_based_people_detection_app.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  * ground_based_people_detection_app.hpp
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #ifndef PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
42 #define PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
43 
44 #include <pcl/people/ground_based_people_detection_app.h>
45 
46 template <typename PointT>
48 {
50 
51  // set default values for optional parameters:
52  sampling_factor_ = 1;
53  voxel_size_ = 0.06;
54  vertical_ = false;
55  head_centroid_ = true;
56  min_height_ = 1.3;
57  max_height_ = 2.3;
58  min_points_ = 30; // this value is adapted to the voxel size in method "compute"
59  max_points_ = 5000; // this value is adapted to the voxel size in method "compute"
60  dimension_limits_set_ = false;
61  heads_minimum_distance_ = 0.3;
62 
63  // set flag values for mandatory parameters:
64  sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
65  person_classifier_set_flag_ = false;
66 }
67 
68 template <typename PointT> void
70 {
71  cloud_ = cloud;
72 }
73 
74 template <typename PointT> void
76 {
77  ground_coeffs_ = ground_coeffs;
78  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
79 }
80 
81 template <typename PointT> void
83 {
84  sampling_factor_ = sampling_factor;
85 }
86 
87 template <typename PointT> void
89 {
90  voxel_size_ = voxel_size;
91 }
92 
93 template <typename PointT> void
95 {
96  intrinsics_matrix_ = intrinsics_matrix;
97 }
98 
99 template <typename PointT> void
101 {
102  person_classifier_ = person_classifier;
103  person_classifier_set_flag_ = true;
104 }
105 
106 template <typename PointT> void
108 {
109  vertical_ = vertical;
110 }
111 
112 template <typename PointT> void
114 {
115  min_height_ = min_height;
116  max_height_ = max_height;
117 }
118 
119 template <typename PointT> void
121 {
122  min_points_ = min_points;
123  max_points_ = max_points;
124  dimension_limits_set_ = true;
125 }
126 
127 template <typename PointT> void
129 {
130  heads_minimum_distance_= heads_minimum_distance;
131 }
132 
133 template <typename PointT> void
135 {
136  head_centroid_ = head_centroid;
137 }
138 
139 template <typename PointT> void
141 {
142  min_height = min_height_;
143  max_height = max_height_;
144 }
145 
146 template <typename PointT> void
148 {
149  min_points = min_points_;
150  max_points = max_points_;
151 }
152 
153 template <typename PointT> float
155 {
156  return (heads_minimum_distance_);
157 }
158 
159 template <typename PointT> Eigen::VectorXf
161 {
162  if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
163  {
164  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
165  }
166  return (ground_coeffs_);
167 }
168 
171 {
172  return (no_ground_cloud_);
173 }
174 
175 template <typename PointT> void
177 {
178  // Extract RGB information from a point cloud and output the corresponding RGB point cloud
179  output_cloud->points.resize(input_cloud->height*input_cloud->width);
180  output_cloud->width = input_cloud->width;
181  output_cloud->height = input_cloud->height;
182 
183  pcl::RGB rgb_point;
184  for (int j = 0; j < input_cloud->width; j++)
185  {
186  for (int i = 0; i < input_cloud->height; i++)
187  {
188  rgb_point.r = (*input_cloud)(j,i).r;
189  rgb_point.g = (*input_cloud)(j,i).g;
190  rgb_point.b = (*input_cloud)(j,i).b;
191  (*output_cloud)(j,i) = rgb_point;
192  }
193  }
194 }
195 
196 template <typename PointT> void
198 {
200  output_cloud->points.resize(cloud->height*cloud->width);
201  output_cloud->width = cloud->height;
202  output_cloud->height = cloud->width;
203  for (int i = 0; i < cloud->width; i++)
204  {
205  for (int j = 0; j < cloud->height; j++)
206  {
207  (*output_cloud)(j,i) = (*cloud)(cloud->width - i - 1, j);
208  }
209  }
210  cloud = output_cloud;
211 }
212 
213 template <typename PointT> bool
215 {
216  // Check if all mandatory variables have been set:
217  if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
218  {
219  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
220  return (false);
221  }
222  if (cloud_ == NULL)
223  {
224  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
225  return (false);
226  }
227  if (intrinsics_matrix_(0) == 0)
228  {
229  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
230  return (false);
231  }
232  if (!person_classifier_set_flag_)
233  {
234  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
235  return (false);
236  }
237 
238  if (!dimension_limits_set_) // if dimension limits have not been set by the user
239  {
240  // Adapt thresholds for clusters points number to the voxel size:
241  max_points_ = int(float(max_points_) * std::pow(0.06/voxel_size_, 2));
242  if (voxel_size_ > 0.06)
243  min_points_ = int(float(min_points_) * std::pow(0.06/voxel_size_, 2));
244  }
245 
246  // Fill rgb image:
247  rgb_image_->points.clear(); // clear RGB pointcloud
248  extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud
249 
250  // Downsample of sampling_factor in every dimension:
251  if (sampling_factor_ != 1)
252  {
253  PointCloudPtr cloud_downsampled(new PointCloud);
254  cloud_downsampled->width = (cloud_->width)/sampling_factor_;
255  cloud_downsampled->height = (cloud_->height)/sampling_factor_;
256  cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
257  cloud_downsampled->is_dense = cloud_->is_dense;
258  for (int j = 0; j < cloud_downsampled->width; j++)
259  {
260  for (int i = 0; i < cloud_downsampled->height; i++)
261  {
262  (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
263  }
264  }
265  (*cloud_) = (*cloud_downsampled);
266  }
267 
268  // Voxel grid filtering:
269  PointCloudPtr cloud_filtered(new PointCloud);
270  pcl::VoxelGrid<PointT> voxel_grid_filter_object;
271  voxel_grid_filter_object.setInputCloud(cloud_);
272  voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
273  voxel_grid_filter_object.filter (*cloud_filtered);
274 
275  // Ground removal and update:
276  pcl::IndicesPtr inliers(new std::vector<int>);
277  boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered));
278  ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
279  no_ground_cloud_ = PointCloudPtr (new PointCloud);
281  extract.setInputCloud(cloud_filtered);
282  extract.setIndices(inliers);
283  extract.setNegative(true);
284  extract.filter(*no_ground_cloud_);
285  if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
286  ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_);
287  else
288  PCL_INFO ("No groundplane update!\n");
289 
290  // Euclidean Clustering:
291  std::vector<pcl::PointIndices> cluster_indices;
293  tree->setInputCloud(no_ground_cloud_);
295  ec.setClusterTolerance(2 * 0.06);
296  ec.setMinClusterSize(min_points_);
297  ec.setMaxClusterSize(max_points_);
298  ec.setSearchMethod(tree);
299  ec.setInputCloud(no_ground_cloud_);
300  ec.extract(cluster_indices);
301 
302  // Head based sub-clustering //
304  subclustering.setInputCloud(no_ground_cloud_);
305  subclustering.setGround(ground_coeffs_);
306  subclustering.setInitialClusters(cluster_indices);
307  subclustering.setHeightLimits(min_height_, max_height_);
308  subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
309  subclustering.setSensorPortraitOrientation(vertical_);
310  subclustering.subcluster(clusters);
311 
312  // Person confidence evaluation with HOG+SVM:
313  if (vertical_) // Rotate the image if the camera is vertical
314  {
315  swapDimensions(rgb_image_);
316  }
317  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
318  {
319  //Evaluate confidence for the current PersonCluster:
320  Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter());
321  centroid /= centroid(2);
322  Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop());
323  top /= top(2);
324  Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom());
325  bottom /= bottom(2);
326  it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
327  }
328 
329  return (true);
330 }
331 
332 template <typename PointT>
334 {
335  // TODO Auto-generated destructor stub
336 }
337 #endif /* PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ */