template <typename PointT> void pcl::people::HeadBasedSubclustering<PointT>::subcluster (std::vector<pcl::people::PersonCluster<PointT> >& clusters) { // Check if all mandatory variables have been set: if (std::isnan(sqrt_ground_coeffs_)) { PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n"); return; } if (cluster_indices_.empty ()) { PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n"); return; } if (cloud_ == nullptr) { PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n"); return; } // Person clusters creation from clusters indices: for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it) { pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation clusters.push_back(cluster); } // Remove clusters with too high height from the ground plane: std::vector<pcl::people::PersonCluster<PointT> > new_clusters; for(size_t i = 0; i < clusters.size(); i++) // for every cluster { if (clusters[i].getHeight() <= max_height_) new_clusters.push_back(clusters[i]); } clusters = new_clusters; new_clusters.clear(); // Merge clusters close in floor coordinates: mergeClustersCloseInFloorCoordinates(clusters, new_clusters); clusters = new_clusters; std::vector<pcl::people::PersonCluster<PointT> > subclusters; int cluster_min_points_sub = int(float(min_points_) * 1.5); // int cluster_max_points_sub = max_points_; // create HeightMap2D object: pcl::people::HeightMap2D<PointT> height_map_obj; height_map_obj.setGround(ground_coeffs_); height_map_obj.setInputCloud(cloud_); height_map_obj.setSensorPortraitOrientation(vertical_); height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_); for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster { float height = it->getHeight(); int number_of_points = it->getNumberPoints(); if(height > min_height_ && height < max_height_) { if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub) { // Compute height map associated to the current cluster and its local maxima (heads): height_map_obj.compute(*it); if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum { // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices: createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters); } else { // Only one maximum --> copy original cluster: subclusters.push_back(*it); } } else { // Cluster properties not good for sub-clustering --> copy original cluster: subclusters.push_back(*it); } } } clusters = subclusters; // substitute clusters with subclusters }