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>
GroundBasedPeopleDetectionApp()50 pcl::people::GroundBasedPeopleDetectionApp<PointT>::GroundBasedPeopleDetectionApp ()
51 {
52 rgb_image_ = pcl::PointCloud<pcl::RGB>::Ptr(new pcl::PointCloud<pcl::RGB>);
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
setInputCloud(PointCloudPtr & cloud)79 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setInputCloud (PointCloudPtr& cloud)
80 {
81 cloud_ = cloud;
82 }
83
84 template <typename PointT> void
setTransformation(const Eigen::Matrix3f & transformation)85 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setTransformation (const Eigen::Matrix3f& transformation)
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
setGround(Eigen::VectorXf & ground_coeffs)99 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setGround (Eigen::VectorXf& ground_coeffs)
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
setSamplingFactor(int sampling_factor)108 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setSamplingFactor (int sampling_factor)
109 {
110 sampling_factor_ = sampling_factor;
111 }
112
113 template <typename PointT> void
setVoxelSize(float voxel_size)114 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setVoxelSize (float voxel_size)
115 {
116 voxel_size_ = voxel_size;
117 updateMinMaxPoints ();
118 }
119
120 template <typename PointT> void
setIntrinsics(Eigen::Matrix3f intrinsics_matrix)121 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setIntrinsics (Eigen::Matrix3f intrinsics_matrix)
122 {
123 intrinsics_matrix_ = intrinsics_matrix;
124 intrinsics_matrix_set_ = true;
125 applyTransformationIntrinsics();
126 }
127
128 template <typename PointT> void
setClassifier(pcl::people::PersonClassifier<pcl::RGB> person_classifier)129 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setClassifier (pcl::people::PersonClassifier<pcl::RGB> person_classifier)
130 {
131 person_classifier_ = person_classifier;
132 person_classifier_set_flag_ = true;
133 }
134
135 template <typename PointT> void
setFOV(float min_fov,float max_fov)136 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setFOV (float min_fov, float max_fov)
137 {
138 min_fov_ = min_fov;
139 max_fov_ = max_fov;
140 }
141
142 template <typename PointT> void
setSensorPortraitOrientation(bool vertical)143 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setSensorPortraitOrientation (bool vertical)
144 {
145 vertical_ = vertical;
146 }
147
148 template<typename PointT>
updateMinMaxPoints()149 void pcl::people::GroundBasedPeopleDetectionApp<PointT>::updateMinMaxPoints ()
150 {
151 min_points_ = (int) (min_height_ * min_width_ / voxel_size_ / voxel_size_);
152 max_points_ = (int) (max_height_ * max_width_ / voxel_size_ / voxel_size_);
153 }
154
155 template <typename PointT> void
setPersonClusterLimits(float min_height,float max_height,float min_width,float max_width)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
setMinimumDistanceBetweenHeads(float heads_minimum_distance)166 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setMinimumDistanceBetweenHeads (float heads_minimum_distance)
167 {
168 heads_minimum_distance_= heads_minimum_distance;
169 }
170
171 template <typename PointT> void
setHeadCentroid(bool head_centroid)172 pcl::people::GroundBasedPeopleDetectionApp<PointT>::setHeadCentroid (bool head_centroid)
173 {
174 head_centroid_ = head_centroid;
175 }
176
177 template <typename PointT> void
getPersonClusterLimits(float & min_height,float & max_height,float & min_width,float & max_width)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
getDimensionLimits(int & min_points,int & max_points)187 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getDimensionLimits (int& min_points, int& max_points)
188 {
189 min_points = min_points_;
190 max_points = max_points_;
191 }
192
193 template <typename PointT> float
getMinimumDistanceBetweenHeads()194 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getMinimumDistanceBetweenHeads ()
195 {
196 return (heads_minimum_distance_);
197 }
198
199 template <typename PointT> Eigen::VectorXf
getGround()200 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getGround ()
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
209 template <typename PointT> typename pcl::people::GroundBasedPeopleDetectionApp<PointT>::PointCloudPtr
getFilteredCloud()210 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getFilteredCloud ()
211 {
212 return (cloud_filtered_);
213 }
214
215 template <typename PointT> typename pcl::people::GroundBasedPeopleDetectionApp<PointT>::PointCloudPtr
getNoGroundCloud()216 pcl::people::GroundBasedPeopleDetectionApp<PointT>::getNoGroundCloud ()
217 {
218 return (no_ground_cloud_);
219 }
220
221 template <typename PointT> void
extractRGBFromPointCloud(PointCloudPtr input_cloud,pcl::PointCloud<pcl::RGB>::Ptr & output_cloud)222 pcl::people::GroundBasedPeopleDetectionApp<PointT>::extractRGBFromPointCloud (PointCloudPtr input_cloud, pcl::PointCloud<pcl::RGB>::Ptr& output_cloud)
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
swapDimensions(pcl::PointCloud<pcl::RGB>::Ptr & cloud)243 pcl::people::GroundBasedPeopleDetectionApp<PointT>::swapDimensions (pcl::PointCloud<pcl::RGB>::Ptr& cloud)
244 {
245 pcl::PointCloud<pcl::RGB>::Ptr output_cloud(new pcl::PointCloud<pcl::RGB>);
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
applyTransformationPointCloud()260 pcl::people::GroundBasedPeopleDetectionApp<PointT>::applyTransformationPointCloud ()
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
applyTransformationGround()271 pcl::people::GroundBasedPeopleDetectionApp<PointT>::applyTransformationGround ()
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
applyTransformationIntrinsics()286 pcl::people::GroundBasedPeopleDetectionApp<PointT>::applyTransformationIntrinsics ()
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
filter()299 pcl::people::GroundBasedPeopleDetectionApp<PointT>::filter ()
300 {
301 cloud_filtered_ = PointCloudPtr (new PointCloud);
302 pcl::VoxelGrid<PointT> grid;
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
compute(std::vector<pcl::people::PersonCluster<PointT>> & clusters)311 pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
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);
366 pcl::ExtractIndices<PointT> extract;
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;
378 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
379 tree->setInputCloud(no_ground_cloud_);
380 pcl::EuclideanClusterExtraction<PointT> ec;
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 //
389 pcl::people::HeadBasedSubclustering<PointT> subclustering;
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(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator 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>
~GroundBasedPeopleDetectionApp()419 pcl::people::GroundBasedPeopleDetectionApp<PointT>::~GroundBasedPeopleDetectionApp ()
420 {
421 // TODO Auto-generated destructor stub
422 }
423 #endif /* PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ */
424