Robotics StackExchange | Archived questions

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

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;
}

ROSINFOSTREAM give as output:

[ INFO] [1463646229.017931019]: 307200 NUM POINTS FILTERED CLOUD 26242 COLOR 26242

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,

Asked by altella on 2016-05-19 03:26:29 UTC

Comments

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

Asked by altella on 2016-05-19 04:34:35 UTC

Answers