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>
boost::shared_ptr< KdTree< PointT > > Ptr
void setIntrinsics(Eigen::Matrix3f intrinsics_matrix)
Set intrinsic parameters of the RGB camera.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void setVoxelSize(float voxel_size)
Set voxel size.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setClassifier(pcl::people::PersonClassifier< pcl::RGB > person_classifier)
Set SVM-based person classifier.
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
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...
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual ~GroundBasedPeopleDetectionApp()
Destructor.
boost::shared_ptr< std::vector< int > > IndicesPtr
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by
void filter(PointCloud &output)
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points 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 setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setInputCloud(PointCloudPtr &cloud)
Set the pointer to the input cloud.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
uint32_t width
The point cloud width (if organized as an image-structure).
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(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
PointCloudPtr getNoGroundCloud()
Get pointcloud after voxel grid filtering and ground removal.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
PersonCluster represents a class for representing information about a cluster containing a person...
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
uint32_t height
The point cloud height (if organized as an image-structure).
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
HeadBasedSubclustering represents a class for searching for people inside a HeightMap2D based on a 3D...
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 getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void swapDimensions(pcl::PointCloud< pcl::RGB >::Ptr &cloud)
Swap rows/cols dimensions of a RGB point cloud (90 degrees counterclockwise rotation).
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
Eigen::VectorXf getGround()
Get floor coefficients.
bool compute(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Perform people detection on the input data and return people clusters information.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
boost::shared_ptr< PointCloud< PointT > > Ptr
void setSamplingFactor(int sampling_factor)
Set sampling factor.
boost::shared_ptr< PointCloud > PointCloudPtr
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode)...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
A structure representing RGB color information.
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
ExtractIndices extracts a set of indices from a point cloud.
GroundBasedPeopleDetectionApp()
Constructor.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...