41#ifndef PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
42#define PCL_PEOPLE_GROUND_BASED_PEOPLE_DETECTION_APP_HPP_
44#include <pcl/people/ground_based_people_detection_app.h>
45#include <pcl/filters/extract_indices.h>
46#include <pcl/segmentation/extract_clusters.h>
47#include <pcl/filters/voxel_grid.h>
49template <
typename Po
intT>
58 head_centroid_ =
true;
65 updateMinMaxPoints ();
66 heads_minimum_distance_ = 0.3;
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;
75 transformation_set_ =
false;
78template <
typename Po
intT>
void
84template <
typename Po
intT>
void
87 if (!transformation.isUnitary())
89 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::setCloudTransform] The cloud transformation matrix must be an orthogonal matrix!\n");
92 transformation_ = transformation;
93 transformation_set_ =
true;
94 applyTransformationGround();
95 applyTransformationIntrinsics();
98template <
typename Po
intT>
void
102 ground_coeffs_set_ =
true;
104 applyTransformationGround();
107template <
typename Po
intT>
void
113template <
typename Po
intT>
void
117 updateMinMaxPoints ();
120template <
typename Po
intT>
void
124 intrinsics_matrix_set_ =
true;
125 applyTransformationIntrinsics();
128template <
typename Po
intT>
void
132 person_classifier_set_flag_ =
true;
135template <
typename Po
intT>
void
142template <
typename Po
intT>
void
148template<
typename Po
intT>
151 min_points_ = (
int) (min_height_ * min_width_ / voxel_size_ / voxel_size_);
152 max_points_ = (
int) (max_height_ * max_width_ / voxel_size_ / voxel_size_);
155template <
typename Po
intT>
void
162 updateMinMaxPoints ();
165template <
typename Po
intT>
void
171template <
typename Po
intT>
void
177template <
typename Po
intT>
void
186template <
typename Po
intT>
void
193template <
typename Po
intT>
float
196 return (heads_minimum_distance_);
199template <
typename Po
intT> Eigen::VectorXf
202 if (!ground_coeffs_set_)
204 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
206 return (ground_coeffs_);
212 return (cloud_filtered_);
218 return (no_ground_cloud_);
221template <
typename Po
intT>
void
230 for (std::uint32_t j = 0; j <
input_cloud->width; j++)
232 for (std::uint32_t i = 0; i <
input_cloud->height; i++)
242template <
typename Po
intT>
void
246 output_cloud->points.resize(cloud->height*cloud->width);
249 for (std::uint32_t i = 0; i < cloud->width; i++)
251 for (std::uint32_t j = 0; j < cloud->height; j++)
253 (*output_cloud)(j,i) = (*cloud)(cloud->width - i - 1, j);
259template <
typename Po
intT>
void
262 if (transformation_set_)
264 Eigen::Transform<float, 3, Eigen::Affine> transform;
265 transform = transformation_;
270template <
typename Po
intT>
void
273 if (transformation_set_ && ground_coeffs_set_)
275 Eigen::Transform<float, 3, Eigen::Affine> transform;
276 transform = transformation_;
277 ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_;
281 ground_coeffs_transformed_ = ground_coeffs_;
285template <
typename Po
intT>
void
288 if (transformation_set_ && intrinsics_matrix_set_)
290 intrinsics_matrix_transformed_ = intrinsics_matrix_ * transformation_.transpose();
294 intrinsics_matrix_transformed_ = intrinsics_matrix_;
298template <
typename Po
intT>
void
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_);
310template <
typename Po
intT>
bool
314 if (!ground_coeffs_set_)
316 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
319 if (cloud_ ==
nullptr)
321 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
324 if (!intrinsics_matrix_set_)
326 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
329 if (!person_classifier_set_flag_)
331 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
336 rgb_image_->points.clear();
337 extractRGBFromPointCloud(cloud_, rgb_image_);
340 if (sampling_factor_ != 1)
351 (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
354 (*cloud_) = (*cloud_downsampled);
357 applyTransformationPointCloud();
367 extract.setInputCloud(cloud_filtered_);
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_);
374 PCL_INFO (
"No groundplane update!\n");
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_);
394 subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
401 swapDimensions(rgb_image_);
406 Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (
it->getTCenter());
407 centroid /= centroid(2);
408 Eigen::Vector3f top = intrinsics_matrix_transformed_ * (
it->getTTop());
410 Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (
it->getTBottom());
412 it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
418template <
typename Po
intT>
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
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.
GroundBasedPeopleDetectionApp()
Constructor.
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.
typename PointCloud::Ptr PointCloudPtr
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 setVoxelSize(float voxel_size)
Set voxel size.
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.
Eigen::VectorXf getGround()
Get floor coefficients.
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.
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
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...
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...
shared_ptr< KdTree< PointT, Tree > > Ptr
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.
shared_ptr< Indices > IndicesPtr
A structure representing RGB color information.