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