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;
227 for (uint32_t j = 0; j < input_cloud->width; j++)
229 for (uint32_t i = 0; i < input_cloud->height; i++)
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
246 for (uint32_t i = 0; i < cloud->
width; i++)
248 for (uint32_t j = 0; j < cloud->
height; j++)
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");
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;
344 for (uint32_t j = 0; j < cloud_downsampled->width; j++)
346 for (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();
361 ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
367 extract.
filter(*no_ground_cloud_);
368 if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
369 ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_);
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>
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
boost::shared_ptr< std::vector< int > > IndicesPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
uint32_t width
The point cloud width (if organized as an image-structure).
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
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 filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setSamplingFactor(int sampling_factor)
Set sampling factor.
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A structure representing RGB color information.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
PersonCluster represents a class for representing information about a cluster containing a person...
boost::shared_ptr< PointCloud > PointCloudPtr
void applyTransformationGround()
Applies the transformation to the ground plane.
void applyTransformationIntrinsics()
Applies the transformation to the intrinsics matrix.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
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...
Eigen::VectorXf getGround()
Get floor coefficients.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud...
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 setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
GroundBasedPeopleDetectionApp()
Constructor.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void filter(PointCloud &output)
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 setVoxelSize(float voxel_size)
Set voxel size.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void filter()
Reduces the input cloud to one point per voxel and limits the field of view.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
uint32_t height
The point cloud height (if organized as an image-structure).