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

edit

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,