[PCL] Errors in cloud size after conditional removal filter? [closed]

asked 2016-05-19 03:26:29 -0500

altella gravatar image

Hello all;

I am trying to identify a blue object in an environment to pick it up. I am using an Asus Xtion as sensor to obtain the color pointcloud. First, I filter the surroundings of the object using an Euclidean filter, and then in the resulting point cloud, I use a conditional removal filter to detect the Blue object. In rviz, visualizing the pointclouds obtained, I can check that they are correct, but when checking the point cloud sizes by code, I obtain an error. I obtain the same number of points for the cloud before and after the conditional removal filtering. This is impossible, as in Rviz it is clear they have different number of points.

My code is:

int FilterObjectForCentroid(geometry_msgs::Point pointedPoint, double clusteringDistFromPointing, geometry_msgs::Point& centroidPoint)
{
    PointCloudTPtr filteredCloud = PointCloudTPtr(new PointCloudT);
    PointCloudTPtr filteredColorCloud = PointCloudTPtr(new PointCloudT);

  pcl::PointXYZRGB min_values, max_values;

    //filter environment
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud (inputCloud);
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (pointedPoint.x - clusteringDistFromPointing, pointedPoint.x + clusteringDistFromPointing);
    pass.filter (*filteredCloud);
    pass.setInputCloud (filteredCloud);
    pass.setFilterFieldName ("y");
    pass.setFilterLimits (pointedPoint.y - clusteringDistFromPointing, pointedPoint.y + clusteringDistFromPointing);
    pass.filter (*filteredCloud);


  //conditional removal (Gaveta Azul)
  // build the condition 
  pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr color_cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ()); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("r", pcl::ComparisonOps::LT, r_max))); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("r", pcl::ComparisonOps::GT, r_min))); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("g", pcl::ComparisonOps::LT, g_max))); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("g", pcl::ComparisonOps::GT, g_min))); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("b", pcl::ComparisonOps::LT, b_max))); 
  color_cond->addComparison (pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr (new pcl::PackedRGBComparison<pcl::PointXYZRGB> ("b", pcl::ComparisonOps::GT, b_min))); 
  // build the filter 
  pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem (color_cond); 
  condrem.setInputCloud (filteredCloud); 
  condrem.setKeepOrganized(true); 
  // apply filter 
  condrem.filter (*filteredColorCloud); 

  ROS_INFO_STREAM(inputCloud->points.size()<<" NUM POINTS FILTERED CLOUD "<< filteredCloud->points.size() << " COLOR " << filteredColorCloud->points.size());

  #ifdef TESTING
        sensor_msgs::PointCloud2 cloudRos;
        pcl::toROSMsg(*filteredColorCloud, cloudRos);
        cloudRos.header.frame_id = "camera_depth_optical_frame";
        cloudRos.header.stamp = ros::Time::now();
        pub.publish(cloudRos);
    #endif

  if(filteredColorCloud->size()< numPointsGaveta)
    return -1;

    //centroid obtaining
    pcl::getMinMax3D(*filteredColorCloud, min_values, max_values);
    centroidPoint.x = (min_values.x + max_values.x)/2;
    centroidPoint.y = (min_values.y + max_values.y)/2;
    centroidPoint.z = (min_values.z + max_values.z)/2;

    return 0;
}

ROS_INFO_STREAM give as output:

Which obviously is not correct in my test with a white table and a blue object (checked with Rviz)

Could anyone give a clue? I would appreaciate it a lot...

Thank you all in advance,

Regards,

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by altella
close date 2016-05-19 04:34:54.073609

Comments

Sorry.... my error. The method condrem.setKeepOrganized(true) maintains the points. Commenting that line, the problem is solved

altella gravatar imagealtella ( 2016-05-19 04:34:35 -0500 )edit