[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