Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
ground_based_people_detection_app.h
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.h
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #pragma once
42 
43 #include <pcl/point_types.h>
44 #include <pcl/sample_consensus/sac_model_plane.h>
45 #include <pcl/sample_consensus/ransac.h>
46 #include <pcl/filters/extract_indices.h>
47 #include <pcl/segmentation/extract_clusters.h>
48 #include <pcl/kdtree/kdtree.h>
49 #include <pcl/filters/voxel_grid.h>
50 #include <pcl/people/person_cluster.h>
51 #include <pcl/people/head_based_subcluster.h>
52 #include <pcl/people/person_classifier.h>
53 #include <pcl/common/transforms.h>
54 
55 namespace pcl
56 {
57  namespace people
58  {
59  /** \brief GroundBasedPeopleDetectionApp performs people detection on RGB-D data having as input the ground plane coefficients.
60  * It implements the people detection algorithm described here:
61  * M. Munaro, F. Basso and E. Menegatti,
62  * Tracking people within groups with RGB-D data,
63  * In Proceedings of the International Conference on Intelligent Robots and Systems (IROS) 2012, Vilamoura (Portugal), 2012.
64  *
65  * \author Matteo Munaro
66  * \ingroup people
67  */
68  template <typename PointT> class GroundBasedPeopleDetectionApp;
69 
70  template <typename PointT>
72  {
73  public:
74 
76  using PointCloudPtr = typename PointCloud::Ptr;
78 
79  /** \brief Constructor. */
81 
82  /** \brief Destructor. */
84 
85  /**
86  * \brief Set the pointer to the input cloud.
87  *
88  * \param[in] cloud A pointer to the input cloud.
89  */
90  void
92 
93  /**
94  * \brief Set the ground coefficients.
95  *
96  * \param[in] ground_coeffs Vector containing the four plane coefficients.
97  */
98  void
99  setGround (Eigen::VectorXf& ground_coeffs);
100 
101  /**
102  * \brief Set the transformation matrix, which is used in order to transform the given point cloud, the ground plane and the intrinsics matrix to the internal coordinate frame.
103  * \param[in] transformation
104  */
105  void
106  setTransformation (const Eigen::Matrix3f& transformation);
107 
108  /**
109  * \brief Set sampling factor.
110  *
111  * \param[in] sampling_factor Value of the downsampling factor (in each dimension) which is applied to the raw point cloud (default = 1.).
112  */
113  void
114  setSamplingFactor (int sampling_factor);
115 
116  /**
117  * \brief Set voxel size.
118  *
119  * \param[in] voxel_size Value of the voxel dimension (default = 0.06m.).
120  */
121  void
122  setVoxelSize (float voxel_size);
123 
124  /**
125  * \brief Set intrinsic parameters of the RGB camera.
126  *
127  * \param[in] intrinsics_matrix RGB camera intrinsic parameters matrix.
128  */
129  void
130  setIntrinsics (Eigen::Matrix3f intrinsics_matrix);
131 
132  /**
133  * \brief Set SVM-based person classifier.
134  *
135  * \param[in] person_classifier Needed for people detection on RGB data.
136  */
137  void
139 
140  /**
141  * \brief Set the field of view of the point cloud in z direction.
142  *
143  * \param[in] min The beginning of the field of view in z-direction, should be usually set to zero.
144  * \param[in] max The end of the field of view in z-direction.
145  */
146  void
147  setFOV (float min, float max);
148 
149  /**
150  * \brief Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode).
151  *
152  * \param[in] vertical Set landscape/portrait camera orientation (default = false).
153  */
154  void
155  setSensorPortraitOrientation (bool vertical);
156 
157  /**
158  * \brief Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole body centroid).
159  *
160  * \param[in] head_centroid Set the location of the person centroid (head or body center) (default = true).
161  */
162  void
163  setHeadCentroid (bool head_centroid);
164 
165  /**
166  * \brief Set minimum and maximum allowed height and width for a person cluster.
167  *
168  * \param[in] min_height Minimum allowed height for a person cluster (default = 1.3).
169  * \param[in] max_height Maximum allowed height for a person cluster (default = 2.3).
170  * \param[in] min_width Minimum width for a person cluster (default = 0.1).
171  * \param[in] max_width Maximum width for a person cluster (default = 8.0).
172  */
173  void
174  setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width);
175 
176  /**
177  * \brief Set minimum distance between persons' heads.
178  *
179  * \param[in] heads_minimum_distance Minimum allowed distance between persons' heads (default = 0.3).
180  */
181  void
182  setMinimumDistanceBetweenHeads (float heads_minimum_distance);
183 
184  /**
185  * \brief Get the minimum and maximum allowed height and width for a person cluster.
186  *
187  * \param[out] min_height Minimum allowed height for a person cluster.
188  * \param[out] max_height Maximum allowed height for a person cluster.
189  * \param[out] min_width Minimum width for a person cluster.
190  * \param[out] max_width Maximum width for a person cluster.
191  */
192  void
193  getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width);
194 
195  /**
196  * \brief Get minimum and maximum allowed number of points for a person cluster.
197  *
198  * \param[out] min_points Minimum allowed number of points for a person cluster.
199  * \param[out] max_points Maximum allowed number of points for a person cluster.
200  */
201  void
202  getDimensionLimits (int& min_points, int& max_points);
203 
204  /**
205  * \brief Get minimum distance between persons' heads.
206  */
207  float
209 
210  /**
211  * \brief Get floor coefficients.
212  */
213  Eigen::VectorXf
214  getGround ();
215 
216  /**
217  * \brief Get the filtered point cloud.
218  */
220  getFilteredCloud ();
221 
222  /**
223  * \brief Get pointcloud after voxel grid filtering and ground removal.
224  */
226  getNoGroundCloud ();
227 
228  /**
229  * \brief Extract RGB information from a point cloud and output the corresponding RGB point cloud.
230  *
231  * \param[in] input_cloud A pointer to a point cloud containing also RGB information.
232  * \param[out] output_cloud A pointer to a RGB point cloud.
233  */
234  void
236 
237  /**
238  * \brief Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
239  *
240  * \param[in,out] cloud A pointer to a RGB point cloud.
241  */
242  void
244 
245  /**
246  * \brief Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel size.
247  */
248  void
250 
251  /**
252  * \brief Applies the transformation to the input point cloud.
253  */
254  void
256 
257  /**
258  * \brief Applies the transformation to the ground plane.
259  */
260  void
262 
263  /**
264  * \brief Applies the transformation to the intrinsics matrix.
265  */
266  void
268 
269  /**
270  * \brief Reduces the input cloud to one point per voxel and limits the field of view.
271  */
272  void
273  filter ();
274 
275  /**
276  * \brief Perform people detection on the input data and return people clusters information.
277  *
278  * \param[out] clusters Vector of PersonCluster.
279  *
280  * \return true if the compute operation is successful, false otherwise.
281  */
282  bool
283  compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters);
284 
285  protected:
286  /** \brief sampling factor used to downsample the point cloud */
288 
289  /** \brief voxel size */
290  float voxel_size_;
291 
292  /** \brief ground plane coefficients */
293  Eigen::VectorXf ground_coeffs_;
294 
295  /** \brief flag stating whether the ground coefficients have been set or not */
297 
298  /** \brief the transformed ground coefficients */
299  Eigen::VectorXf ground_coeffs_transformed_;
300 
301  /** \brief ground plane normalization factor */
303 
304  /** \brief rotation matrix which transforms input point cloud to internal people tracker coordinate frame */
305  Eigen::Matrix3f transformation_;
306 
307  /** \brief flag stating whether the transformation matrix has been set or not */
309 
310  /** \brief pointer to the input cloud */
312 
313  /** \brief pointer to the filtered cloud */
315 
316  /** \brief pointer to the cloud after voxel grid filtering and ground removal */
318 
319  /** \brief pointer to a RGB cloud corresponding to cloud_ */
321 
322  /** \brief person clusters maximum height from the ground plane */
323  float max_height_;
324 
325  /** \brief person clusters minimum height from the ground plane */
326  float min_height_;
327 
328  /** \brief person clusters maximum width, used to estimate how many points maximally represent a person cluster */
329  float max_width_;
330 
331  /** \brief person clusters minimum width, used to estimate how many points minimally represent a person cluster */
332  float min_width_;
333 
334  /** \brief the beginning of the field of view in z-direction, should be usually set to zero */
335  float min_fov_;
336 
337  /** \brief the end of the field of view in z-direction */
338  float max_fov_;
339 
340  /** \brief if true, the sensor is considered to be vertically placed (portrait mode) */
341  bool vertical_;
342 
343  /** \brief if true, the person centroid is computed as the centroid of the cluster points belonging to the head;
344  * if false, the person centroid is computed as the centroid of the whole cluster points (default = true) */
345  bool head_centroid_; // if true, the person centroid is computed as the centroid of the cluster points belonging to the head (default = true)
346  // if false, the person centroid is computed as the centroid of the whole cluster points
347  /** \brief maximum number of points for a person cluster */
349 
350  /** \brief minimum number of points for a person cluster */
352 
353  /** \brief minimum distance between persons' heads */
355 
356  /** \brief intrinsic parameters matrix of the RGB camera */
357  Eigen::Matrix3f intrinsics_matrix_;
358 
359  /** \brief flag stating whether the intrinsics matrix has been set or not */
361 
362  /** \brief the transformed intrinsics matrix */
364 
365  /** \brief SVM-based person classifier */
367 
368  /** \brief flag stating if the classifier has been set or not */
370  };
371  } /* namespace people */
372 } /* namespace pcl */
373 #include <pcl/people/impl/ground_based_people_detection_app.hpp>
float min_width_
person clusters minimum width, used to estimate how many points minimally represent a person cluster ...
float min_fov_
the beginning of the field of view in z-direction, should be usually set to zero
GroundBasedPeopleDetectionApp performs people detection on RGB-D data having as input the ground plan...
Eigen::Matrix3f transformation_
rotation matrix which transforms input point cloud to internal people tracker coordinate frame ...
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
float max_width_
person clusters maximum width, used to estimate how many points maximally represent a person cluster ...
PersonCluster represents a class for representing information about a cluster containing a person...
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
int min_points_
minimum number of points for a person cluster
Eigen::VectorXf ground_coeffs_transformed_
the transformed ground coefficients
float min_height_
person clusters minimum height from the ground plane
bool person_classifier_set_flag_
flag stating if the classifier has been set or not
Eigen::VectorXf ground_coeffs_
ground plane coefficients
int sampling_factor_
sampling factor used to downsample the point cloud
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void setSamplingFactor(int sampling_factor)
Set sampling factor.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud...
void filter()
Reduces the input cloud to one point per voxel and limits the field of view.
bool ground_coeffs_set_
flag stating whether the ground coefficients have been set or not
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
Eigen::Matrix3f intrinsics_matrix_transformed_
the transformed intrinsics matrix
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
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.
float max_fov_
the end of the field of view in z-direction
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
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.
pcl::PointCloud< pcl::RGB >::Ptr rgb_image_
pointer to a RGB cloud corresponding to cloud_
pcl::people::PersonClassifier< pcl::RGB > person_classifier_
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...
PointCloudPtr cloud_filtered_
pointer to the filtered cloud
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
Eigen::Matrix3f intrinsics_matrix_
intrinsic parameters matrix of the RGB camera
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...
bool vertical_
if true, the sensor is considered to be vertically placed (portrait mode)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void applyTransformationIntrinsics()
Applies the transformation to the intrinsics matrix.
void applyTransformationGround()
Applies the transformation to the ground plane.
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
PointCloudPtr no_ground_cloud_
pointer to the cloud after voxel grid filtering and ground removal
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
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.
float heads_minimum_distance_
minimum distance between persons' heads
bool transformation_set_
flag stating whether the transformation matrix has been set or not
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
bool head_centroid_
if true, the person centroid is computed as the centroid of the cluster points belonging to the head;...
bool intrinsics_matrix_set_
flag stating whether the intrinsics matrix has been set or not
int max_points_
maximum number of points for a person cluster
float sqrt_ground_coeffs_
ground plane normalization factor
float max_height_
person clusters maximum height from the ground plane
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.