PointCloudFilter.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    VisionX::ArmarXObjects::PointCloudFilter
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudFilter.h"
 
#include <pcl/common/colors.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/filters/statistical_outlier_removal.h>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <ArmarXGui/libraries/RemoteGui/Storage.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/libraries/PointCloudTools/FramedPointCloud.h>
 
 
using namespace armarx;
 
namespace visionx
{
 
    PointCloudFilterPropertyDefinitions::PointCloudFilterPropertyDefinitions(std::string prefix) :
        PointCloudProcessorPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent",
                                            "Name of the robot state component.");
 
        defineOptionalProperty<bool>("sourceFrameNameAuto", true,
                                     "If enabled, try to get and use source frame name from point cloud provider.");
        defineOptionalProperty<std::string>("sourceFrameName", "DepthCamera", "The source frame name.");
        defineOptionalProperty<std::string>("targetFrameName", "Global", "The target frame name.");
 
        defineOptionalProperty<std::string>("PointCloudFormat", "XYZRGBA", "Format of the input and output point cloud (XYZRGBA, XYZL, XYZRGBL).");
 
        defineOptionalProperty<bool>("EnableDownsampling", true, "Enable/disable downsampling.");
        defineOptionalProperty<float>("leafSize", 5.0f, "The voxel grid leaf size.");
 
 
        defineOptionalProperty<bool>("StatisticalOutlierRemoval_enabled", false, "Enable StatisticalOutlierRemoval (pre downsampling)");
        defineOptionalProperty<int>("StatisticalOutlierRemoval_MeanK", 50,
                                    "(pcl parameter) number of nearest neighbors to use for mean distance estimation. ");
        defineOptionalProperty<float>("StatisticalOutlierRemoval_StddevMulThresh", 1,
                                      "(pcl parameter) standard deviation multiplier for the distance threshold calculation. ");
 
        defineOptionalProperty<bool>("EnableCropping", true, "Enable/disable cropping.");
        defineOptionalPropertyVector<Eigen::Vector3f>("minPoint", Eigen::Vector3f(-1000.0f, -1000.0f, 0.0f),
                "Minimal point of cropping box.", ',');
        defineOptionalPropertyVector<Eigen::Vector3f>("maxPoint", Eigen::Vector3f(4800.0f, 11500.0f, 1800.0f),
                "Maximal point of cropping box.", ',');
        defineOptionalPropertyVector<Eigen::Vector3f>("rpy", {0, 0, 0},
                "Roll, Pitch and Yaw applied to the points in the croping frame", ',');
 
        defineOptionalPropertyVector<Eigen::Vector3f>("ProviderFrameTranslation", Eigen::Vector3f(0.0f, 0.0f, 0.0f), "", ',');
        defineOptionalPropertyVector<Eigen::Vector3f>("ProviderFrameRotation", Eigen::Vector3f(0.0f, 0.0f, 0.0f), "", ',');
 
        defineOptionalProperty<std::string>("croppingFrame", "Global", "The coordinate frame in which cropping is applied.");
 
        defineOptionalProperty<bool>("applyCollisionModelFilter", true,
                                     "Discard points that appear to belong to the robot itself.");
 
        defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote GUI provider");
 
        defineOptionalProperty<bool>("reportCloudOutsideOfCroppingArea", false,
                                     "If cropping is enabled, the points outside of the area are reported in a separate cloud");
    }
 
 
    std::string PointCloudFilter::getDefaultName() const
    {
        return "PointCloudFilter";
    }
 
    std::string PointCloudFilter::getReferenceFrame(const Ice::Current&)
    {
        std::unique_lock lock(parametersMutex);
        return parameters.targetFrameName;
    }
 
    void PointCloudFilter::setCroppingParameters(
        const Vector3BasePtr& min, const Vector3BasePtr& max,
        const std::string& frame, const Ice::Current&)
    {
        std::unique_lock lock(parametersMutex);
        parameters.croppingEnabled = true;
        parameters.croppingMinPoint = Vector3Ptr::dynamicCast(min)->toEigen();
        parameters.croppingMaxPoint = Vector3Ptr::dynamicCast(max)->toEigen();
        parameters.croppingFrameName = frame;
    }
 
    void PointCloudFilter::onInitPointCloudProcessor()
    {
        getProperty(pointCloudFormat, "PointCloudFormat");
 
 
        getProperty(parameters.sourceFrameName, "sourceFrameName");
        getProperty(parameters.targetFrameName, "targetFrameName");
 
        getProperty(parameters.croppingEnabled, "EnableCropping");
        getProperty(parameters.croppingFrameName, "croppingFrame");
        getProperty(parameters.croppingMinPoint, "minPoint");
        getProperty(parameters.croppingMaxPoint, "maxPoint");
        getProperty(parameters.croppingRPY, "rpy");
 
        getProperty(parameters.downsamplingEnabled, "EnableDownsampling");
        getProperty(parameters.gridLeafSize, "leafSize");
 
        getProperty(parameters.statisticalOutlierRemoval.enabled, "StatisticalOutlierRemoval_enabled");
        getProperty(parameters.statisticalOutlierRemoval.meanK, "StatisticalOutlierRemoval_MeanK");
        getProperty(parameters.statisticalOutlierRemoval.stddevMulThresh, "StatisticalOutlierRemoval_StddevMulThresh");
 
        {
            parameters.providerFrameTransformation = simox::math::pos_rpy_to_mat4f(
                        getProperty<Eigen::Vector3f>("ProviderFrameTranslation").getValue(),
                        getProperty<Eigen::Vector3f>("ProviderFrameRotation").getValue()
                    );
        }
 
        std::string robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
        if (!robotStateComponentName.empty())
        {
            usingProxyFromProperty("RobotStateComponentName");
        }
        else
        {
            ARMARX_INFO << "No RobotStateComponent configured, not performing any point cloud transformations.";
        }
 
        getProperty(parameters.applyCollisionModelFilter, "applyCollisionModelFilter");
        getProperty(parameters.reportCloudOutsideOfCroppingArea, "reportCloudOutsideOfCroppingArea");
        if (parameters.applyCollisionModelFilter && robotStateComponentName.empty())
        {
            ARMARX_WARNING << "Collision model filter activated, but no RobotStateComponent configured, deactivating collision model filtering";
            parameters.applyCollisionModelFilter = false;
        }
 
        debugDrawer.setLayer(getName());
        debugDrawer.offeringTopic(*this);
        debugDrawer.setEnabled(false);
    }
 
    void PointCloudFilter::onConnectPointCloudProcessor()
    {
        const std::string robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
        if (!robotStateComponentName.empty())
        {
            getProxy(robotStateComponent, robotStateComponentName);
            localRobot = RemoteRobot::createLocalCloneFromFile(
                             robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
        }
 
        {
            const std::vector<std::string> providerNames = getPointCloudProviderNames();
            ARMARX_CHECK_EQUAL(providerNames.size(), 1);
 
            this->providerSourceFrameName = getPointCloudFrame(providerNames.front());
            if (getProperty<bool>("sourceFrameNameAuto") && !providerSourceFrameName.empty())
            {
                ARMARX_INFO << "Using source frame '" << providerSourceFrameName << "' from provider '" << providerNames.front() << "'.";
                this->parameters.sourceFrameName = providerSourceFrameName;
            }
        }
 
        if (pointCloudFormat == "XYZRGBA")
        {
            enableResultPointClouds<pcl::PointXYZRGBA>(getName() + "Result");
            enableResultPointClouds<pcl::PointXYZRGBA>(getName() + "Outside");
        }
        else if (pointCloudFormat == "XYZL")
        {
            enableResultPointClouds<pcl::PointXYZL>(getName() + "Result");
            enableResultPointClouds<pcl::PointXYZL>(getName() + "Outside");
        }
        else if (pointCloudFormat == "XYZRGBL")
        {
            enableResultPointClouds<pcl::PointXYZRGBL>(getName() + "Result");
            enableResultPointClouds<pcl::PointXYZRGBL>(getName() + "Outside");
        }
        else
        {
            ARMARX_ERROR << "Could not initialize point cloud, because format '" << pointCloudFormat << "' is unknown."
                         << "\nKnown formats are: XYZL, XYZRGBA, XYZGBL.";
        }
 
        getProxyFromProperty(remoteGui, "RemoteGuiName", false, "", false);
        if (remoteGui)
        {
            remoteGuiCreate();
            remoteGuiTask = new armarx::SimpleRunningTask<>(
                [this]()
            {
                this->remoteGuiRun();
            }, "RemoteGuiTask");
            remoteGuiTask->start();
        }
 
        debugDrawer.getTopic(*this);
        // Clear the layer (disregarding whether debug drawer is enabled).
        debugDrawer.getTopic()->clearLayer(debugDrawer.getLayer());
    }
 
    void PointCloudFilter::onExitPointCloudProcessor()
    {
 
    }
 
    void PointCloudFilter::process()
    {
        if (pointCloudFormat == "XYZRGBA")
        {
            processPointCloud<pcl::PointXYZRGBA>();
        }
        else if (pointCloudFormat == "XYZL")
        {
            processPointCloud<pcl::PointXYZL>();
        }
        else if (pointCloudFormat == "XYZRGBL")
        {
            processPointCloud<pcl::PointXYZRGBL>();
        }
        else
        {
            ARMARX_ERROR << "Could not process point cloud, because format '" << pointCloudFormat << "' is unknown."
                         << "\nKnown formats are: XYZL, XYZRGBA, XYZRGBL.";
        }
    }
 
    void PointCloudFilter::onDisconnectPointCloudProcessor()
    {
        if (remoteGuiTask)
        {
            remoteGuiTask->stop();
            remoteGuiTask = nullptr;
        }
        if (remoteGui)
        {
            remoteGui->removeTab(getName());
        }
    }
 
 
    static std::string toUpper(const std::string& string)
    {
        std::string upper;
        std::transform(string.begin(), string.end(), std::back_inserter(upper), ::toupper);
        return upper;
    }
 
    template <typename PointT>
    void PointCloudFilter::processPointCloud()
    {
        using PointCloudPtr = typename pcl::PointCloud<PointT>::Ptr;
 
        // Get current parameters.
        Parameters params;
        {
            std::unique_lock lock(parametersMutex);
            params = this->parameters;
        }
 
        // Only input cloud manages frame.
        FramedPointCloud<PointT> inputCloud(params.sourceFrameName);
        PointCloudPtr tempCloud(new pcl::PointCloud<PointT>());
        FramedPointCloud<PointT> outsideCloud(params.sourceFrameName);
 
        if (waitForPointClouds(10000))
        {
            getPointClouds(inputCloud.cloud);
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error while waiting for point cloud data.";
            return;
        }
 
        const uint32_t originalWidth = inputCloud.cloud->width;
        const uint32_t originalHeight = inputCloud.cloud->height;
 
        // Synchronize robot.
        if (robotStateComponent)
        {
            // visionx::MetaPointCloudFormatPtr format = getPointCloudFormat(providerName);
            // Use timestamp from point cloud instead of format->timeProvided
 
            long timestampMicroseconds = static_cast<long>(inputCloud.cloud->header.stamp);
            //            RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, timestampMicroseconds);
            (void) timestampMicroseconds;
            RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
        }
 
        if (params.statisticalOutlierRemoval.enabled)
        {
            // Perform downsampling.
            ARMARX_DEBUG << "statisticalOutlierRemoval ...";
            pcl::StatisticalOutlierRemoval<PointT> sor;
            sor.setMeanK(params.statisticalOutlierRemoval.meanK);
            sor.setStddevMulThresh(params.statisticalOutlierRemoval.stddevMulThresh);
 
            sor.setInputCloud(inputCloud.cloud);
            tempCloud->clear();
            sor.filter(*tempCloud);
            tempCloud.swap(inputCloud.cloud);
        }
 
        if (params.downsamplingEnabled)
        {
            // Perform downsampling.
            ARMARX_DEBUG << "Downsample ...";
            pcl::ApproximateVoxelGrid<PointT> grid;
            grid.setLeafSize(params.gridLeafSize, params.gridLeafSize, params.gridLeafSize);
            grid.setInputCloud(inputCloud.cloud);
 
            tempCloud->clear();
            grid.filter(*tempCloud);
 
            tempCloud.swap(inputCloud.cloud);
        }
 
        drawFrame(inputCloud.getFramedPose(localRobot), "source");
 
        ARMARX_DEBUG << "transformBy\n" << params.providerFrameTransformation;
        inputCloud.transformBy(params.providerFrameTransformation);
 
 
        if (params.croppingEnabled)
        {
            try
            {
                // Transform to cropping frame.
                changePointCloudFrame(inputCloud, "cropping", params.croppingFrameName);
            }
            catch (const armarx::LocalException& e)
            {
                ARMARX_ERROR << deactivateSpam(5) << "Caught armarx::LocalException (stopping processing).\n" << e.what();
                return;
            }
 
 
            // Perform cropping.
            ARMARX_DEBUG << "Crop ...";
 
            tempCloud->clear();
            outsideCloud.cloud->clear();
            outsideCloud.setFrameWithoutTransformation(inputCloud.getFrame());
            const Eigen::Matrix3f transform = VirtualRobot::MathTools::rpy2eigen3f(params.croppingRPY).inverse();
            for (const auto& p : inputCloud.cloud->points)
            {
                const Eigen::Vector3f transfed = transform * Eigen::Vector3f{p.x, p.y, p.z};
                if (
                    transfed(0) >= params.croppingMinPoint(0) && transfed(0) <= params.croppingMaxPoint(0) &&
                    transfed(1) >= params.croppingMinPoint(1) && transfed(1) <= params.croppingMaxPoint(1) &&
                    transfed(2) >= params.croppingMinPoint(2) && transfed(2) <= params.croppingMaxPoint(2))
                {
                    tempCloud->push_back(p);
                }
                else if (params.reportCloudOutsideOfCroppingArea)
                {
                    outsideCloud.cloud->push_back(p);
                }
            }
            tempCloud.swap(inputCloud.cloud);
        }
 
        if (debugDrawer.enabled())
        {
            // Draw crop box.
            Eigen::Matrix32f aabb;
            aabb.col(0) = params.croppingMinPoint;
            aabb.col(1) = params.croppingMaxPoint;
            debugDrawer.drawBoxEdges(
                "cropBox", aabb,
                inputCloud.getGlobalPose(localRobot) * VirtualRobot::MathTools::rpy2eigen4f(params.croppingRPY));
        }
        else
        {
            debugDrawer.removeboxEdges("cropBox");
        }
 
        const bool collisionModelFilterEnabled = params.applyCollisionModelFilter && robotStateComponent;
        if (collisionModelFilterEnabled)
        {
            // This requires inputCloud to be in global frame.
            changePointCloudFrame(inputCloud, "collision model filter", armarx::GlobalFrame);
            if (params.reportCloudOutsideOfCroppingArea)
            {
                changePointCloudFrame(outsideCloud, "collision model filter", armarx::GlobalFrame);
            }
 
            ARMARX_DEBUG << "Apply collision model filter ...";
 
            PointCloudPtr collisionCloudPtr(new pcl::PointCloud<PointT>());
 
            for (VirtualRobot::CollisionModelPtr collisionModel : localRobot->getCollisionModels())
            {
                std::vector<Eigen::Vector3f> vertices = collisionModel->getModelVeticesGlobal();
 
                pcl::PointCloud<PointT> hullCloud;
                hullCloud.width = static_cast<uint32_t>(vertices.size());
                hullCloud.height = 1;
                hullCloud.points.resize(vertices.size());
                for (size_t i = 0; i < vertices.size(); ++i)
                {
                    hullCloud.points[i].getVector3fMap() = vertices[i].cast<float>();
                }
 
                (*collisionCloudPtr) += hullCloud;
            }
 
            pcl::ConvexHull<PointT> hull;
            hull.setInputCloud(collisionCloudPtr);
            hull.setDimension(3);
 
            std::vector<pcl::Vertices> polygons;
            PointCloudPtr surfaceHull(new pcl::PointCloud<PointT>);
            hull.reconstruct(*surfaceHull, polygons);
 
            pcl::CropHull<PointT> cropHull;
            cropHull.setDim(3);
            cropHull.setHullIndices(polygons);
            cropHull.setHullCloud(surfaceHull);
            cropHull.setCropOutside(false);  // Remove points inside hull.
            cropHull.setInputCloud(inputCloud.cloud);
 
            tempCloud->clear();
            cropHull.filter(*tempCloud);
 
            tempCloud.swap(inputCloud.cloud);
 
            if (params.reportCloudOutsideOfCroppingArea)
            {
                cropHull.setInputCloud(outsideCloud.cloud);
                tempCloud->clear();
                cropHull.filter(*tempCloud);
                tempCloud.swap(outsideCloud.cloud);
            }
 
        }
 
        // Transform to target.
        try
        {
            // Transform to target frame.
            changePointCloudFrame(inputCloud, "target", params.targetFrameName);
            if (params.reportCloudOutsideOfCroppingArea)
            {
                changePointCloudFrame(outsideCloud, "collision model filter", params.targetFrameName);
            }
        }
        catch (const armarx::LocalException& e)
        {
            ARMARX_ERROR << deactivateSpam(5) << "Caught armarx::LocalException (stopping processing).\n" << e.what();
            return;
        }
 
        // Provide result.
        PointCloudPtr resultCloud = inputCloud.cloud;
 
        ARMARX_DEBUG << deactivateSpam(5)
                     << "Input cloud " << originalWidth << " x " << originalHeight
                     << " --> Filtered cloud " << resultCloud->width <<  " x " << resultCloud->height;
 
        provideResultPointClouds(getName() + "Result", resultCloud);
        provideResultPointClouds(getName() + "Outside", outsideCloud.cloud);
    }
 
 
    void PointCloudFilter::drawFrame(const FramedPose& frame, const std::string& role)
    {
        if (debugDrawer.enabled())
        {
            debugDrawer.drawPose(toUpper(role) + " (" + frame.getFrame() + ")", frame.toEigen());
        }
    }
 
 
 
    template<typename PointT>
    void PointCloudFilter::changePointCloudFrame(FramedPointCloud<PointT>& pointCloud,
            const std::string& role, const std::string& frame)
    {
        ARMARX_DEBUG << "Transform from " << pointCloud.getFrame()
                     << " to " << toUpper(role) << " (" << frame << ")";
        pointCloud.changeFrame(frame, localRobot);
        drawFrame(pointCloud.getFramedPose(localRobot), role);
    }
 
 
    armarx::PropertyDefinitionsPtr PointCloudFilter::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new PointCloudFilterPropertyDefinitions(getConfigIdentifier()));
    }
 
 
    void PointCloudFilter::remoteGuiCreate()
    {
        using namespace armarx::RemoteGui;
 
        const auto& p = parameters;
 
        auto rootLayout = makeVBoxLayout();
 
        // Misc
        {
            rootLayout.addChild(
                makeSimpleGridLayout().cols(2)
                .addChild(makeTextLabel("Source Frame: "))
                .addChild(makeHBoxLayout()
                          .addChild(makeCheckBox("SourceFrameAutoEnabled")
                                    .label("Auto").value(getProperty<bool>("sourceFrameNameAuto")))
                          .addChild(makeComboBox("SourceFrame")
                                    .addOptions({armarx::GlobalFrame, "root"})
                                    .addOptions(localRobot->getRobotNodeNames())
                                    .value(p.sourceFrameName)))
 
                .addChild(makeTextLabel("Target Frame: "))
                .addChild(makeComboBox("TargetFrame")
                          .options({armarx::GlobalFrame, "root"})
                          .addOptions(localRobot->getRobotNodeNames())
                          .value(p.targetFrameName))
 
                .addChild(makeTextLabel("StatisticalOutlierRemoval: "))
                .addChild(makeHBoxLayout()
                          .addChild(makeCheckBox("StatisticalOutlierRemoval_enabled")
                                    .label("Enabled").value(p.statisticalOutlierRemoval.enabled))
                          .addChild(makeTextLabel("MeanK"))
                          .addChild(makeIntSpinBox("StatisticalOutlierRemoval_MeanK")
                                    .min(1).max(1000).value(p.statisticalOutlierRemoval.meanK))
                          .addChild(makeTextLabel("StddevMulThresh"))
                          .addChild(makeFloatSpinBox("StatisticalOutlierRemoval_StddevMulThresh")
                                    .min(0.001f).max(100.f).decimals(3).value(p.statisticalOutlierRemoval.stddevMulThresh)))
 
                .addChild(makeTextLabel("Downsampling: "))
                .addChild(makeHBoxLayout()
                          .addChild(makeCheckBox("DownsamplingEnabled")
                                    .label("Enabled").value(p.downsamplingEnabled))
                          .addChild(makeTextLabel("Grid Leaf Size: "))
                          .addChild(makeFloatSpinBox("DownsamplingGridLeafSize")
                                    .min(0.1f).max(1000.f).decimals(1).value(p.gridLeafSize)))
 
                .addChild(makeTextLabel("Collision Model Filter: "))
                .addChild(makeCheckBox("CollisionModelFilterEnabled")
                          .label("Enabled").value(p.applyCollisionModelFilter))
            );
        }
 
        //transform
        {
            const Eigen::Vector3f pos = simox::math::mat4f_to_pos(p.providerFrameTransformation);
            const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(p.providerFrameTransformation);
 
            rootLayout.addChild(
                makeGroupBox("Transform").addChild(
                    makeSimpleGridLayout().cols(2)
                    .addTextLabel("XYC")
                    .addChild(makeVector3fSpinBoxes("transform.XYC").value(pos).min(-10000).max(10000).decimals(3))
                    .addTextLabel("RPY")
                    .addChild(makeVector3fSpinBoxes("transform.RPY").value(rpy).min(-2 * M_PI).max(2 * M_PI).decimals(3))
                ));
        }
 
        // Cropping
        {
            auto cropGroupLayout = makeVBoxLayout();
 
            WidgetPtr frameLabel = makeTextLabel("Frame: ");
            WidgetPtr minLabel = makeTextLabel("Minimum: ");
            WidgetPtr maxLabel = makeTextLabel("Maximum: ");
            WidgetPtr rpyLabel = makeTextLabel("Roll / Pitch / Yaw: ");
 
            WidgetPtr enabledCheckBox = makeCheckBox("CropEnabled").value(p.croppingEnabled).label("Enabled");
            WidgetPtr reportAllBox = makeCheckBox("reportCloudOutsideOfCroppingArea").value(p.reportCloudOutsideOfCroppingArea).label("Report points outside");
 
            WidgetPtr frameLine = makeComboBox("CropFrame")
                                  .options({armarx::GlobalFrame, "root"})
                                  .addOptions(localRobot->getRobotNodeNames())
                                  .value(p.croppingFrameName);
            WidgetPtr minVector3Spin = makeVector3fSpinBoxes("CropMin").value(p.croppingMinPoint)
                                       .min(-100000).max(100000);
            WidgetPtr maxVector3Spin = makeVector3fSpinBoxes("CropMax").value(p.croppingMaxPoint)
                                       .min(-100000).max(100000);
            WidgetPtr rpyVector3Spin = makeVector3fSpinBoxes("CropRPY").value(p.croppingRPY)
                                       .min(-M_PI).max(M_PI).decimals(3);
 
            cropGroupLayout.addChild(makeSimpleGridLayout().cols(2)
                                     .children(
            {
                enabledCheckBox, reportAllBox,
                frameLabel, frameLine,
                minLabel, minVector3Spin,
                maxLabel, maxVector3Spin,
                rpyLabel, rpyVector3Spin
            }));
 
            rootLayout.addChild(makeGroupBox("CropGroup")
                                .label("Cropping").child(cropGroupLayout));
        }
 
        // Visualization
        {
            auto visuGroupLayout = makeVBoxLayout();
 
            WidgetPtr enabledCheckBox = makeCheckBox("VisuEnabled")
                                        .label("Enabled").value(debugDrawer.enabled());
 
            visuGroupLayout.addChild(makeSimpleGridLayout().cols(2)
                                     .addChild(enabledCheckBox));
 
            rootLayout.addChild(makeGroupBox("VisuGroup")
                                .label("Visualization").child(visuGroupLayout));
        }
 
 
        const std::string tabName = getName();
        remoteGui->createTab(tabName, rootLayout);
        remoteGuiTab = armarx::RemoteGui::TabProxy(remoteGui, tabName);
    }
 
 
    void PointCloudFilter::remoteGuiRun()
    {
        int cycleDurationMs = 50;
        CycleUtil c(cycleDurationMs);
        while (!remoteGuiTask->isStopped())
        {
            remoteGuiTab.receiveUpdates();
 
            remoteGuiUpdate(remoteGuiTab);
 
            remoteGuiTab.sendUpdates();
            c.waitForCycleDuration();
        }
    }
 
 
    void PointCloudFilter::remoteGuiUpdate(RemoteGui::TabProxy& tab)
    {
        using namespace armarx::RemoteGui;
 
        {
            std::unique_lock lock(parametersMutex);
 
            // Frames
            ValueProxy<std::string> sourceFrame = tab.getValue<std::string>("SourceFrame");
 
            if (tab.getValue<bool>("SourceFrameAutoEnabled").get())
            {
                parameters.sourceFrameName = this->providerSourceFrameName;
                sourceFrame.set(parameters.sourceFrameName);
                // sourceFrame.setDisabled(true);  // Seems not to do the right thing.
            }
            else
            {
                parameters.sourceFrameName = sourceFrame.get();
                // sourceFrame.setDisabled(false);  // Seems not to do the right thing.
            }
 
            tab.getValue(parameters.targetFrameName, "TargetFrame");
 
            // Misc
            tab.getValue(parameters.downsamplingEnabled, "DownsamplingEnabled");
            tab.getValue(parameters.gridLeafSize, "DownsamplingGridLeafSize");
            tab.getValue(parameters.applyCollisionModelFilter, "CollisionModelFilterEnabled");
            tab.getValue(parameters.reportCloudOutsideOfCroppingArea, "reportCloudOutsideOfCroppingArea");
            tab.getValue(parameters.statisticalOutlierRemoval.enabled, "StatisticalOutlierRemoval_enabled");
            tab.getValue(parameters.statisticalOutlierRemoval.meanK, "StatisticalOutlierRemoval_MeanK");
            tab.getValue(parameters.statisticalOutlierRemoval.stddevMulThresh, "StatisticalOutlierRemoval_StddevMulThresh");
 
            // Cropping
            tab.getValue(parameters.croppingEnabled, "CropEnabled");
            tab.getValue(parameters.croppingFrameName, "CropFrame");
            tab.getValue(parameters.croppingMinPoint, "CropMin");
            tab.getValue(parameters.croppingMaxPoint, "CropMax");
            tab.getValue(parameters.croppingRPY, "CropRPY");
 
            parameters.providerFrameTransformation = simox::math::pos_rpy_to_mat4f(
                        tab.getValue<Eigen::Vector3f>("transform.XYC").get(),
                        tab.getValue<Eigen::Vector3f>("transform.RPY").get()
                    );
 
        }
 
        // Visu
        bool visuEnabled = tab.getValue<bool>("VisuEnabled").get();
        if (!visuEnabled)
        {
            // This will clear the layer if the visualization was enabled before.
            debugDrawer.clearLayer(false);
        }
        // This has to come *after* clearing.
        debugDrawer.setEnabled(visuEnabled);
 
    }
 
 
}