head_based_subcluster.hpp

/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2013-, Open Perception, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * * Neither the name of the copyright holder(s) nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * head_based_subcluster.hpp
 * Created on: Nov 30, 2012
 * Author: Matteo Munaro
 */
 
#ifndef PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
#define PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
 
#include <pcl/people/head_based_subcluster.h>
 
template <typename PointT>
pcl::people::HeadBasedSubclustering<PointT>::HeadBasedSubclustering ()
{
  // set default values for optional parameters:
  vertical_ = false;
  head_centroid_ = true;
  min_height_ = 1.3;
  max_height_ = 2.3;
  min_points_ = 30;
  max_points_ = 5000;
  heads_minimum_distance_ = 0.3;
 
  // set flag values for mandatory parameters:
  sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setInputCloud (PointCloudPtr& cloud)
{
  cloud_ = cloud;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setGround (Eigen::VectorXf& ground_coeffs)
{
  ground_coeffs_ = ground_coeffs;
  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setInitialClusters (std::vector<pcl::PointIndices>& cluster_indices)
{
  cluster_indices_ = cluster_indices;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setSensorPortraitOrientation (bool vertical)
{
  vertical_ = vertical;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setHeightLimits (float min_height, float max_height)
{
  min_height_ = min_height;
  max_height_ = max_height;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setDimensionLimits (int min_points, int max_points)
{
  min_points_ = min_points;
  max_points_ = max_points;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setMinimumDistanceBetweenHeads (float heads_minimum_distance)
{
  heads_minimum_distance_= heads_minimum_distance;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::setHeadCentroid (bool head_centroid)
{
  head_centroid_ = head_centroid;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::getHeightLimits (float& min_height, float& max_height)
{
  min_height = min_height_;
  max_height = max_height_;
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::getDimensionLimits (int& min_points, int& max_points)
{
  min_points = min_points_;
  max_points = max_points_;
}
 
template <typename PointT> float
pcl::people::HeadBasedSubclustering<PointT>::getMinimumDistanceBetweenHeads ()
{
  return (heads_minimum_distance_);
}
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::mergeClustersCloseInFloorCoordinates (std::vector<pcl::people::PersonCluster<PointT> >& input_clusters,
    std::vector<pcl::people::PersonCluster<PointT> >& output_clusters)
{
  float min_distance_between_cluster_centers = 0.4;                   // meters
  float normalize_factor = std::pow(sqrt_ground_coeffs_, 2);          // sqrt_ground_coeffs ^ 2 (precomputed for speed)
  Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head<3>();        // ground plane normal (precomputed for speed)
  std::vector <std::vector<int> > connected_clusters;
  connected_clusters.resize(input_clusters.size());
  std::vector<bool> used_clusters;          // 0 in correspondence of clusters remained to process, 1 for already used clusters
  used_clusters.resize(input_clusters.size());
  for(std::size_t i = 0; i < input_clusters.size(); i++)             // for every cluster
  {
    Eigen::Vector3f theoretical_center = input_clusters[i].getTCenter();
    float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor;    // height from the ground
    Eigen::Vector3f current_cluster_center_projection = theoretical_center - head_ground_coeffs * t;    // projection of the point on the groundplane
    for(std::size_t j = i+1; j < input_clusters.size(); j++)         // for every remaining cluster
    {
      theoretical_center = input_clusters[j].getTCenter();
      float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor;    // height from the ground
      Eigen::Vector3f new_cluster_center_projection = theoretical_center - head_ground_coeffs * t;      // projection of the point on the groundplane
      if (((new_cluster_center_projection - current_cluster_center_projection).norm()) < min_distance_between_cluster_centers)
      {
        connected_clusters[i].push_back(j);
      }
    }
  }
 
  for(std::size_t i = 0; i < connected_clusters.size(); i++)   // for every cluster
  {
    if (!used_clusters[i])                                      // if this cluster has not been used yet
    {
      used_clusters[i] = true;
      if (connected_clusters[i].empty())                        // no other clusters to merge
      {
        output_clusters.push_back(input_clusters[i]);
      }
      else
      {
        // Copy cluster points into new cluster:
        pcl::PointIndices point_indices;
        point_indices = input_clusters[i].getIndices();
        for(const int &cluster : connected_clusters[i])
        {
          if (!used_clusters[cluster])         // if this cluster has not been used yet
          {
            used_clusters[cluster] = true;
            for(const auto& cluster_idx : input_clusters[cluster].getIndices().indices)
            {
              point_indices.indices.push_back(cluster_idx);
            }
          }
        }
        pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
        output_clusters.push_back(cluster);
      }
    }
  }
    }
 
template <typename PointT> void
pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::PersonCluster<PointT>& cluster, int maxima_number,
    std::vector<int>& maxima_cloud_indices, std::vector<pcl::people::PersonCluster<PointT> >& subclusters)
{
  // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices:
  float normalize_factor = std::pow(sqrt_ground_coeffs_, 2);          // sqrt_ground_coeffs ^ 2 (precomputed for speed)
  Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head<3>();        // ground plane normal (precomputed for speed)
  Eigen::Matrix3Xf maxima_projected(3,maxima_number);                 // matrix containing the projection of maxima onto the ground plane
  Eigen::VectorXi subclusters_number_of_points(maxima_number);        // subclusters number of points
  std::vector <pcl::Indices> sub_clusters_indices;                    // vector of vectors with the cluster indices for every maximum
  sub_clusters_indices.resize(maxima_number);                         // resize to number of maxima
 
  // Project maxima on the ground plane:
  for(int i = 0; i < maxima_number; i++)                              // for every maximum
  {
    PointT* current_point = &(*cloud_)[maxima_cloud_indices[i]]; // current maximum point cloud point
    Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z);  // conversion to eigen
    float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor;       // height from the ground
    maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t;         // projection of the point on the groundplane
    subclusters_number_of_points(i) = 0;                              // initialize number of points
  }
 
  // Associate cluster points to one of the maximum:
  for(const auto& cluster_idx : cluster.getIndices().indices)
  {
    Eigen::Vector3f p_current_eigen((*cloud_)[cluster_idx].x, (*cloud_)[cluster_idx].y, (*cloud_)[cluster_idx].z);  // conversion to eigen
    float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor;       // height from the ground
    p_current_eigen -= head_ground_coeffs * t;       // projection of the point on the groundplane
 
    int i = 0;
    bool correspondence_detected = false;
    while ((!correspondence_detected) && (i < maxima_number))
    {
      if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
      {
        correspondence_detected = true;
        sub_clusters_indices[i].push_back(cluster_idx);
        subclusters_number_of_points(i)++;
      }
      else
        i++;
    }
  }
 
  // Create a subcluster if the number of points associated to a maximum is over a threshold:
  for(int i = 0; i < maxima_number; i++)                              // for every maximum
  {
    if (subclusters_number_of_points(i) > min_points_)
    {
      pcl::PointIndices point_indices;
      point_indices.indices = sub_clusters_indices[i];                // indices associated to the i-th maximum
 
      pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_);
      subclusters.push_back(cluster);
      //std::cout << "Cluster number of points: " << subclusters_number_of_points(i) << std::endl;
    }
  }
}
 
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(const auto & cluster_index : cluster_indices_)
  {
    pcl::people::PersonCluster<PointT> cluster(cloud_, cluster_index, 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(std::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 = static_cast<int>(static_cast<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(auto 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
}
 
template <typename PointT>
pcl::people::HeadBasedSubclustering<PointT>::~HeadBasedSubclustering () = default;
#endif /* PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ */