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 <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <pcl/common/colors.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/surface/convex_hull.h>
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/math/Helpers.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()
    {
        return "PointCloudFilter";
    }
 
    std::string
    PointCloudFilter::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    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);
    }
 
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::PointCloudFilter, ::visionx::PointCloudFilter::GetDefaultName());
} // namespace visionx