Esempio n. 1
0
bool TOMPCFilter<T>::segmentation(CloudPtr &cloud_in, VecCloud &clouds_out, double tolerance, double minSize, double maxSize)
{
    //    std::cout << "Object segmentation" << std::endl;
    // object clustering
    // Creating the KdTree object for the search method of the extraction
    typename pcl::search::KdTree<T>::Ptr tree (new pcl::search::KdTree<T>);
    tree->setInputCloud (cloud_in);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<T> ec;
    ec.setClusterTolerance (tolerance); // 2cm
    ec.setMinClusterSize (minSize);
    ec.setMaxClusterSize (maxSize);
    ec.setSearchMethod (tree);
    ec.setInputCloud( cloud_in);
    ec.extract (cluster_indices);

    int j = 0;
    clouds_out.clear();
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        CloudPtr cloud_cluster (new pcl::PointCloud<T>);
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
            cloud_cluster->points.push_back (cloud_in->points[*pit]); //*
        clouds_out.push_back(cloud_cluster);
    }
    return 1;
}
Esempio n. 2
0
	void performTracking(const PointCloudConstPtr& cloud)
	{
		typename pcl::search::KdTree<PointType>::Ptr search(
				new pcl::search::KdTree<PointType>());
		search->setInputCloud(cloud);
		performTracking(cloud, search);
	}
Esempio n. 3
0
void ICCVTutorial<FeatureType>::segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const
{
  cout << "segmentation..." << std::flush;
  // fit plane and keep points above that plane
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.02);

  seg.setInputCloud (source);
  seg.segment (*inliers, *coefficients);

  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  extract.setInputCloud (source);
  extract.setIndices (inliers);
  extract.setNegative (true);

  extract.filter (*segmented);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
  cout << "OK" << endl;

  cout << "clustering..." << std::flush;
  // euclidean clustering
  typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (segmented);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
  clustering.setClusterTolerance (0.02); // 2cm
  clustering.setMinClusterSize (1000);
  clustering.setMaxClusterSize (250000);
  clustering.setSearchMethod (tree);
  clustering.setInputCloud(segmented);
  clustering.extract (cluster_indices);

  if (!cluster_indices.empty ())//use largest cluster
  {
    cout << cluster_indices.size() << " clusters found";
    if (cluster_indices.size() > 1)
      cout <<" Using largest one...";
    cout << endl;
    typename pcl::IndicesPtr indices (new std::vector<int>);
    *indices = cluster_indices[0].indices;
    extract.setInputCloud (segmented);
    extract.setIndices (indices);
    extract.setNegative (false);

    extract.filter (*segmented);
  }
}
Esempio n. 4
0
File: gicp.hpp Progetto: 87west/pcl
template<typename PointT> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
                                                                                    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
                                                                                    std::vector<Eigen::Matrix3d>& cloud_covariances)
{
  if (k_correspondences_ > int (cloud->size ()))
  {
    PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
    return;
  }

  Eigen::Vector3d mean;
  std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
  std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);

  // We should never get there but who knows
  if(cloud_covariances.size () < cloud->size ())
    cloud_covariances.resize (cloud->size ());

  typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
  std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
  for(;
      points_iterator != cloud->end ();
      ++points_iterator, ++matrices_iterator)
  {
    const PointT &query_point = *points_iterator;
    Eigen::Matrix3d &cov = *matrices_iterator;
    // Zero out the cov and mean
    cov.setZero ();
    mean.setZero ();

    // Search for the K nearest neighbours
    kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
    
    // Find the covariance matrix
    for(int j = 0; j < k_correspondences_; j++) {
      const PointT &pt = (*cloud)[nn_indecies[j]];
      
      mean[0] += pt.x;
      mean[1] += pt.y;
      mean[2] += pt.z;
      
      cov(0,0) += pt.x*pt.x;
      
      cov(1,0) += pt.y*pt.x;
      cov(1,1) += pt.y*pt.y;
      
      cov(2,0) += pt.z*pt.x;
      cov(2,1) += pt.z*pt.y;
      cov(2,2) += pt.z*pt.z;    
    }
  
    mean /= static_cast<double> (k_correspondences_);
    // Get the actual covariance
    for (int k = 0; k < 3; k++)
      for (int l = 0; l <= k; l++) 
      {
        cov(k,l) /= static_cast<double> (k_correspondences_);
        cov(k,l) -= mean[k]*mean[l];
        cov(l,k) = cov(k,l);
      }
    
    // Compute the SVD (covariance matrix is symmetric so U = V')
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
    cov.setZero ();
    Eigen::Matrix3d U = svd.matrixU ();
    // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
    for(int k = 0; k < 3; k++) {
      Eigen::Vector3d col = U.col(k);
      double v = 1.; // biggest 2 singular values replaced by 1
      if(k == 2)   // smallest singular value replaced by gicp_epsilon
        v = gicp_epsilon_;
      cov+= v * col * col.transpose(); 
    }
  }
}
void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr> & clusters)
{

  // Has the input dataset been set already?
  if (!input_)
  {
    PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
    return;
  }

  CloudConstPtr cloud_;
  CloudPtr cloud_filtered_ (new Cloud ());
  CloudPtr cloud_downsampled_ (new Cloud ());
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
  pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
  pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
  CloudPtr table_projected_ (new Cloud ());
  CloudPtr table_hull_ (new Cloud ());
  CloudPtr cloud_objects_ (new Cloud ());
  CloudPtr cluster_object_ (new Cloud ());

  typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
  typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
  clusters_tree_->setEpsilon (1);

  // Normal estimation parameters
  n3d_.setKSearch (10);
  n3d_.setSearchMethod (normals_tree_);

  // Table model fitting parameters
  seg_.setDistanceThreshold (sac_distance_threshold_);
  seg_.setMaxIterations (2000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_MSAC);
  seg_.setProbability (0.98);

  proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);

  prism_.setHeightLimits (object_min_height_, object_max_height_);

  // Clustering parameters
  cluster_.setClusterTolerance (object_cluster_tolerance_);
  cluster_.setMinClusterSize (object_cluster_min_size_);
  cluster_.setSearchMethod (clusters_tree_);

  // ---[ PassThroughFilter
  pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
  pass_.setFilterFieldName ("z");
  pass_.setInputCloud (input_);
  pass_.filter (*cloud_filtered_);

  if (int (cloud_filtered_->points.size ()) < k_)
  {
    PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
        cloud_filtered_->points.size ());
    return;
  }

  // Downsample the point cloud
  grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
  grid_.setDownsampleAllData (false);
  grid_.setInputCloud (cloud_filtered_);
  grid_.filter (*cloud_downsampled_);

  PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %zu out of %zu\n",
      min_z_bounds_, max_z_bounds_, cloud_downsampled_->points.size (), input_->points.size ());

  // ---[ Estimate the point normals
  n3d_.setInputCloud (cloud_downsampled_);
  n3d_.compute (*cloud_normals_);

  PCL_INFO ("[DominantPlaneSegmentation] %zu normals estimated. \n", cloud_normals_->points.size ());

  // ---[ Perform segmentation
  seg_.setInputCloud (cloud_downsampled_);
  seg_.setInputNormals (cloud_normals_);
  seg_.segment (*table_inliers_, *table_coefficients_);

  if (table_inliers_->indices.size () == 0)
  {
    PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
    return;
  }

  // ---[ Extract the plane
  proj_.setInputCloud (cloud_downsampled_);
  proj_.setIndices (table_inliers_);
  proj_.setModelCoefficients (table_coefficients_);
  proj_.filter (*table_projected_);

  // ---[ Estimate the convex hull
  std::vector<pcl::Vertices> polygons;
  CloudPtr table_hull (new Cloud ());
  hull_.setInputCloud (table_projected_);
  hull_.reconstruct (*table_hull, polygons);

  // Compute the plane coefficients
  Eigen::Vector4f model_coefficients;
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;

  model_coefficients[0] = table_coefficients_->values[0];
  model_coefficients[1] = table_coefficients_->values[1];
  model_coefficients[2] = table_coefficients_->values[2];
  model_coefficients[3] = table_coefficients_->values[3];

  // Need to flip the plane normal towards the viewpoint
  Eigen::Vector4f vp (0, 0, 0, 0);
  // See if we need to flip any plane normals
  vp -= table_hull->points[0].getVector4fMap ();
  vp[3] = 0;
  // Dot product between the (viewpoint - point) and the plane normal
  float cos_theta = vp.dot (model_coefficients);
  // Flip the plane normal
  if (cos_theta < 0)
  {
    model_coefficients *= -1;
    model_coefficients[3] = 0;
    // Hessian form (D = nc . p_plane (centroid here) + p)
    model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
  }

  //Set table_coeffs
  table_coeffs_ = model_coefficients;

  // ---[ Get the objects on top of the table
  pcl::PointIndices cloud_object_indices;
  prism_.setInputCloud (cloud_filtered_);
  prism_.setInputPlanarHull (table_hull);
  prism_.segment (cloud_object_indices);

  pcl::ExtractIndices<PointType> extract_object_indices;
  extract_object_indices.setInputCloud (cloud_downsampled_);
  extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  extract_object_indices.filter (*cloud_objects_);

  if (cloud_objects_->points.size () == 0)
    return;

  // ---[ Split the objects into Euclidean clusters
  std::vector<pcl::PointIndices> clusters2;
  cluster_.setInputCloud (cloud_filtered_);
  cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  cluster_.extract (clusters2);

  PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %zu.\n",
      clusters2.size ());

  clusters.resize (clusters2.size ());

  for (size_t i = 0; i < clusters2.size (); ++i)
  {
    clusters[i] = CloudPtr (new Cloud ());
    pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]);
  }
}
template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr> & clusters)
{
  // Has the input dataset been set already?
  if (!input_)
  {
    PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
    return;
  }

  // Is the input dataset organized?
  if (input_->is_dense)
  {
    PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n");
    return;
  }

  CloudConstPtr cloud_;
  CloudPtr cloud_filtered_ (new Cloud ());
  CloudPtr cloud_downsampled_ (new Cloud ());
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
  pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
  pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
  CloudPtr table_projected_ (new Cloud ());
  CloudPtr table_hull_ (new Cloud ());
  CloudPtr cloud_objects_ (new Cloud ());
  CloudPtr cluster_object_ (new Cloud ());

  typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
  typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
  clusters_tree_->setEpsilon (1);

  // Normal estimation parameters
  n3d_.setKSearch (k_);
  n3d_.setSearchMethod (normals_tree_);

  // Table model fitting parameters
  seg_.setDistanceThreshold (sac_distance_threshold_);
  seg_.setMaxIterations (2000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);

  proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);

  prism_.setHeightLimits (object_min_height_, object_max_height_);

  // Clustering parameters
  cluster_.setClusterTolerance (object_cluster_tolerance_);
  cluster_.setMinClusterSize (object_cluster_min_size_);
  cluster_.setSearchMethod (clusters_tree_);

  // ---[ PassThroughFilter
  pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
  pass_.setFilterFieldName ("z");
  pass_.setInputCloud (input_);
  pass_.filter (*cloud_filtered_);

  if (int (cloud_filtered_->points.size ()) < k_)
  {
    PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
        cloud_filtered_->points.size ());
    return;
  }

  // Downsample the point cloud
  grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
  grid_.setDownsampleAllData (false);
  grid_.setInputCloud (cloud_filtered_);
  grid_.filter (*cloud_downsampled_);

  // ---[ Estimate the point normals
  n3d_.setInputCloud (cloud_downsampled_);
  n3d_.compute (*cloud_normals_);

  // ---[ Perform segmentation
  seg_.setInputCloud (cloud_downsampled_);
  seg_.setInputNormals (cloud_normals_);
  seg_.segment (*table_inliers_, *table_coefficients_);

  if (table_inliers_->indices.size () == 0)
  {
    PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
    return;
  }

  // ---[ Extract the plane
  proj_.setInputCloud (cloud_downsampled_);
  proj_.setIndices (table_inliers_);
  proj_.setModelCoefficients (table_coefficients_);
  proj_.filter (*table_projected_);

  // ---[ Estimate the convex hull
  std::vector<pcl::Vertices> polygons;
  CloudPtr table_hull (new Cloud ());
  hull_.setInputCloud (table_projected_);
  hull_.reconstruct (*table_hull, polygons);

  // Compute the plane coefficients
  Eigen::Vector4f model_coefficients;
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;

  model_coefficients[0] = table_coefficients_->values[0];
  model_coefficients[1] = table_coefficients_->values[1];
  model_coefficients[2] = table_coefficients_->values[2];
  model_coefficients[3] = table_coefficients_->values[3];

  // Need to flip the plane normal towards the viewpoint
  Eigen::Vector4f vp (0, 0, 0, 0);
  // See if we need to flip any plane normals
  vp -= table_hull->points[0].getVector4fMap ();
  vp[3] = 0;
  // Dot product between the (viewpoint - point) and the plane normal
  float cos_theta = vp.dot (model_coefficients);
  // Flip the plane normal
  if (cos_theta < 0)
  {
    model_coefficients *= -1;
    model_coefficients[3] = 0;
    // Hessian form (D = nc . p_plane (centroid here) + p)
    model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
  }

  //Set table_coeffs
  table_coeffs_ = model_coefficients;

  // ---[ Get the objects on top of the table
  pcl::PointIndices cloud_object_indices;
  prism_.setInputCloud (input_);
  prism_.setInputPlanarHull (table_hull);
  prism_.segment (cloud_object_indices);

  pcl::ExtractIndices<PointType> extract_object_indices;
  extract_object_indices.setInputCloud (input_);
  extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  extract_object_indices.filter (*cloud_objects_);

  //create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise
  pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>);

  {
    binary_cloud->width = input_->width;
    binary_cloud->height = input_->height;
    binary_cloud->points.resize (input_->points.size ());
    binary_cloud->is_dense = input_->is_dense;

    size_t idx;
    for (size_t i = 0; i < cloud_object_indices.indices.size (); ++i)
    {
      idx = cloud_object_indices.indices[i];
      binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap ();
      binary_cloud->points[idx].intensity = 1.0;
    }
  }

  //connected components on the binary image

  std::map<float, float> connected_labels;
  float c_intensity = 0.1f;
  float intensity_incr = 0.1f;

  {

    int wsize = wsize_;
    for (int i = 0; i < int (binary_cloud->width); i++)
    {
      for (int j = 0; j < int (binary_cloud->height); j++)
      {
        if (binary_cloud->at (i, j).intensity != 0)
        {
          //check neighboring pixels, first left and then top
          //be aware of margins

          if ((i - 1) < 0 && (j - 1) < 0)
          {
            //top-left pixel
            (*binary_cloud) (i, j).intensity = c_intensity;
          }
          else
          {
            if ((j - 1) < 0)
            {
              //top-row, check on the left of pixel to assign a new label or not
              int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
              if (left)
              {
                //Nothing found on the left, check bigger window

                bool found = false;
                for (int kk = 2; kk < wsize && !found; kk++)
                {
                  if ((i - kk) < 0)
                    continue;

                  int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
                  if (left == 0)
                  {
                    found = true;
                  }
                }

                if (!found)
                {
                  c_intensity += intensity_incr;
                  (*binary_cloud) (i, j).intensity = c_intensity;
                }

              }
            }
            else
            {
              if ((i - 1) == 0)
              {
                //check only top
                int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
                if (top)
                {
                  bool found = false;
                  for (int kk = 2; kk < wsize && !found; kk++)
                  {
                    if ((j - kk) < 0)
                      continue;

                    int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
                    if (top == 0)
                    {
                      found = true;
                    }
                  }

                  if (!found)
                  {
                    c_intensity += intensity_incr;
                    (*binary_cloud) (i, j).intensity = c_intensity;
                  }
                }

              }
              else
              {
                //check left and top
                int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
                int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);

                if (left == 0 && top == 0)
                {
                  //both top and left had labels, check if they are different
                  //if they are, take the smallest one and mark labels to be connected..

                  if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity)
                  {
                    float smaller_intensity = (*binary_cloud) (i - 1, j).intensity;
                    float bigger_intensity = (*binary_cloud) (i, j - 1).intensity;

                    if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity)
                    {
                      smaller_intensity = (*binary_cloud) (i, j - 1).intensity;
                      bigger_intensity = (*binary_cloud) (i - 1, j).intensity;
                    }

                    connected_labels[bigger_intensity] = smaller_intensity;
                    (*binary_cloud) (i, j).intensity = smaller_intensity;
                  }
                }

                if (left == 1 && top == 1)
                {
                  //if none had labels, increment c_intensity
                  //search first on bigger window
                  bool found = false;
                  for (int dist = 2; dist < wsize && !found; dist++)
                  {
                    if (((i - dist) < 0) || ((j - dist) < 0))
                      continue;

                    int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
                    int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);

                    if (left == 0 && top == 0)
                    {
                      if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity)
                      {
                        float smaller_intensity = (*binary_cloud) (i - dist, j).intensity;
                        float bigger_intensity = (*binary_cloud) (i, j - dist).intensity;

                        if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity)
                        {
                          smaller_intensity = (*binary_cloud) (i, j - dist).intensity;
                          bigger_intensity = (*binary_cloud) (i - dist, j).intensity;
                        }

                        connected_labels[bigger_intensity] = smaller_intensity;
                        (*binary_cloud) (i, j).intensity = smaller_intensity;
                        found = true;
                      }
                    }
                    else if (left == 0 || top == 0)
                    {
                      //one had label
                      found = true;
                    }
                  }

                  if (!found)
                  {
                    //none had label in the bigger window
                    c_intensity += intensity_incr;
                    (*binary_cloud) (i, j).intensity = c_intensity;
                  }
                }
              }
            }
          }

        }
      }
    }
  }

  std::map<float, pcl::PointIndices> clusters_map;

  {
    std::map<float, float>::iterator it;

    for (int i = 0; i < int (binary_cloud->width); i++)
    {
      for (int j = 0; j < int (binary_cloud->height); j++)
      {
        if (binary_cloud->at (i, j).intensity != 0)
        {
          //check if this is a root label...
          it = connected_labels.find (binary_cloud->at (i, j).intensity);
          while (it != connected_labels.end ())
          {
            //the label is on the list, change pixel intensity until it has a root label
            (*binary_cloud) (i, j).intensity = (*it).second;
            it = connected_labels.find (binary_cloud->at (i, j).intensity);
          }

          std::map<float, pcl::PointIndices>::iterator it_indices;
          it_indices = clusters_map.find (binary_cloud->at (i, j).intensity);
          if (it_indices == clusters_map.end ())
          {
            pcl::PointIndices indices;
            clusters_map[binary_cloud->at (i, j).intensity] = indices;
          }

          clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (static_cast<int> (j * binary_cloud->width + i));
        }
      }
    }
  }

  clusters.resize (clusters_map.size ());

  std::map<float, pcl::PointIndices>::iterator it_indices;
  int k = 0;
  for (it_indices = clusters_map.begin (); it_indices != clusters_map.end (); it_indices++)
  {

    if (int ((*it_indices).second.indices.size ()) >= object_cluster_min_size_)
    {

      clusters[k] = CloudPtr (new Cloud ());
      pcl::copyPointCloud (*input_, (*it_indices).second.indices, *clusters[k]);
      k++;
      indices_clusters_.push_back((*it_indices).second);
    }
  }

  clusters.resize (k);

}
template <typename PointT> void
pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
  // Check if all mandatory variables have been set:
  if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
    return;
  }
  if (cloud_ == NULL)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
    return;
  }
  if (intrinsics_matrix_(0) == 0)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
    return;
  }
  if (!person_classifier_set_flag_)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
    return;
  }

  if (!dimension_limits_set_)    // if dimension limits have not been set by the user
  {
    // Adapt thresholds for clusters points number to the voxel size:
    max_points_ = int(float(max_points_) * std::pow(0.06/voxel_size_, 2));
    if (voxel_size_ > 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

  // Voxel grid filtering:
  PointCloudPtr cloud_filtered(new PointCloud);
  pcl::VoxelGrid<PointT> 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);

  // Ground removal and update:
  pcl::IndicesPtr inliers(new std::vector<int>);
  boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered));
  ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
  no_ground_cloud_ = PointCloudPtr (new PointCloud);
  pcl::ExtractIndices<PointT> extract;
  extract.setInputCloud(cloud_filtered);
  extract.setIndices(inliers);
  extract.setNegative(true);
  extract.filter(*no_ground_cloud_);
  if((inliers->size() >= 300*0.06/voxel_size_))
    ground_model->optimizeModelCoefficients(*inliers, ground_coeffs_, ground_coeffs_);
  else
    std::cout << "No groundplane update!" << std::endl;

  // Euclidean Clustering:
  std::vector<pcl::PointIndices> cluster_indices;
  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
  tree->setInputCloud(no_ground_cloud_);
  pcl::EuclideanClusterExtraction<PointT> ec;
  ec.setClusterTolerance(2 * 0.06);
  ec.setMinClusterSize(min_points_);
  ec.setMaxClusterSize(max_points_);
  ec.setSearchMethod(tree);
  ec.setInputCloud(no_ground_cloud_);
  ec.extract(cluster_indices);

  // Head based sub-clustering //
  pcl::people::HeadBasedSubclustering<PointT> subclustering;
  subclustering.setInputCloud(no_ground_cloud_);
  subclustering.setGround(ground_coeffs_);
  subclustering.setInitialClusters(cluster_indices);
  subclustering.setHeightLimits(min_height_, max_height_);
  subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
  subclustering.setSensorPortraitOrientation(vertical_);
  subclustering.subcluster(clusters);

  // Person confidence evaluation with HOG+SVM:
  if (vertical_)  // Rotate the image if the camera is vertical
  {
    swapDimensions(rgb_image_);
  }
  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
  {
    //Evaluate confidence for the current PersonCluster:
    Eigen::Vector3f centroid = intrinsics_matrix_ * (it->getTCenter());
    centroid /= centroid(2);
    Eigen::Vector3f top = intrinsics_matrix_ * (it->getTTop());
    top /= top(2);
    Eigen::Vector3f bottom = intrinsics_matrix_ * (it->getTBottom());
    bottom /= bottom(2);
    it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, intrinsics_matrix_, vertical_));
  }
}
template <typename PointT> bool
pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
  // Check if all mandatory variables have been set:
  if (!ground_coeffs_set_)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
    return (false);
  }
  if (cloud_ == NULL)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
    return (false);
  }
  if (!intrinsics_matrix_set_)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
    return (false);
  }
  if (!person_classifier_set_flag_)
  {
    PCL_ERROR ("[pcl::people::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
    return (false);
  }

  // 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)
  {
    PointCloudPtr cloud_downsampled(new PointCloud);
    cloud_downsampled->width = (cloud_->width)/sampling_factor_;
    cloud_downsampled->height = (cloud_->height)/sampling_factor_;
    cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
    cloud_downsampled->is_dense = cloud_->is_dense;
    for (uint32_t j = 0; j < cloud_downsampled->width; j++)
    {
      for (uint32_t i = 0; i < cloud_downsampled->height; i++)
      {
        (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
      }
    }
    (*cloud_) = (*cloud_downsampled);
  }

  applyTransformationPointCloud();

  filter();

  // Ground removal and update:
  pcl::IndicesPtr inliers(new std::vector<int>);
  boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered_));
  ground_model->selectWithinDistance(ground_coeffs_transformed_, 2 * voxel_size_, *inliers);
  no_ground_cloud_ = PointCloudPtr (new PointCloud);
  pcl::ExtractIndices<PointT> extract;
  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<double> (sampling_factor_), 2)))
    ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_transformed_, ground_coeffs_transformed_);
  else
    PCL_INFO ("No groundplane update!\n");

  // Euclidean Clustering:
  std::vector<pcl::PointIndices> cluster_indices;
  typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
  tree->setInputCloud(no_ground_cloud_);
  pcl::EuclideanClusterExtraction<PointT> ec;
  ec.setClusterTolerance(2 * voxel_size_);
  ec.setMinClusterSize(min_points_);
  ec.setMaxClusterSize(max_points_);
  ec.setSearchMethod(tree);
  ec.setInputCloud(no_ground_cloud_);
  ec.extract(cluster_indices);

  // Head based sub-clustering //
  pcl::people::HeadBasedSubclustering<PointT> subclustering;
  subclustering.setInputCloud(no_ground_cloud_);
  subclustering.setGround(ground_coeffs_transformed_);
  subclustering.setInitialClusters(cluster_indices);
  subclustering.setHeightLimits(min_height_, max_height_);
  subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
  subclustering.setSensorPortraitOrientation(vertical_);
  subclustering.subcluster(clusters);

  // Person confidence evaluation with HOG+SVM:
  if (vertical_)  // Rotate the image if the camera is vertical
  {
    swapDimensions(rgb_image_);
  }
  for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
  {
    //Evaluate confidence for the current PersonCluster:
    Eigen::Vector3f centroid = intrinsics_matrix_transformed_ * (it->getTCenter());
    centroid /= centroid(2);
    Eigen::Vector3f top = intrinsics_matrix_transformed_ * (it->getTTop());
    top /= top(2);
    Eigen::Vector3f bottom = intrinsics_matrix_transformed_ * (it->getTBottom());
    bottom /= bottom(2);
    it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
  }
 
  return (true);
}
template <typename PointT> bool
open_ptrack::detection::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters)
{
  frame_counter_++;

  // Define if debug info should be written or not for this frame:
  bool debug_flag = false;
  if ((frame_counter_ % 60) == 0)
  {
    debug_flag = true;
  }

  // Check if all mandatory variables have been set:
  if (debug_flag)
  {
    if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_)
    {
      PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Floor parameters have not been set or they are not valid!\n");
      return (false);
    }
    if (cloud_ == NULL)
    {
      PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Input cloud has not been set!\n");
      return (false);
    }
    if (intrinsics_matrix_(0) == 0)
    {
      PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Camera intrinsic parameters have not been set!\n");
      return (false);
    }
    if (!person_classifier_set_flag_)
    {
      PCL_ERROR ("[open_ptrack::detection::GroundBasedPeopleDetectionApp::compute] Person classifier has not been set!\n");
      return (false);
    }
  }

  if (!dimension_limits_set_)    // if dimension limits have not been set by the user
  {
    // Adapt thresholds for clusters points number to the voxel size:
    max_points_ = int(float(max_points_) * std::pow(0.06/voxel_size_, 2));
    if (voxel_size_ > 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)
  {
    PointCloudPtr cloud_downsampled(new PointCloud);
    cloud_downsampled->width = (cloud_->width)/sampling_factor_;
    cloud_downsampled->height = (cloud_->height)/sampling_factor_;
    cloud_downsampled->points.resize(cloud_downsampled->height*cloud_downsampled->width);
    cloud_downsampled->is_dense = cloud_->is_dense;
    cloud_downsampled->header = cloud_->header;
    for (int j = 0; j < cloud_downsampled->width; j++)
    {
      for (int i = 0; i < cloud_downsampled->height; i++)
      {
        (*cloud_downsampled)(j,i) = (*cloud_)(sampling_factor_*j,sampling_factor_*i);
      }
    }
    (*cloud_) = (*cloud_downsampled);
  }

  if (use_rgb_)
  {
    // Compute mean luminance:
    int n_points = cloud_->points.size();
    double sumR, sumG, sumB = 0.0;
    for (int j = 0; j < cloud_->width; j++)
    {
      for (int i = 0; i < cloud_->height; i++)
      {
        sumR += (*cloud_)(j,i).r;
        sumG += (*cloud_)(j,i).g;
        sumB += (*cloud_)(j,i).b;
      }
    }
    mean_luminance_ = 0.3 * sumR/n_points + 0.59 * sumG/n_points + 0.11 * sumB/n_points;
    //    mean_luminance_ = 0.2126 * sumR/n_points + 0.7152 * sumG/n_points + 0.0722 * sumB/n_points;
  }

  // Voxel grid filtering:
  PointCloudPtr cloud_filtered(new PointCloud);
  pcl::VoxelGrid<PointT> 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.setFilterFieldName("z");
  voxel_grid_filter_object.setFilterLimits(0.0, max_distance_);
  voxel_grid_filter_object.filter (*cloud_filtered);

  // Ground removal and update:
  pcl::IndicesPtr inliers(new std::vector<int>);
  boost::shared_ptr<pcl::SampleConsensusModelPlane<PointT> > ground_model(new pcl::SampleConsensusModelPlane<PointT>(cloud_filtered));
  ground_model->selectWithinDistance(ground_coeffs_, voxel_size_, *inliers);
  no_ground_cloud_ = PointCloudPtr (new PointCloud);
  pcl::ExtractIndices<PointT> extract;
  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<double> (sampling_factor_), 2)))
    ground_model->optimizeModelCoefficients (*inliers, ground_coeffs_, ground_coeffs_);
  else
  {
    if (debug_flag)
    {
      PCL_INFO ("No groundplane update!\n");
    }
  }

  // Background Subtraction (optional):
  if (background_subtraction_)
  {
    PointCloudPtr foreground_cloud(new PointCloud);
    for (unsigned int i = 0; i < no_ground_cloud_->points.size(); i++)
    {
      if (not (background_octree_->isVoxelOccupiedAtPoint(no_ground_cloud_->points[i].x, no_ground_cloud_->points[i].y, no_ground_cloud_->points[i].z)))
      {
        foreground_cloud->points.push_back(no_ground_cloud_->points[i]);
      }
    }
    no_ground_cloud_ = foreground_cloud;
  }

  if (no_ground_cloud_->points.size() > 0)
  {
    // Euclidean Clustering:
    std::vector<pcl::PointIndices> cluster_indices;
    typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
    tree->setInputCloud(no_ground_cloud_);
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance(2 * 0.06);
    ec.setMinClusterSize(min_points_);
    ec.setMaxClusterSize(max_points_);
    ec.setSearchMethod(tree);
    ec.setInputCloud(no_ground_cloud_);
    ec.extract(cluster_indices);

    // Sensor tilt compensation to improve people detection:
    PointCloudPtr no_ground_cloud_rotated(new PointCloud);
    Eigen::VectorXf ground_coeffs_new;
    if(sensor_tilt_compensation_)
    {
      // We want to rotate the point cloud so that the ground plane is parallel to the xOz plane of the sensor:
      Eigen::Vector3f input_plane, output_plane;
      input_plane << ground_coeffs_(0), ground_coeffs_(1), ground_coeffs_(2);
      output_plane << 0.0, -1.0, 0.0;

      Eigen::Vector3f axis = input_plane.cross(output_plane);
      float angle = acos( input_plane.dot(output_plane)/ ( input_plane.norm()/output_plane.norm() ) );
      transform_ = Eigen::AngleAxisf(angle, axis);

      // Setting also anti_transform for later
      anti_transform_ = transform_.inverse();
      no_ground_cloud_rotated = rotateCloud(no_ground_cloud_, transform_);
      ground_coeffs_new.resize(4);
      ground_coeffs_new = rotateGround(ground_coeffs_, transform_);
    }
    else
    {
      transform_ = transform_.Identity();
      anti_transform_ = transform_.inverse();
      no_ground_cloud_rotated = no_ground_cloud_;
      ground_coeffs_new = ground_coeffs_;
    }

    // To avoid PCL warning:
    if (cluster_indices.size() == 0)
      cluster_indices.push_back(pcl::PointIndices());

    // Head based sub-clustering //
    pcl::people::HeadBasedSubclustering<PointT> subclustering;
    subclustering.setInputCloud(no_ground_cloud_rotated);
    subclustering.setGround(ground_coeffs_new);
    subclustering.setInitialClusters(cluster_indices);
    subclustering.setHeightLimits(min_height_, max_height_);
    subclustering.setMinimumDistanceBetweenHeads(heads_minimum_distance_);
    subclustering.setSensorPortraitOrientation(vertical_);
    subclustering.subcluster(clusters);

//    for (unsigned int i = 0; i < rgb_image_->points.size(); i++)
//    {
//      if ((rgb_image_->points[i].r < 0) | (rgb_image_->points[i].r > 255) | isnan(rgb_image_->points[i].r))
//      {
//        std::cout << "ERROR in RGB data!" << std::endl;
//      }
//    }

    if (use_rgb_) // if RGB information can be used
    {
      // Person confidence evaluation with HOG+SVM:
      if (vertical_)  // Rotate the image if the camera is vertical
      {
        swapDimensions(rgb_image_);
      }
      for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
      {
        //Evaluate confidence for the current PersonCluster:
        Eigen::Vector3f centroid = intrinsics_matrix_ * (anti_transform_ * it->getTCenter());
        centroid /= centroid(2);
        Eigen::Vector3f top = intrinsics_matrix_ * (anti_transform_ * it->getTTop());
        top /= top(2);
        Eigen::Vector3f bottom = intrinsics_matrix_ * (anti_transform_ * it->getTBottom());
        bottom /= bottom(2);

        it->setPersonConfidence(person_classifier_.evaluate(rgb_image_, bottom, top, centroid, vertical_));
      }
    }
    else
    {
      for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
      {
        it->setPersonConfidence(-100.0);
      }
    }
  }

  return (true);
}