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;
60 dimension_limits_set_ =
false;
61 heads_minimum_distance_ = 0.3;
64 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
65 person_classifier_set_flag_ =
false;
68 template <
typename Po
intT>
void
74 template <
typename Po
intT>
void
77 ground_coeffs_ = ground_coeffs;
78 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
81 template <
typename Po
intT>
void
84 sampling_factor_ = sampling_factor;
87 template <
typename Po
intT>
void
90 voxel_size_ = voxel_size;
93 template <
typename Po
intT>
void
96 intrinsics_matrix_ = intrinsics_matrix;
99 template <
typename Po
intT>
void
102 person_classifier_ = person_classifier;
103 person_classifier_set_flag_ =
true;
106 template <
typename Po
intT>
void
109 vertical_ = vertical;
112 template <
typename Po
intT>
void
115 min_height_ = min_height;
116 max_height_ = max_height;
119 template <
typename Po
intT>
void
122 min_points_ = min_points;
123 max_points_ = max_points;
124 dimension_limits_set_ =
true;
127 template <
typename Po
intT>
void
130 heads_minimum_distance_= heads_minimum_distance;
133 template <
typename Po
intT>
void
136 head_centroid_ = head_centroid;
139 template <
typename Po
intT>
void
142 min_height = min_height_;
143 max_height = max_height_;
146 template <
typename Po
intT>
void
149 min_points = min_points_;
150 max_points = max_points_;
153 template <
typename Po
intT>
float
156 return (heads_minimum_distance_);
159 template <
typename Po
intT> Eigen::VectorXf
162 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
164 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n");
166 return (ground_coeffs_);
172 return (no_ground_cloud_);
175 template <
typename Po
intT>
void
179 output_cloud->
points.resize(input_cloud->height*input_cloud->width);
180 output_cloud->
width = input_cloud->width;
181 output_cloud->
height = input_cloud->height;
184 for (
int j = 0; j < input_cloud->width; j++)
186 for (
int i = 0; i < input_cloud->height; i++)
188 rgb_point.r = (*input_cloud)(j,i).r;
189 rgb_point.g = (*input_cloud)(j,i).g;
190 rgb_point.b = (*input_cloud)(j,i).b;
191 (*output_cloud)(j,i) = rgb_point;
196 template <
typename Po
intT>
void
203 for (
int i = 0; i < cloud->
width; i++)
205 for (
int j = 0; j < cloud->
height; j++)
207 (*output_cloud)(j,i) = (*cloud)(cloud->
width - i - 1, j);
210 cloud = output_cloud;
213 template <
typename Po
intT>
bool
217 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
219 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
224 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
227 if (intrinsics_matrix_(0) == 0)
229 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
232 if (!person_classifier_set_flag_)
234 PCL_ERROR (
"[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
238 if (!dimension_limits_set_)
241 max_points_ = int(
float(max_points_) * std::pow(0.06/voxel_size_, 2));
242 if (voxel_size_ > 0.06)
243 min_points_ = int(
float(min_points_) * std::pow(0.06/voxel_size_, 2));
247 rgb_image_->points.clear();
248 extractRGBFromPointCloud(cloud_, rgb_image_);
251 if (sampling_factor_ != 1)
254 cloud_downsampled->width = (cloud_->width)/sampling_factor_;
255 cloud_downsampled->height = (cloud_->height)/sampling_factor_;
256 cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
257 cloud_downsampled->is_dense = cloud_->is_dense;
258 for (
int j = 0; j < cloud_downsampled->width; j++)
260 for (
int i = 0; i < cloud_downsampled->height; i++)
262 (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
265 (*cloud_) = (*cloud_downsampled);
272 voxel_grid_filter_object.
setLeafSize (voxel_size_, voxel_size_, voxel_size_);
273 voxel_grid_filter_object.
filter (*cloud_filtered);
278 ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
284 extract.
filter(*no_ground_cloud_);
285 if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast<double> (sampling_factor_), 2)))
286 ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_);
288 PCL_INFO (
"No groundplane update!\n");
291 std::vector<pcl::PointIndices> cluster_indices;
315 swapDimensions(rgb_image_);
320 Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter());
321 centroid /= centroid(2);
322 Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop());
324 Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom());
326 it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
332 template <
typename Po
intT>
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
void filter(PointCloud &output)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
ExtractIndices extracts a set of indices from a point cloud.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
uint32_t width
The point cloud width (if organized as an image-structure).
void setSamplingFactor(int sampling_factor)
Set sampling factor.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
boost::shared_ptr< PointCloud > PointCloudPtr
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 subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
GroundBasedPeopleDetectionApp()
Constructor.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void setVoxelSize(float voxel_size)
Set voxel size.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
A structure representing RGB color information.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
boost::shared_ptr< PointCloud< PointT > > Ptr
boost::shared_ptr< KdTree< PointT > > Ptr
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...
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
boost::shared_ptr< std::vector< int > > IndicesPtr
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
PersonCluster represents a class for representing information about a cluster containing a person...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
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 setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Eigen::VectorXf getGround()
Get floor coefficients.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.