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>
boost::shared_ptr< PointCloud< PointT > > Ptr
boost::shared_ptr< PointCloud > PointCloudPtr
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...
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
uint32_t width
The point cloud width (if organized as an image-structure).
PersonCluster represents a class for representing information about a cluster containing a person...
GroundBasedPeopleDetectionApp()
Constructor.
boost::shared_ptr< std::vector< int > > IndicesPtr
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void applyTransformationPointCloud()
Applies the transformation to the input point cloud.
void setSamplingFactor(int sampling_factor)
Set sampling factor.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setTransformation(const Eigen::Matrix3f &transformation)
Set the transformation matrix, which is used in order to transform the given point cloud...
void filter()
Reduces the input cloud to one point per voxel and limits the field of view.
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...
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void updateMinMaxPoints()
Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel siz...
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
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.
A structure representing RGB color information.
PointCloudPtr getFilteredCloud()
Get the filtered point cloud.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Eigen::VectorXf getGround()
Get floor coefficients.
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=true)
Apply an affine transform defined by an Eigen Transform.
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.
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 setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
uint32_t height
The point cloud height (if organized as an image-structure).
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...
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void applyTransformationIntrinsics()
Applies the transformation to the intrinsics matrix.
void filter(PointCloud &output)
void applyTransformationGround()
Applies the transformation to the ground plane.
void setFOV(float min, float max)
Set the field of view of the point cloud in z direction.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
void setVoxelSize(float voxel_size)
Set voxel size.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.