Point Cloud Library (PCL)
1.11.0
|
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>
46 template <
typename Po
intT>
55 head_centroid_ =
true;
62 updateMinMaxPoints ();
63 heads_minimum_distance_ = 0.3;
66 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
67 ground_coeffs_set_ =
false;
68 intrinsics_matrix_set_ =
false;
69 person_classifier_set_flag_ =
false;
72 transformation_set_ =
false;
75 template <
typename Po
intT>
void
81 template <
typename Po
intT>
void
84 if (!transformation.isUnitary())
86 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::setCloudTransform] The cloud transformation matrix must be an orthogonal matrix!\n");
89 transformation_ = transformation;
90 transformation_set_ =
true;
91 applyTransformationGround();
92 applyTransformationIntrinsics();
95 template <
typename Po
intT>
void
98 ground_coeffs_ = ground_coeffs;
99 ground_coeffs_set_ =
true;
100 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
101 applyTransformationGround();
104 template <
typename Po
intT>
void
107 sampling_factor_ = sampling_factor;
110 template <
typename Po
intT>
void
113 voxel_size_ = voxel_size;
114 updateMinMaxPoints ();
117 template <
typename Po
intT>
void
120 intrinsics_matrix_ = intrinsics_matrix;
121 intrinsics_matrix_set_ =
true;
122 applyTransformationIntrinsics();
125 template <
typename Po
intT>
void
128 person_classifier_ = person_classifier;
129 person_classifier_set_flag_ =
true;
132 template <
typename Po
intT>
void
139 template <
typename Po
intT>
void
142 vertical_ = vertical;
145 template<
typename Po
intT>
148 min_points_ = (int) (min_height_ * min_width_ / voxel_size_ / voxel_size_);
149 max_points_ = (int) (max_height_ * max_width_ / voxel_size_ / voxel_size_);
152 template <
typename Po
intT>
void
155 min_height_ = min_height;
156 max_height_ = max_height;
157 min_width_ = min_width;
158 max_width_ = max_width;
159 updateMinMaxPoints ();
162 template <
typename Po
intT>
void
165 heads_minimum_distance_= heads_minimum_distance;
168 template <
typename Po
intT>
void
171 head_centroid_ = head_centroid;
174 template <
typename Po
intT>
void
177 min_height = min_height_;
178 max_height = max_height_;
179 min_width = min_width_;
180 max_width = max_width_;
183 template <
typename Po
intT>
void
186 min_points = min_points_;
187 max_points = max_points_;
190 template <
typename Po
intT>
float
193 return (heads_minimum_distance_);
196 template <
typename Po
intT> Eigen::VectorXf
199 if (!ground_coeffs_set_)
201 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
203 return (ground_coeffs_);
209 return (cloud_filtered_);
215 return (no_ground_cloud_);
218 template <
typename Po
intT>
void
222 output_cloud->
points.resize(input_cloud->height*input_cloud->width);
223 output_cloud->
width = input_cloud->width;
224 output_cloud->
height = input_cloud->height;
231 rgb_point.r = (*input_cloud)(j,i).r;
232 rgb_point.g = (*input_cloud)(j,i).g;
233 rgb_point.b = (*input_cloud)(j,i).b;
234 (*output_cloud)(j,i) = rgb_point;
239 template <
typename Po
intT>
void
250 (*output_cloud)(j,i) = (*cloud)(cloud->
width - i - 1, j);
253 cloud = output_cloud;
256 template <
typename Po
intT>
void
259 if (transformation_set_)
261 Eigen::Transform<float, 3, Eigen::Affine> transform;
262 transform = transformation_;
267 template <
typename Po
intT>
void
270 if (transformation_set_ && ground_coeffs_set_)
272 Eigen::Transform<float, 3, Eigen::Affine> transform;
273 transform = transformation_;
274 ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_;
278 ground_coeffs_transformed_ = ground_coeffs_;
282 template <
typename Po
intT>
void
285 if (transformation_set_ && intrinsics_matrix_set_)
287 intrinsics_matrix_transformed_ = intrinsics_matrix_ * transformation_.transpose();
291 intrinsics_matrix_transformed_ = intrinsics_matrix_;
295 template <
typename Po
intT>
void
301 grid.
setLeafSize(voxel_size_, voxel_size_, voxel_size_);
304 grid.
filter(*cloud_filtered_);
307 template <
typename Po
intT>
bool
311 if (!ground_coeffs_set_)
313 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
316 if (cloud_ ==
nullptr)
318 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
321 if (!intrinsics_matrix_set_)
323 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
326 if (!person_classifier_set_flag_)
328 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
333 rgb_image_->points.clear();
334 extractRGBFromPointCloud(cloud_, rgb_image_);
337 if (sampling_factor_ != 1)
340 cloud_downsampled->width = (cloud_->width)/sampling_factor_;
341 cloud_downsampled->height = (cloud_->height)/sampling_factor_;
342 cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
343 cloud_downsampled->is_dense = cloud_->is_dense;
346 for (
std::uint32_t i = 0; i < cloud_downsampled->height; i++)
348 (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
351 (*cloud_) = (*cloud_downsampled);
354 applyTransformationPointCloud();
367 extract.
filter(*no_ground_cloud_);
368 if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (
static_cast<double> (sampling_factor_), 2)))
371 PCL_INFO (
"No groundplane update!\n");
374 std::vector<pcl::PointIndices> cluster_indices;
388 subclustering.
setGround(ground_coeffs_transformed_);
398 swapDimensions(rgb_image_);
403 Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter());
404 centroid /= centroid(2);
405 Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop());
407 Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom());
409 it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
415 template <
typename Po
intT>
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
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.
shared_ptr< Indices > IndicesPtr
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud,...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
Eigen::VectorXf getGround()
Get floor coefficients.
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
shared_ptr< SampleConsensusModelPlane< PointT > > Ptr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
shared_ptr< KdTree< PointT, Tree > > Ptr
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions.
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
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 transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
void applyTransformationGround()
Applies the transformation to the ground plane.
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
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 setVoxelSize(float voxel_size)
Set voxel size.
GroundBasedPeopleDetectionApp()
Constructor.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
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.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setSamplingFactor(int sampling_factor)
Set sampling factor.
A structure representing RGB color information.
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode).
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
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.
PersonCluster represents a class for representing information about a cluster containing a person.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
shared_ptr< PointCloud< PointT > > Ptr
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
void filter(std::vector< int > &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
typename PointCloud::Ptr PointCloudPtr
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.