TabletopSegmentation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ActiveVision::ArmarXObjects::TabletopSegmentation
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TabletopSegmentation.h"
 
#include <limits.h>
 
#include <Eigen/Core>
 
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/region_growing.h>
 
#include <VirtualRobot/math/FitPlane.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace armarx;
 
armarx::PropertyDefinitionsPtr
TabletopSegmentation::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new TabletopSegmentationPropertyDefinitions(getConfigIdentifier()));
}
 
void
armarx::TabletopSegmentation::onInitPointCloudProcessor()
{
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    serviceProvider =
        new ServiceProvider("TabletopSegmentation",
                            [&](int timeout)
                            {
                                nextTimeoutAbs =
                                    TimeUtil::GetTime() + IceUtil::Time::milliSeconds(timeout);
                                ARMARX_INFO << "Next timeout at " + nextTimeoutAbs.toDateTime();
                            });
    auto prx = getIceManager()->registerObject(serviceProvider, getName() + "ServiceProvider");
    getIceManager()->subscribeTopic(prx.first, "ServiceRequests");
}
 
void
armarx::TabletopSegmentation::onConnectPointCloudProcessor()
{
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
    debugDrawer = getTopic<DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    prop_DistanceThreshold = getProperty<double>("DistanceThreshold");
    prop_EpsAngle = getProperty<double>("EpsAngle");
 
    prop_IncludeTabletopPlane = getProperty<bool>("IncludeTabletopPlane");
    prop_CalculatePlane = getProperty<bool>("CalculatePlane");
 
    prop_ClusterTolerance = getProperty<double>("ClusterTolerance");
    prop_MinClusterSize = getProperty<int>("MinClusterSize");
    prop_MaxClusterSize = getProperty<int>("MaxClusterSize");
 
    prop_HeightMin = getProperty<double>("HeightMin");
    prop_HeightMax = getProperty<double>("HeightMax");
 
    enableResultPointClouds<PointL>(getName() + "Result");
    enableResultPointClouds<PointT>(getName() + "Plane");
 
    getProxyFromProperty(remoteGui, "RemoteGuiName", false, "", false);
    if (remoteGui)
    {
        remoteGuiTask = new RunningTask<TabletopSegmentation>(this, &TabletopSegmentation::guiTask);
        remoteGuiTask->start();
    }
}
 
void
armarx::TabletopSegmentation::onDisconnectPointCloudProcessor()
{
    if (remoteGuiTask)
    {
        remoteGuiTask->stop();
        remoteGuiTask = nullptr;
    }
}
 
void
armarx::TabletopSegmentation::onExitPointCloudProcessor()
{
}
 
void
armarx::TabletopSegmentation::process()
{
    pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
    pcl::PointCloud<PointT>::Ptr plane(new pcl::PointCloud<PointT>());
 
    if (!getProperty<bool>("AlwaysActive").getValue() && TimeUtil::GetTime() > nextTimeoutAbs)
    {
        usleep(10000);
        return;
    }
    if (!waitForPointClouds())
    {
        ARMARX_INFO << "Timeout or error while waiting for point cloud data";
        return;
    }
    else
    {
        getPointClouds(inputCloud);
    }
 
    ARMARX_VERBOSE << "Got point cloud";
 
    if (inputCloud->points.size() == 0)
    {
        ARMARX_DEBUG << "empty point cloud.";
        return;
    }
 
    bool calculatePlane = false;
    bool includeTabletopPlane = false;
    {
        // Update parameters
        std::unique_lock<std::mutex> lock(prop_mutex);
 
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(1000);
        seg.setDistanceThreshold(prop_DistanceThreshold);
        seg.setAxis(Eigen::Vector3f::UnitZ());
        seg.setEpsAngle(prop_EpsAngle);
        seg.setProbability(0.99);
 
        hull.setDimension(2);
        prism.setHeightLimits(prop_HeightMin, prop_HeightMax);
        prism.setViewPoint(std::numeric_limits<float>::max(),
                           std::numeric_limits<float>::max(),
                           std::numeric_limits<float>::max());
 
        ec.setClusterTolerance(prop_ClusterTolerance);
        ec.setMinClusterSize(prop_MinClusterSize);
        ec.setMaxClusterSize(prop_MaxClusterSize);
 
        includeTabletopPlane = prop_IncludeTabletopPlane;
        calculatePlane = prop_CalculatePlane;
    }
 
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<PointT>::Ptr tempCloud(new pcl::PointCloud<PointT>());
 
    // todo maybe downsample the point cloud first.
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud(inputCloud);
    seg.segment(*inliers, *coefficients);
 
    if (!inliers->indices.size())
    {
        ARMARX_INFO << deactivateSpam(60) << "Unable to find table";
        return;
    }
 
    // Extract the planar inliers from the input cloud
    extract.setInputCloud(inputCloud);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(*plane);
 
    extract.setNegative(true);
    extract.filter(*tempCloud);
 
    pcl::PointCloud<PointT>::Ptr hullPoints(new pcl::PointCloud<PointT>());
    hull.setInputCloud(inputCloud);
    hull.setIndices(inliers);
    hull.reconstruct(*hullPoints);
 
    prism.setInputCloud(tempCloud);
    prism.setInputPlanarHull(hullPoints);
    prism.segment(*inliers);
 
    extract.setInputCloud(tempCloud);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(*tempCloud);
 
 
    ARMARX_VERBOSE << "Plane: " << plane->size();
    provideResultPointClouds(plane, getName() + "Plane");
 
    if (calculatePlane)
    {
        std::vector<Eigen::Vector3f> planePoints;
        for (const PointT& p : plane->points)
        {
            planePoints.emplace_back(Eigen::Vector3f(p.x, p.y, p.z));
        }
        math::Plane mplane = math::FitPlane::Fit(planePoints);
        std::unique_lock guard(tablePlaneMutex);
        Eigen::Vector3f planeNormal = mplane.GetNormal(Eigen::Vector3f::UnitZ());
        //        ARMARX_IMPORTANT << VAROUT(planeNormal.transpose());
        tablePlane = new visionx::TabletopSegmentationPlane(new Vector3(mplane.Pos()),
                                                            new Vector3(planeNormal));
    }
 
    pcl::PointCloud<PointL>::Ptr resultCloud(new pcl::PointCloud<PointL>());
    extractEuclideanClusters(tempCloud, resultCloud);
 
    if (includeTabletopPlane)
    {
        pcl::PointCloud<PointL>::Ptr planeLabel(new pcl::PointCloud<PointL>());
        pcl::copyPointCloud(*plane, *planeLabel);
        for (PointL& p : planeLabel->points)
        {
            p.label = 1;
        }
        pcl::PointCloud<PointL>::Ptr combinedCloud(new pcl::PointCloud<PointL>());
        *combinedCloud = *resultCloud + *planeLabel;
        resultCloud->swap(*combinedCloud);
    }
    resultCloud->header.stamp = inputCloud->header.stamp;
 
    ARMARX_VERBOSE << "Result: " << resultCloud->size();
    provideResultPointClouds(resultCloud, getName() + "Result");
}
 
void
armarx::TabletopSegmentation::extractEuclideanClusters(
    const pcl::PointCloud<PointT>::ConstPtr& inputCloud,
    const pcl::PointCloud<PointL>::Ptr& resultCloud)
{
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    tree->setInputCloud(inputCloud);
 
    std::vector<pcl::PointIndices> clusterIndices;
    ec.setSearchMethod(tree);
    ec.setInputCloud(inputCloud);
    ec.extract(clusterIndices);
 
    uint32_t j = 2;
    for (std::vector<pcl::PointIndices>::const_iterator it = clusterIndices.begin();
         it != clusterIndices.end();
         ++it)
    {
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end();
             ++pit)
        {
            const unsigned int i = static_cast<unsigned int>(*pit);
            const PointT& src = inputCloud->points[i];
 
            PointL p;
            p.getVector3fMap() = src.getVector3fMap();
            p.rgba = src.rgba; // p.getRGBAVector4i() = src.getRGBAVector4i();
            p.label = j;
 
            resultCloud->points.push_back(p);
        }
        j++;
    }
 
    resultCloud->width = static_cast<unsigned int>(resultCloud->points.size());
    resultCloud->height = 1;
    resultCloud->is_dense = false;
}
 
void
TabletopSegmentation::guiTask()
{
    guiCreate();
 
    RemoteGui::TabProxy tab(remoteGui, getName());
    CycleUtil c(20);
    while (!remoteGuiTask->isStopped())
    {
        tab.receiveUpdates();
        guiUpdate(tab);
 
        c.waitForCycleDuration();
    }
}
 
void
TabletopSegmentation::guiCreate()
{
    auto grid = RemoteGui::makeGridLayout("Grid");
 
    int row = 0;
    {
        grid.addTextLabel("Distance Threshold", row, 0);
        auto slider = RemoteGui::makeFloatSlider("DistanceThreshold")
                          .value(prop_DistanceThreshold)
                          .min(1.0f)
                          .max(50.0f)
                          .steps(50);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Eps Angle", row, 0);
        auto slider = RemoteGui::makeFloatSlider("EpsAngle")
                          .value(prop_EpsAngle)
                          .min(pcl::deg2rad(1.0))
                          .max(pcl::deg2rad(30.0))
                          .steps(30);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Cluster Tolerance", row, 0);
        auto slider = RemoteGui::makeFloatSlider("ClusterTolerance")
                          .value(prop_ClusterTolerance)
                          .min(1.0f)
                          .max(50.0f)
                          .steps(50);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Min Cluster Size ", row, 0);
        auto slider = RemoteGui::makeIntSlider("MinClusterSize")
                          .value(prop_MinClusterSize)
                          .min(100)
                          .max(1000000);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Max Cluster Size ", row, 0);
        auto slider = RemoteGui::makeIntSlider("MaxClusterSize")
                          .value(prop_MaxClusterSize)
                          .min(25000)
                          .max(4000000);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Height Min", row, 0);
        auto slider = RemoteGui::makeFloatSlider("HeightMin")
                          .value(prop_HeightMin)
                          .min(0.0f)
                          .max(500.0f)
                          .steps(50);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Height Max", row, 0);
        auto slider = RemoteGui::makeFloatSlider("HeightMax")
                          .value(prop_HeightMax)
                          .min(100.0f)
                          .max(2000.0f)
                          .steps(50);
        grid.addChild(slider, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Include Tabletop Plane", row, 0);
        auto checkbox =
            RemoteGui::makeCheckBox("IncludeTabletopPlane").value(prop_IncludeTabletopPlane);
        grid.addChild(checkbox, row, 1);
        row += 1;
    }
    {
        grid.addTextLabel("Calculate Plane", row, 0);
        auto checkbox = RemoteGui::makeCheckBox("CalculatePlane").value(prop_CalculatePlane);
        grid.addChild(checkbox, row, 1);
        row += 1;
    }
 
    remoteGui->createTab(getName(), grid);
}
 
void
TabletopSegmentation::guiUpdate(RemoteGui::TabProxy& tab)
{
    std::unique_lock<std::mutex> lock(prop_mutex);
#define ARMARX_REMOTE_GUI_GET_PROP(name)                                                           \
    prop_##name = tab.getValue<decltype(prop_##name)>(#name).get()
 
    ARMARX_REMOTE_GUI_GET_PROP(DistanceThreshold);
    ARMARX_REMOTE_GUI_GET_PROP(EpsAngle);
    ARMARX_REMOTE_GUI_GET_PROP(ClusterTolerance);
    ARMARX_REMOTE_GUI_GET_PROP(MinClusterSize);
    ARMARX_REMOTE_GUI_GET_PROP(MaxClusterSize);
    ARMARX_REMOTE_GUI_GET_PROP(HeightMin);
    ARMARX_REMOTE_GUI_GET_PROP(HeightMax);
    ARMARX_REMOTE_GUI_GET_PROP(IncludeTabletopPlane);
    ARMARX_REMOTE_GUI_GET_PROP(CalculatePlane);
 
#undef ARMARX_REMOTE_GUI_GET_PROP
}
 
visionx::TabletopSegmentationPlanePtr
armarx::TabletopSegmentation::getTablePlane(const Ice::Current&)
{
    std::unique_lock guard(tablePlaneMutex);
    return tablePlane;
}
 
ServiceProvider::ServiceProvider(const std::string& serviceName,
                                 const std::function<void(int)>& callback) :
    serviceName(serviceName), callback(callback)
{
}
 
void
ServiceProvider::requestService(const std::string& serviceName,
                                Ice::Int relativeTimeoutMs,
                                const Ice::Current&)
{
    if (serviceName == this->serviceName)
    {
        callback(relativeTimeoutMs);
    }
}