Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
ground_based_people_detection_app.hpp
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
49template <typename PointT>
51{
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
78template <typename PointT> void
83
84template <typename PointT> void
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
98template <typename PointT> void
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
107template <typename PointT> void
109{
110 sampling_factor_ = sampling_factor;
111}
112
113template <typename PointT> void
115{
116 voxel_size_ = voxel_size;
117 updateMinMaxPoints ();
118}
119
120template <typename PointT> void
122{
123 intrinsics_matrix_ = intrinsics_matrix;
124 intrinsics_matrix_set_ = true;
125 applyTransformationIntrinsics();
126}
127
128template <typename PointT> void
130{
131 person_classifier_ = person_classifier;
132 person_classifier_set_flag_ = true;
133}
134
135template <typename PointT> void
137{
138 min_fov_ = min_fov;
139 max_fov_ = max_fov;
140}
141
142template <typename PointT> void
147
148template<typename PointT>
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
155template <typename PointT> void
156pcl::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
165template <typename PointT> void
167{
168 heads_minimum_distance_= heads_minimum_distance;
169}
170
171template <typename PointT> void
173{
174 head_centroid_ = head_centroid;
175}
176
177template <typename PointT> void
178pcl::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
186template <typename PointT> void
188{
189 min_points = min_points_;
190 max_points = max_points_;
191}
192
193template <typename PointT> float
198
199template <typename PointT> Eigen::VectorXf
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
214
220
221template <typename PointT> void
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
242template <typename PointT> void
244{
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
259template <typename PointT> void
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
270template <typename PointT> void
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
285template <typename PointT> void
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
298template <typename PointT> void
300{
301 cloud_filtered_ = PointCloudPtr (new PointCloud);
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
310template <typename PointT> bool
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);
364 ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
365 no_ground_cloud_ = PointCloudPtr (new PointCloud);
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;
379 tree->setInputCloud(no_ground_cloud_);
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 //
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
418template <typename PointT>
423#endif /* PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_ */
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void setMaxClusterSize(pcl::uindex_t max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid.
void setMinClusterSize(pcl::uindex_t min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid.
ExtractIndices extracts a set of indices from a point cloud.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition filter.h:121
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the plane coefficients using the given inlier set and return them to the user.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
shared_ptr< SampleConsensusModelPlane< PointT > > Ptr
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition voxel_grid.h:391
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:221
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Definition voxel_grid.h:408
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void filter()
Reduces the input cloud to one point per voxel and limits the field of view.
void applyTransformationIntrinsics()
Applies the transformation to the intrinsics matrix.
void setSamplingFactor(int sampling_factor)
Set sampling factor.
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.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set 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...
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
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.
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
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.
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud,...
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void applyTransformationGround()
Applies the transformation to the ground plane.
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode).
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
PersonCluster represents a class for representing information about a cluster containing a person.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition kdtree.hpp:76
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
A structure representing RGB color information.