コード例 #1
0
ファイル: head_based_subcluster.hpp プロジェクト: taketwo/pcl
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
}