Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2013-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * ground_based_people_detection_app.hpp 00037 * Created on: Nov 30, 2012 00038 * Author: Matteo Munaro 00039 */ 00040 00041 #ifndef PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ 00042 #define PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ 00043 00044 #include <pcl/people/ground_based_people_detection_app.h> 00045 00046 template <typename PointT> 00047 pcl::people::GroundBasedPeopleDetectionApp<PointT>::GroundBasedPeopleDetectionApp () 00048 { 00049 rgb_image_ = pcl::PointCloud<pcl::RGB>::Ptr(new pcl::PointCloud<pcl::RGB>); 00050 00051 // set default values for optional parameters: 00052 sampling_factor_ = 1; 00053 voxel_size_ = 0.06; 00054 vertical_ = false; 00055 head_centroid_ = true; 00056 min_height_ = 1.3; 00057 max_height_ = 2.3; 00058 min_points_ = 30; // this value is adapted to the voxel size in method "compute" 00059 max_points_ = 5000; // this value is adapted to the voxel size in method "compute" 00060 dimension_limits_set_ = false; 00061 heads_minimum_distance_ = 0.3; 00062 00063 // set flag values for mandatory parameters: 00064 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN(); 00065 person_classifier_set_flag_ = false; 00066 } 00067 00068 template <typename PointT> void 00069 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setInputCloud (PointCloudPtr& cloud) 00070 { 00071 cloud_ = cloud; 00072 } 00073 00074 template <typename PointT> void 00075 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setGround (Eigen::VectorXf& ground_coeffs) 00076 { 00077 ground_coeffs_ = ground_coeffs; 00078 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); 00079 } 00080 00081 template <typename PointT> void 00082 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setSamplingFactor (int sampling_factor) 00083 { 00084 sampling_factor_ = sampling_factor; 00085 } 00086 00087 template <typename PointT> void 00088 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setVoxelSize (float voxel_size) 00089 { 00090 voxel_size_ = voxel_size; 00091 } 00092 00093 template <typename PointT> void 00094 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setIntrinsics (Eigen::Matrix3f intrinsics_matrix) 00095 { 00096 intrinsics_matrix_ = intrinsics_matrix; 00097 } 00098 00099 template <typename PointT> void 00100 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setClassifier (pcl::people::PersonClassifier<pcl::RGB> person_classifier) 00101 { 00102 person_classifier_ = person_classifier; 00103 person_classifier_set_flag_ = true; 00104 } 00105 00106 template <typename PointT> void 00107 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setSensorPortraitOrientation (bool vertical) 00108 { 00109 vertical_ = vertical; 00110 } 00111 00112 template <typename PointT> void 00113 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setHeightLimits (float min_height, float max_height) 00114 { 00115 min_height_ = min_height; 00116 max_height_ = max_height; 00117 } 00118 00119 template <typename PointT> void 00120 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setDimensionLimits (int min_points, int max_points) 00121 { 00122 min_points_ = min_points; 00123 max_points_ = max_points; 00124 dimension_limits_set_ = true; 00125 } 00126 00127 template <typename PointT> void 00128 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setMinimumDistanceBetweenHeads (float heads_minimum_distance) 00129 { 00130 heads_minimum_distance_= heads_minimum_distance; 00131 } 00132 00133 template <typename PointT> void 00134 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setHeadCentroid (bool head_centroid) 00135 { 00136 head_centroid_ = head_centroid; 00137 } 00138 00139 template <typename PointT> void 00140 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getHeightLimits (float& min_height, float& max_height) 00141 { 00142 min_height = min_height_; 00143 max_height = max_height_; 00144 } 00145 00146 template <typename PointT> void 00147 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getDimensionLimits (int& min_points, int& max_points) 00148 { 00149 min_points = min_points_; 00150 max_points = max_points_; 00151 } 00152 00153 template <typename PointT> float 00154 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getMinimumDistanceBetweenHeads () 00155 { 00156 return (heads_minimum_distance_); 00157 } 00158 00159 template <typename PointT> Eigen::VectorXf 00160 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getGround () 00161 { 00162 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) 00163 { 00164 PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n"); 00165 } 00166 return (ground_coeffs_); 00167 } 00168 00169 template <typename PointT> typename pcl::people::GroundBasedPeopleDetectionApp<PointT>::PointCloudPtr 00170 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getNoGroundCloud () 00171 { 00172 return (no_ground_cloud_); 00173 } 00174 00175 template <typename PointT> void 00176 pcl::people::GroundBasedPeopleDetectionApp<PointT>::extractRGBFromPointCloud (PointCloudPtr input_cloud, pcl::PointCloud<pcl::RGB>::Ptr& output_cloud) 00177 { 00178 // Extract RGB information from a point cloud and output the corresponding RGB point cloud 00179 output_cloud->points.resize(input_cloud->height*input_cloud->width); 00180 output_cloud->width = input_cloud->width; 00181 output_cloud->height = input_cloud->height; 00182 00183 pcl::RGB rgb_point; 00184 for (int j = 0; j < input_cloud->width; j++) 00185 { 00186 for (int i = 0; i < input_cloud->height; i++) 00187 { 00188 rgb_point.r = (*input_cloud)(j,i).r; 00189 rgb_point.g = (*input_cloud)(j,i).g; 00190 rgb_point.b = (*input_cloud)(j,i).b; 00191 (*output_cloud)(j,i) = rgb_point; 00192 } 00193 } 00194 } 00195 00196 template <typename PointT> void 00197 pcl::people::GroundBasedPeopleDetectionApp<PointT>::swapDimensions (pcl::PointCloud<pcl::RGB>::Ptr& cloud) 00198 { 00199 pcl::PointCloud<pcl::RGB>::Ptr output_cloud(new pcl::PointCloud<pcl::RGB>); 00200 output_cloud->points.resize(cloud->height*cloud->width); 00201 output_cloud->width = cloud->height; 00202 output_cloud->height = cloud->width; 00203 for (int i = 0; i < cloud->width; i++) 00204 { 00205 for (int j = 0; j < cloud->height; j++) 00206 { 00207 (*output_cloud)(j,i) = (*cloud)(cloud->width - i - 1, j); 00208 } 00209 } 00210 cloud = output_cloud; 00211 } 00212 00213 template <typename PointT> bool 00214 pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters) 00215 { 00216 // Check if all mandatory variables have been set: 00217 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) 00218 { 00219 PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n"); 00220 return (false); 00221 } 00222 if (cloud_ == NULL) 00223 { 00224 PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n"); 00225 return (false); 00226 } 00227 if (intrinsics_matrix_(0) == 0) 00228 { 00229 PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n"); 00230 return (false); 00231 } 00232 if (!person_classifier_set_flag_) 00233 { 00234 PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n"); 00235 return (false); 00236 } 00237 00238 if (!dimension_limits_set_) // if dimension limits have not been set by the user 00239 { 00240 // Adapt thresholds for clusters points number to the voxel size: 00241 max_points_ = int(float(max_points_) * std::pow(0.06/voxel_size_, 2)); 00242 if (voxel_size_ > 0.06) 00243 min_points_ = int(float(min_points_) * std::pow(0.06/voxel_size_, 2)); 00244 } 00245 00246 // Fill rgb image: 00247 rgb_image_->points.clear(); // clear RGB pointcloud 00248 extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud 00249 00250 // Downsample of sampling_factor in every dimension: 00251 if (sampling_factor_ != 1) 00252 { 00253 PointCloudPtr cloud_downsampled(new PointCloud); 00254 cloud_downsampled->width = (cloud_->width)/sampling_factor_; 00255 cloud_downsampled->height = (cloud_->height)/sampling_factor_; 00256 cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width); 00257 cloud_downsampled->is_dense = cloud_->is_dense; 00258 for (int j = 0; j < cloud_downsampled->width; j++) 00259 { 00260 for (int i = 0; i < cloud_downsampled->height; i++) 00261 { 00262 (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i); 00263 } 00264 } 00265 (*cloud_) = (*cloud_downsampled); 00266 } 00267 00268 // Voxel grid filtering: 00269 PointCloudPtr cloud_filtered(new PointCloud); 00270 pcl::VoxelGrid<PointT> voxel_grid_filter_object; 00271 voxel_grid_filter_object.setInputCloud(cloud_); 00272 voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_); 00273 voxel_grid_filter_object.filter (*cloud_filtered); 00274 00275 // Ground removal and update: 00276 pcl::IndicesPtr inliers(new std::vector<int>); 00277 boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered)); 00278 ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers); 00279 no_ground_cloud_ = PointCloudPtr (new PointCloud); 00280 pcl::ExtractIndices<PointT> extract; 00281 extract.setInputCloud(cloud_filtered); 00282 extract.setIndices(inliers); 00283 extract.setNegative(true); 00284 extract.filter(*no_ground_cloud_); 00285 if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2))) 00286 ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_); 00287 else 00288 PCL_INFO ("No groundplane update!\n"); 00289 00290 // Euclidean Clustering: 00291 std::vector<pcl::PointIndices> cluster_indices; 00292 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>); 00293 tree->setInputCloud(no_ground_cloud_); 00294 pcl::EuclideanClusterExtraction<PointT> ec; 00295 ec.setClusterTolerance(2 * 0.06); 00296 ec.setMinClusterSize(min_points_); 00297 ec.setMaxClusterSize(max_points_); 00298 ec.setSearchMethod(tree); 00299 ec.setInputCloud(no_ground_cloud_); 00300 ec.extract(cluster_indices); 00301 00302 // Head based sub-clustering // 00303 pcl::people::HeadBasedSubclustering<PointT> subclustering; 00304 subclustering.setInputCloud(no_ground_cloud_); 00305 subclustering.setGround(ground_coeffs_); 00306 subclustering.setInitialClusters(cluster_indices); 00307 subclustering.setHeightLimits(min_height_, max_height_); 00308 subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_); 00309 subclustering.setSensorPortraitOrientation(vertical_); 00310 subclustering.subcluster(clusters); 00311 00312 // Person confidence evaluation with HOG+SVM: 00313 if (vertical_) // Rotate the image if the camera is vertical 00314 { 00315 swapDimensions(rgb_image_); 00316 } 00317 for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) 00318 { 00319 //Evaluate confidence for the current PersonCluster: 00320 Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter()); 00321 centroid /= centroid(2); 00322 Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop()); 00323 top /= top(2); 00324 Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom()); 00325 bottom /= bottom(2); 00326 it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_)); 00327 } 00328 00329 return (true); 00330 } 00331 00332 template <typename PointT> 00333 pcl::people::GroundBasedPeopleDetectionApp<PointT>::~GroundBasedPeopleDetectionApp () 00334 { 00335 // TODO Auto-generated destructor stub 00336 } 00337 #endif /* PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ */