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