Search code examples
c++point-cloud-libraryeigen3

PCL remove points under a segmented table


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

Solution

  • 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