PCLObjectDetector.cpp

Go to the documentation of this file.

#include "PCLObjectDetector.hpp"
 
#include <utils/logger.hpp>
 
using namespace galaxis::utils;
 
std::shared_ptr<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>> EuclideanDetector::cluster_data(pclPtrType p_pclPtr, std::vector<Object>& p_objectsVecRef, float clusterTolerance, int minClusterSize, int maxClusterSize) const {
  
  auto pointCloudPtrVectorSPtr = std::make_shared<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>();
  
  // check if filtered cloud is empty to skip unnecessary computation
  if(p_pclPtr->size() == 0) return pointCloudPtrVectorSPtr;
 
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(p_pclPtr);
 
  std::vector<pcl::PointIndices> clusterIndices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(clusterTolerance); // m
  ec.setMinClusterSize(minClusterSize);
  ec.setMaxClusterSize(maxClusterSize);
  ec.setSearchMethod(tree);
  ec.setInputCloud(p_pclPtr);
  ec.extract(clusterIndices);
 
 
  float sizeX, sizeY, sizeZ;
  for(std::vector<pcl::PointIndices>::const_iterator it = clusterIndices.begin(); it != clusterIndices.end(); ++it) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClusterPtr(new pcl::PointCloud<pcl::PointXYZ>);
 
    // create a sub cloud with all the indices extracted above
    for(std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
      cloudClusterPtr->points.push_back(p_pclPtr->points[*pit]);
    }
 
    // post processing
    pcl::PointXYZ minDimensions;
    pcl::PointXYZ maxDimensions;
    pcl::getMinMax3D(*cloudClusterPtr, minDimensions, maxDimensions);
 
    sizeX = std::fabs(maxDimensions.x - minDimensions.x);
    sizeY = std::fabs(maxDimensions.y - minDimensions.y);
    sizeZ = std::fabs(maxDimensions.z - minDimensions.z);
 
    if(cloudClusterPtr->size() < m_postProcessingClusterSizeMin) {
      log_debug("[RealSense] Object too small (min cluster points)");
      continue;
    }
 
    if(cloudClusterPtr->size() > m_postProcessingClusterSizeMax) {
      log_debug("[RealSense] Object too large (max cluster points)");
      continue;
    }
 
    if(sizeX > m_maxSizeX) {
      log_debug("[RealSense] Object too large (x)");
      continue;
    }
    if (sizeY > m_maxSizeY) {
      log_debug("[RealSense] Object too large (y)");
      continue;
    }
    if(sizeZ > m_maxSizeZ) {
      log_debug("[RealSense] Object too large (z)");
      continue;
    }
 
    if(sizeX < m_minSizeX) {
      log_debug("[RealSense] Object too small (x)");
      continue;
    }
    if (sizeY < m_minSizeY) {
      log_debug("[RealSense] Object too small (y)");
      continue;
    }
    if(sizeZ < m_minSizeZ) {
      log_debug("[RealSense] Object too small (z)");
      continue;
    }
 
    pcl::PointXYZ centerPoint;
    // get centerpoint and MinMax dimensions
    int centroidReturnValue = pcl::computeCentroid(*cloudClusterPtr, centerPoint);
 
    // store object in return vector
    Object foundObject;
    foundObject.set_max_dimensions(maxDimensions);
    foundObject.set_min_dimensions(minDimensions);
    foundObject.set_center_point(centerPoint);
    foundObject.size = cloudClusterPtr->size();
    p_objectsVecRef.push_back(foundObject);
 
    pointCloudPtrVectorSPtr->push_back(cloudClusterPtr);
  }
 
  log_debug("Objects Detected: ", p_objectsVecRef.size());
 
  return pointCloudPtrVectorSPtr;
}
 
void PCLObjectDetector::downsampling(pclPtrType p_inputCloudPtr, pclPtrType p_outputCloudPtr, float p_leafSize) {
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(p_inputCloudPtr);
  sor.setLeafSize(p_leafSize, p_leafSize, p_leafSize);
  sor.filter(*p_outputCloudPtr);
}
 
void PCLObjectDetector::passThroughFilter(pclPtrType p_inputCloudPtr, pclPtrType p_outputCloudPtr, float y_min, float y_max) {
  // pcl::PassThrough<pcl::PointXYZ> pass_z;
  // pass_z.setInputCloud(p_inputCloudPtr);
  // pass_z.setFilterFieldName("z");
  // pass_z.setFilterLimits(0.18, 2.0);
  // pass_z.filter(*p_outputCloudPtr);
 
  // pcl::PassThrough<pcl::PointXYZ> pass_y;
  // pass_y.setInputCloud(p_inputCloudPtr);
  // pass_y.setFilterFieldName("y");
  // pass_y.setFilterLimits(y_min, y_max);
  // pass_y.filter(*p_outputCloudPtr);
}