Point Cloud Library (PCL)  1.8.1-dev
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_fov_ = 0;
57  max_fov_ = 50;
58  min_height_ = 1.3;
59  max_height_ = 2.3;
60  min_width_ = 0.1;
61  max_width_ = 8.0;
62  updateMinMaxPoints ();
63  heads_minimum_distance_ = 0.3;
64 
65  // set flag values for mandatory parameters:
66  sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
67  ground_coeffs_set_ = false;
68  intrinsics_matrix_set_ = false;
69  person_classifier_set_flag_ = false;
70 
71  // set other flags
72  transformation_set_ = false;
73 }
74 
75 template <typename PointT> void
77 {
78  cloud_ = cloud;
79 }
80 
81 template <typename PointT> void
83 {
84  if (!transformation.isUnitary())
85  {
86  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::setCloudTransform] The cloud transformation matrix must be an orthogonal matrix!\n");
87  }
88 
89  transformation_ = transformation;
90  transformation_set_ = true;
91  applyTransformationGround();
92  applyTransformationIntrinsics();
93 }
94 
95 template <typename PointT> void
97 {
98  ground_coeffs_ = ground_coeffs;
99  ground_coeffs_set_ = true;
100  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
101  applyTransformationGround();
102 }
103 
104 template <typename PointT> void
106 {
107  sampling_factor_ = sampling_factor;
108 }
109 
110 template <typename PointT> void
112 {
113  voxel_size_ = voxel_size;
114  updateMinMaxPoints ();
115 }
116 
117 template <typename PointT> void
119 {
120  intrinsics_matrix_ = intrinsics_matrix;
121  intrinsics_matrix_set_ = true;
122  applyTransformationIntrinsics();
123 }
124 
125 template <typename PointT> void
127 {
128  person_classifier_ = person_classifier;
129  person_classifier_set_flag_ = true;
130 }
131 
132 template <typename PointT> void
134 {
135  min_fov_ = min_fov;
136  max_fov_ = max_fov;
137 }
138 
139 template <typename PointT> void
141 {
142  vertical_ = vertical;
143 }
144 
145 template<typename PointT>
147 {
148  min_points_ = (int) (min_height_ * min_width_ / voxel_size_ / voxel_size_);
149  max_points_ = (int) (max_height_ * max_width_ / voxel_size_ / voxel_size_);
150 }
151 
152 template <typename PointT> void
153 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width)
154 {
155  min_height_ = min_height;
156  max_height_ = max_height;
157  min_width_ = min_width;
158  max_width_ = max_width;
159  updateMinMaxPoints ();
160 }
161 
162 template <typename PointT> void
164 {
165  heads_minimum_distance_= heads_minimum_distance;
166 }
167 
168 template <typename PointT> void
170 {
171  head_centroid_ = head_centroid;
172 }
173 
174 template <typename PointT> void
175 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width)
176 {
177  min_height = min_height_;
178  max_height = max_height_;
179  min_width = min_width_;
180  max_width = max_width_;
181 }
182 
183 template <typename PointT> void
185 {
186  min_points = min_points_;
187  max_points = max_points_;
188 }
189 
190 template <typename PointT> float
192 {
193  return (heads_minimum_distance_);
194 }
195 
196 template <typename PointT> Eigen::VectorXf
198 {
199  if (!ground_coeffs_set_)
200  {
201  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
202  }
203  return (ground_coeffs_);
204 }
205 
208 {
209  return (cloud_filtered_);
210 }
211 
214 {
215  return (no_ground_cloud_);
216 }
217 
218 template <typename PointT> void
220 {
221  // Extract RGB information from a point cloud and output the corresponding RGB point cloud
222  output_cloud->points.resize(input_cloud->height*input_cloud->width);
223  output_cloud->width = input_cloud->width;
224  output_cloud->height = input_cloud->height;
225 
226  pcl::RGB rgb_point;
227  for (uint32_t j = 0; j < input_cloud->width; j++)
228  {
229  for (uint32_t i = 0; i < input_cloud->height; i++)
230  {
231  rgb_point.r = (*input_cloud)(j,i).r;
232  rgb_point.g = (*input_cloud)(j,i).g;
233  rgb_point.b = (*input_cloud)(j,i).b;
234  (*output_cloud)(j,i) = rgb_point;
235  }
236  }
237 }
238 
239 template <typename PointT> void
241 {
243  output_cloud->points.resize(cloud->height*cloud->width);
244  output_cloud->width = cloud->height;
245  output_cloud->height = cloud->width;
246  for (uint32_t i = 0; i < cloud->width; i++)
247  {
248  for (uint32_t j = 0; j < cloud->height; j++)
249  {
250  (*output_cloud)(j,i) = (*cloud)(cloud->width - i - 1, j);
251  }
252  }
253  cloud = output_cloud;
254 }
255 
256 template <typename PointT> void
258 {
259  if (transformation_set_)
260  {
261  Eigen::Transform<float, 3, Eigen::Affine> transform;
262  transform = transformation_;
263  pcl::transformPointCloud(*cloud_, *cloud_, transform);
264  }
265 }
266 
267 template <typename PointT> void
269 {
270  if (transformation_set_ && ground_coeffs_set_)
271  {
272  Eigen::Transform<float, 3, Eigen::Affine> transform;
273  transform = transformation_;
274  ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_;
275  }
276  else
277  {
278  ground_coeffs_transformed_ = ground_coeffs_;
279  }
280 }
281 
282 template <typename PointT> void
284 {
285  if (transformation_set_ && intrinsics_matrix_set_)
286  {
287  intrinsics_matrix_transformed_ = intrinsics_matrix_ * transformation_.transpose();
288  }
289  else
290  {
291  intrinsics_matrix_transformed_ = intrinsics_matrix_;
292  }
293 }
294 
295 template <typename PointT> void
297 {
298  cloud_filtered_ = PointCloudPtr (new PointCloud);
300  grid.setInputCloud(cloud_);
301  grid.setLeafSize(voxel_size_, voxel_size_, voxel_size_);
302  grid.setFilterFieldName("z");
303  grid.setFilterLimits(min_fov_, max_fov_);
304  grid.filter(*cloud_filtered_);
305 }
306 
307 template <typename PointT> bool
309 {
310  // Check if all mandatory variables have been set:
311  if (!ground_coeffs_set_)
312  {
313  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
314  return (false);
315  }
316  if (cloud_ == NULL)
317  {
318  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
319  return (false);
320  }
321  if (!intrinsics_matrix_set_)
322  {
323  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
324  return (false);
325  }
326  if (!person_classifier_set_flag_)
327  {
328  PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
329  return (false);
330  }
331 
332  // Fill rgb image:
333  rgb_image_->points.clear(); // clear RGB pointcloud
334  extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud
335 
336  // Downsample of sampling_factor in every dimension:
337  if (sampling_factor_ != 1)
338  {
339  PointCloudPtr cloud_downsampled(new PointCloud);
340  cloud_downsampled->width = (cloud_->width)/sampling_factor_;
341  cloud_downsampled->height = (cloud_->height)/sampling_factor_;
342  cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
343  cloud_downsampled->is_dense = cloud_->is_dense;
344  for (uint32_t j = 0; j < cloud_downsampled->width; j++)
345  {
346  for (uint32_t i = 0; i < cloud_downsampled->height; i++)
347  {
348  (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
349  }
350  }
351  (*cloud_) = (*cloud_downsampled);
352  }
353 
354  applyTransformationPointCloud();
355 
356  filter();
357 
358  // Ground removal and update:
359  pcl::IndicesPtr inliers(new std::vector<int>);
360  boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered_));
361  ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
362  no_ground_cloud_ = PointCloudPtr (new PointCloud);
364  extract.setInputCloud(cloud_filtered_);
365  extract.setIndices(inliers);
366  extract.setNegative(true);
367  extract.filter(*no_ground_cloud_);
368  if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
369  ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_);
370  else
371  PCL_INFO ("No groundplane update!\n");
372 
373  // Euclidean Clustering:
374  std::vector<pcl::PointIndices> cluster_indices;
376  tree->setInputCloud(no_ground_cloud_);
378  ec.setClusterTolerance(2 * voxel_size_);
379  ec.setMinClusterSize(min_points_);
380  ec.setMaxClusterSize(max_points_);
381  ec.setSearchMethod(tree);
382  ec.setInputCloud(no_ground_cloud_);
383  ec.extract(cluster_indices);
384 
385  // Head based sub-clustering //
387  subclustering.setInputCloud(no_ground_cloud_);
388  subclustering.setGround(ground_coeffs_transformed_);
389  subclustering.setInitialClusters(cluster_indices);
390  subclustering.setHeightLimits(min_height_, max_height_);
391  subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
392  subclustering.setSensorPortraitOrientation(vertical_);
393  subclustering.subcluster(clusters);
394 
395  // Person confidence evaluation with HOG+SVM:
396  if (vertical_) // Rotate the image if the camera is vertical
397  {
398  swapDimensions(rgb_image_);
399  }
400  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
401  {
402  //Evaluate confidence for the current PersonCluster:
403  Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter());
404  centroid /= centroid(2);
405  Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop());
406  top /= top(2);
407  Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom());
408  bottom /= bottom(2);
409  it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
410  }
411 
412  return (true);
413 }
414 
415 template <typename PointT>
417 {
418  // TODO Auto-generated destructor stub
419 }
420 #endif /* PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ */
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition: voxel_grid.h:393
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Definition: voxel_grid.h:410
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
void setPersonClusterLimits(float min_height, float max_height, float min_width, float max_width)
Set minimum and maximum allowed height and width for a person cluster.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:132
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setSamplingFactor(int sampling_factor)
Set sampling factor.
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
A structure representing RGB color information.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
PersonCluster represents a class for representing information about a cluster containing a person...
void applyTransformationGround()
Applies the transformation to the ground plane.
void applyTransformationIntrinsics()
Applies the transformation to the intrinsics matrix.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
void extractRGBFromPointCloud(PointCloudPtr input_cloud, pcl::PointCloud< pcl::RGB >::Ptr &output_cloud)
Extract RGB information from a point cloud and output the corresponding RGB point cloud...
Eigen::VectorXf getGround()
Get floor coefficients.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud...
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void getPersonClusterLimits(float &min_height, float &max_height, float &min_width, float &max_width)
Get the minimum and maximum allowed height and width for a person cluster.
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:73
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
void filter(PointCloud &output)
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
void setHeadCentroid(bool head_centroid)
Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole bo...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void filter()
Reduces the input cloud to one point per voxel and limits the field of view.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
ExtractIndices extracts a set of indices from a point cloud.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:223
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415