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