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_ */
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
void filter(PointCloud &output)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
ExtractIndices extracts a set of indices from a point cloud.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void setSamplingFactor(int sampling_factor)
Set sampling factor.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
SampleConsensusModelPlane defines a model for 3D plane segmentation.
A structure representing RGB color information.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:79
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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...
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
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 filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:131
PersonCluster represents a class for representing information about a cluster containing a person...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
void getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:222
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Eigen::VectorXf getGround()
Get floor coefficients.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.