diff --git a/people/apps/main_ground_based_people_detection.cpp b/people/apps/main_ground_based_people_detection.cpp index 31d22f73012..b44d3197cd3 100644 --- a/people/apps/main_ground_based_people_detection.cpp +++ b/people/apps/main_ground_based_people_detection.cpp @@ -118,6 +118,8 @@ int main (int argc, char** argv) // Algorithm parameters: std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml"; float min_confidence = -1.5; + float min_width = 0.1; + float max_width = 8.0; float min_height = 1.3; float max_height = 2.3; float voxel_size = 0.06; @@ -190,7 +192,7 @@ int main (int argc, char** argv) people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier - people_detector.setHeightLimits(min_height, max_height); // set person classifier + people_detector.setPersonClusterLimits(min_height, max_height, min_width, max_width); people_detector.setSamplingFactor(sampling_factor); // set a downsampling factor to the point cloud (for increasing speed) // people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical diff --git a/people/include/pcl/people/ground_based_people_detection_app.h b/people/include/pcl/people/ground_based_people_detection_app.h index 08ff969960e..a4672c428ff 100644 --- a/people/include/pcl/people/ground_based_people_detection_app.h +++ b/people/include/pcl/people/ground_based_people_detection_app.h @@ -51,6 +51,7 @@ #include #include #include +#include namespace pcl { @@ -98,6 +99,14 @@ namespace pcl void setGround (Eigen::VectorXf& ground_coeffs); + /** + * \brief Set the transformation matrix, which is used in order to transform the given point cloud, the ground plane and the intrinsics matrix to the internal coordinate frame. + * + * \param[in] cloud A pointer to the input cloud. + */ + void + setTransformation (const Eigen::Matrix3f& transformation); + /** * \brief Set sampling factor. * @@ -130,6 +139,15 @@ namespace pcl void setClassifier (pcl::people::PersonClassifier person_classifier); + /** + * \brief Set the field of view of the point cloud in z direction. + * + * \param[in] min The beginning of the field of view in z-direction, should be usually set to zero. + * \param[in] max The end of the field of view in z-direction. + */ + void + setFOV (float min, float max); + /** * \brief Set sensor orientation (vertical = true means portrait mode, vertical = false means landscape mode). * @@ -147,22 +165,15 @@ namespace pcl setHeadCentroid (bool head_centroid); /** - * \brief Set minimum and maximum allowed height for a person cluster. + * \brief Set minimum and maximum allowed height and width for a person cluster. * * \param[in] min_height Minimum allowed height for a person cluster (default = 1.3). * \param[in] max_height Maximum allowed height for a person cluster (default = 2.3). + * \param[in] min_width Minimum width for a person cluster (default = 0.1). + * \param[in] max_width Maximum width for a person cluster (default = 8.0). */ void - setHeightLimits (float min_height, float max_height); - - /** - * \brief Set minimum and maximum allowed number of points for a person cluster. - * - * \param[in] min_points Minimum allowed number of points for a person cluster. - * \param[in] max_points Maximum allowed number of points for a person cluster. - */ - void - setDimensionLimits (int min_points, int max_points); + setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width); /** * \brief Set minimum distance between persons' heads. @@ -173,13 +184,15 @@ namespace pcl setMinimumDistanceBetweenHeads (float heads_minimum_distance); /** - * \brief Get minimum and maximum allowed height for a person cluster. + * \brief Get the minimum and maximum allowed height and width for a person cluster. * * \param[out] min_height Minimum allowed height for a person cluster. * \param[out] max_height Maximum allowed height for a person cluster. + * \param[out] min_width Minimum width for a person cluster. + * \param[out] max_width Maximum width for a person cluster. */ void - getHeightLimits (float& min_height, float& max_height); + getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width); /** * \brief Get minimum and maximum allowed number of points for a person cluster. @@ -202,6 +215,12 @@ namespace pcl Eigen::VectorXf getGround (); + /** + * \brief Get the filtered point cloud. + */ + PointCloudPtr + getFilteredCloud (); + /** * \brief Get pointcloud after voxel grid filtering and ground removal. */ @@ -225,6 +244,36 @@ namespace pcl void swapDimensions (pcl::PointCloud::Ptr& cloud); + /** + * \brief Estimates min_points_ and max_points_ based on the minimal and maximal cluster size and the voxel size. + */ + void + updateMinMaxPoints (); + + /** + * \brief Applies the transformation to the input point cloud. + */ + void + applyTransformationPointCloud (); + + /** + * \brief Applies the transformation to the ground plane. + */ + void + applyTransformationGround (); + + /** + * \brief Applies the transformation to the intrinsics matrix. + */ + void + applyTransformationIntrinsics (); + + /** + * \brief Reduces the input cloud to one point per voxel and limits the field of view. + */ + void + filter (); + /** * \brief Perform people detection on the input data and return people clusters information. * @@ -243,13 +292,28 @@ namespace pcl float voxel_size_; /** \brief ground plane coefficients */ - Eigen::VectorXf ground_coeffs_; - + Eigen::VectorXf ground_coeffs_; + + /** \brief flag stating whether the ground coefficients have been set or not */ + bool ground_coeffs_set_; + + /** \brief the transformed ground coefficients */ + Eigen::VectorXf ground_coeffs_transformed_; + /** \brief ground plane normalization factor */ - float sqrt_ground_coeffs_; - + float sqrt_ground_coeffs_; + + /** \brief rotation matrix which transforms input point cloud to internal people tracker coordinate frame */ + Eigen::Matrix3f transformation_; + + /** \brief flag stating whether the transformation matrix has been set or not */ + bool transformation_set_; + /** \brief pointer to the input cloud */ - PointCloudPtr cloud_; + PointCloudPtr cloud_; + + /** \brief pointer to the filtered cloud */ + PointCloudPtr cloud_filtered_; /** \brief pointer to the cloud after voxel grid filtering and ground removal */ PointCloudPtr no_ground_cloud_; @@ -261,8 +325,20 @@ namespace pcl float max_height_; /** \brief person clusters minimum height from the ground plane */ - float min_height_; - + float min_height_; + + /** \brief person clusters maximum width, used to estimate how many points maximally represent a person cluster */ + float max_width_; + + /** \brief person clusters minimum width, used to estimate how many points minimally represent a person cluster */ + float min_width_; + + /** \brief the beginning of the field of view in z-direction, should be usually set to zero */ + float min_fov_; + + /** \brief the end of the field of view in z-direction */ + float max_fov_; + /** \brief if true, the sensor is considered to be vertically placed (portrait mode) */ bool vertical_; @@ -276,20 +352,23 @@ namespace pcl /** \brief minimum number of points for a person cluster */ int min_points_; - /** \brief true if min_points and max_points have been set by the user, false otherwise */ - bool dimension_limits_set_; - /** \brief minimum distance between persons' heads */ float heads_minimum_distance_; /** \brief intrinsic parameters matrix of the RGB camera */ - Eigen::Matrix3f intrinsics_matrix_; - + Eigen::Matrix3f intrinsics_matrix_; + + /** \brief flag stating whether the intrinsics matrix has been set or not */ + bool intrinsics_matrix_set_; + + /** \brief the transformed intrinsics matrix */ + Eigen::Matrix3f intrinsics_matrix_transformed_; + /** \brief SVM-based person classifier */ pcl::people::PersonClassifier person_classifier_; /** \brief flag stating if the classifier has been set or not */ - bool person_classifier_set_flag_; + bool person_classifier_set_flag_; }; } /* namespace people */ } /* namespace pcl */ diff --git a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp index cce8263d5b9..19a56466d6e 100644 --- a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp +++ b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp @@ -53,16 +53,23 @@ pcl::people::GroundBasedPeopleDetectionApp::GroundBasedPeopleDetectionAp voxel_size_ = 0.06; vertical_ = false; head_centroid_ = true; + min_fov_ = 0; + max_fov_ = 50; min_height_ = 1.3; max_height_ = 2.3; - min_points_ = 30; // this value is adapted to the voxel size in method "compute" - max_points_ = 5000; // this value is adapted to the voxel size in method "compute" - dimension_limits_set_ = false; + min_width_ = 0.1; + max_width_ = 8.0; + updateMinMaxPoints (); heads_minimum_distance_ = 0.3; // set flag values for mandatory parameters: sqrt_ground_coeffs_ = std::numeric_limits::quiet_NaN(); + ground_coeffs_set_ = false; + intrinsics_matrix_set_ = false; person_classifier_set_flag_ = false; + + // set other flags + transformation_set_ = false; } template void @@ -71,11 +78,27 @@ pcl::people::GroundBasedPeopleDetectionApp::setInputCloud (PointCloudPtr cloud_ = cloud; } +template void +pcl::people::GroundBasedPeopleDetectionApp::setTransformation (const Eigen::Matrix3f& transformation) +{ + if (!transformation.isUnitary()) + { + PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::setCloudTransform] The cloud transformation matrix must be an orthogonal matrix!\n"); + } + + transformation_ = transformation; + transformation_set_ = true; + applyTransformationGround(); + applyTransformationIntrinsics(); +} + template void pcl::people::GroundBasedPeopleDetectionApp::setGround (Eigen::VectorXf& ground_coeffs) { ground_coeffs_ = ground_coeffs; + ground_coeffs_set_ = true; sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); + applyTransformationGround(); } template void @@ -88,12 +111,15 @@ template void pcl::people::GroundBasedPeopleDetectionApp::setVoxelSize (float voxel_size) { voxel_size_ = voxel_size; + updateMinMaxPoints (); } template void pcl::people::GroundBasedPeopleDetectionApp::setIntrinsics (Eigen::Matrix3f intrinsics_matrix) { intrinsics_matrix_ = intrinsics_matrix; + intrinsics_matrix_set_ = true; + applyTransformationIntrinsics(); } template void @@ -103,25 +129,34 @@ pcl::people::GroundBasedPeopleDetectionApp::setClassifier (pcl::people:: person_classifier_set_flag_ = true; } +template void +pcl::people::GroundBasedPeopleDetectionApp::setFOV (float min_fov, float max_fov) +{ + min_fov_ = min_fov; + max_fov_ = max_fov; +} + template void pcl::people::GroundBasedPeopleDetectionApp::setSensorPortraitOrientation (bool vertical) { vertical_ = vertical; } -template void -pcl::people::GroundBasedPeopleDetectionApp::setHeightLimits (float min_height, float max_height) +template +void pcl::people::GroundBasedPeopleDetectionApp::updateMinMaxPoints () { - min_height_ = min_height; - max_height_ = max_height; + min_points_ = (int) (min_height_ * min_width_ / voxel_size_ / voxel_size_); + max_points_ = (int) (max_height_ * max_width_ / voxel_size_ / voxel_size_); } template void -pcl::people::GroundBasedPeopleDetectionApp::setDimensionLimits (int min_points, int max_points) +pcl::people::GroundBasedPeopleDetectionApp::setPersonClusterLimits (float min_height, float max_height, float min_width, float max_width) { - min_points_ = min_points; - max_points_ = max_points; - dimension_limits_set_ = true; + min_height_ = min_height; + max_height_ = max_height; + min_width_ = min_width; + max_width_ = max_width; + updateMinMaxPoints (); } template void @@ -137,10 +172,12 @@ pcl::people::GroundBasedPeopleDetectionApp::setHeadCentroid (bool head_c } template void -pcl::people::GroundBasedPeopleDetectionApp::getHeightLimits (float& min_height, float& max_height) +pcl::people::GroundBasedPeopleDetectionApp::getPersonClusterLimits (float& min_height, float& max_height, float& min_width, float& max_width) { min_height = min_height_; max_height = max_height_; + min_width = min_width_; + max_width = max_width_; } template void @@ -159,13 +196,19 @@ pcl::people::GroundBasedPeopleDetectionApp::getMinimumDistanceBetweenHea template Eigen::VectorXf pcl::people::GroundBasedPeopleDetectionApp::getGround () { - if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) + if (!ground_coeffs_set_) { PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::getGround] Floor parameters have not been set or they are not valid!\n"); } return (ground_coeffs_); } +template typename pcl::people::GroundBasedPeopleDetectionApp::PointCloudPtr +pcl::people::GroundBasedPeopleDetectionApp::getFilteredCloud () +{ + return (cloud_filtered_); +} + template typename pcl::people::GroundBasedPeopleDetectionApp::PointCloudPtr pcl::people::GroundBasedPeopleDetectionApp::getNoGroundCloud () { @@ -210,11 +253,62 @@ pcl::people::GroundBasedPeopleDetectionApp::swapDimensions (pcl::PointCl cloud = output_cloud; } +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationPointCloud () +{ + if (transformation_set_) + { + Eigen::Transform transform; + transform = transformation_; + pcl::transformPointCloud(*cloud_, *cloud_, transform); + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationGround () +{ + if (transformation_set_ && ground_coeffs_set_) + { + Eigen::Transform transform; + transform = transformation_; + ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_; + } + else + { + ground_coeffs_transformed_ = ground_coeffs_; + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::applyTransformationIntrinsics () +{ + if (transformation_set_ && intrinsics_matrix_set_) + { + intrinsics_matrix_transformed_ = intrinsics_matrix_ * transformation_.transpose(); + } + else + { + intrinsics_matrix_transformed_ = intrinsics_matrix_; + } +} + +template void +pcl::people::GroundBasedPeopleDetectionApp::filter () +{ + cloud_filtered_ = PointCloudPtr (new PointCloud); + pcl::VoxelGrid grid; + grid.setInputCloud(cloud_); + grid.setLeafSize(voxel_size_, voxel_size_, voxel_size_); + grid.setFilterFieldName("z"); + grid.setFilterLimits(min_fov_, max_fov_); + grid.filter(*cloud_filtered_); +} + template bool pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector >& clusters) { // Check if all mandatory variables have been set: - if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) + if (!ground_coeffs_set_) { PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n"); return (false); @@ -224,7 +318,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector::compute (std::vector 0.06) - min_points_ = int(float(min_points_) * std::pow(0.06/voxel_size_, 2)); - } - // Fill rgb image: rgb_image_->points.clear(); // clear RGB pointcloud extractRGBFromPointCloud(cloud_, rgb_image_); // fill RGB pointcloud - + // Downsample of sampling_factor in every dimension: if (sampling_factor_ != 1) { @@ -265,25 +351,22 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector voxel_grid_filter_object; - voxel_grid_filter_object.setInputCloud(cloud_); - voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_); - voxel_grid_filter_object.filter (*cloud_filtered); + applyTransformationPointCloud(); + + filter(); // Ground removal and update: pcl::IndicesPtr inliers(new std::vector); - boost::shared_ptr > ground_model(new pcl::SampleConsensusModelPlane(cloud_filtered)); - ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers); + boost::shared_ptr > ground_model(new pcl::SampleConsensusModelPlane(cloud_filtered_)); + ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers); no_ground_cloud_ = PointCloudPtr (new PointCloud); pcl::ExtractIndices extract; - extract.setInputCloud(cloud_filtered); + extract.setInputCloud(cloud_filtered_); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*no_ground_cloud_); if (inliers->size () >= (300 * 0.06 / voxel_size_ / std::pow (static_cast (sampling_factor_), 2))) - ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_); + ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_); else PCL_INFO ("No groundplane update!\n"); @@ -292,7 +375,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector::Ptr tree (new pcl::search::KdTree); tree->setInputCloud(no_ground_cloud_); pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(2 * 0.06); + ec.setClusterTolerance(2 * voxel_size_); ec.setMinClusterSize(min_points_); ec.setMaxClusterSize(max_points_); ec.setSearchMethod(tree); @@ -302,7 +385,7 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector subclustering; subclustering.setInputCloud(no_ground_cloud_); - subclustering.setGround(ground_coeffs_); + subclustering.setGround(ground_coeffs_transformed_); subclustering.setInitialClusters(cluster_indices); subclustering.setHeightLimits(min_height_, max_height_); subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_); @@ -317,11 +400,11 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector >::iterator it = clusters.begin(); it != clusters.end(); ++it) { //Evaluate confidence for the current PersonCluster: - Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter()); + Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter()); centroid /= centroid(2); - Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop()); + Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop()); top /= top(2); - Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom()); + Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom()); bottom /= bottom(2); it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_)); } diff --git a/test/test_people_groundBasedPeopleDetectionApp.cpp b/test/test_people_groundBasedPeopleDetectionApp.cpp index 027cd87bd2c..72ce2e434d4 100644 --- a/test/test_people_groundBasedPeopleDetectionApp.cpp +++ b/test/test_people_groundBasedPeopleDetectionApp.cpp @@ -60,6 +60,8 @@ PointCloudT::Ptr cloud; pcl::people::PersonClassifier person_classifier; std::string svm_filename; float min_confidence; +float min_width; +float max_width; float min_height; float max_height; float voxel_size; @@ -79,7 +81,7 @@ TEST (PCL, GroundBasedPeopleDetectionApp) people_detector.setVoxelSize(voxel_size); // set the voxel size people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters people_detector.setClassifier(person_classifier); // set person classifier - people_detector.setHeightLimits(min_height, max_height); // set person classifier + people_detector.setPersonClusterLimits(min_height, max_height, min_width, max_width); // Perform people detection on the new cloud: std::vector > clusters; // vector containing persons clusters @@ -120,6 +122,8 @@ int main (int argc, char** argv) // Algorithm parameters: svm_filename = argv[1]; min_confidence = -1.5; + min_width = 0.1; + max_width = 8.0; min_height = 1.3; max_height = 2.3; voxel_size = 0.06;