I want to remove all points under a recognized table. How should I do this? I have the following functional code to segment/remove the table from a point cloud:
std::cout << "(II) segmenting table plane ..." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_scene(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
SACSegmentation<PointXYZ> segmentation;
segmentation.setInputCloud(downsampledScene);
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setDistanceThreshold(threshold);
segmentation.setOptimizeCoefficients(true);
segmentation.setMaxIterations(1000);
//PointIndices inlierIndices;
segmentation.segment(*inliers, *coefficients);
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(downsampledScene);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*segmented_scene);
//copyPointCloud<pcl::PointXYZ>(*downsampledScene, inlierIndices, *segmented_scene);
copyPointCloud(*segmented_scene, *outputCloud);
Or how can I set the origin of the coordinate system on the table to use a passthrough filter to filter all points under the table?
I tested this with the following code, but I think this is not correct:
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
cout << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << endl;
transform_1.translation() << coefficients->values[0], coefficients->values[1], coefficients->values[2];
transformPointCloud(*segmented_scene, *segmented_scene, transform_1);
I solved the problem like this:
Create Eigen::Vector3d n of coefficients(0,1,2).
Calculate the angle between the Vector and the z-axis.
invert n, coefficients.value(3) and angle if angle > 0, because z filter limits [ setFilterLimits(FLT_MIN,FLT_MAX); ]
Create Eigen::Affine3d t (Eigen::Translation3d(n*coefficients.values(3)));
Calculate the r AngelAxisd(angle,axis)
Calculate the transformation r * t
transform the point Cloud
Use a passthrough filter and set the limits.
And then inverse transformation