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