MaskFilterPointCloudProcessor.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    VisionX::ArmarXObjects::MaskFilterPointCloudProcessor
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "MaskFilterPointCloudProcessor.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/tools/ImageUtil.h>
 
static constexpr auto InMaskSuff = "_in_mask";
static constexpr auto OutOfMaskSuff = "_out_of_mask";
static constexpr auto OutOfMaskImgSuff = "_out_of_image";
static constexpr auto MaskVisuSuff = "_mask_visu";
static constexpr auto MaskAsCloudSuff = "_mask_as_cloud";
 
#define call_template_function(F)                                                                            \
    switch (_pointCloudProviderInfo.pointCloudFormat->type)                                                  \
    {                                                                                                        \
            /*case visionx::PointContentType::ePoints                 : F<pcl::PointXYZ>(); break;     */    \
        case visionx::PointContentType::eColoredPoints:                                                      \
            F<pcl::PointXYZRGBA>();                                                                          \
            break;                                                                                           \
        case visionx::PointContentType::eColoredOrientedPoints:                                              \
            F<pcl::PointXYZRGBNormal>();                                                                     \
            break;                                                                                           \
            /*case visionx::PointContentType::eLabeledPoints          : F<pcl::PointXYZL>(); break;       */ \
        case visionx::PointContentType::eColoredLabeledPoints:                                               \
            F<pcl::PointXYZRGBL>();                                                                          \
            break;                                                                                           \
            /*case visionx::PointContentType::eIntensity              : F<pcl::PointXYZI>(); break;   */     \
        case visionx::PointContentType::eOrientedPoints:                                                     \
            ARMARX_ERROR << "eOrientedPoints NOT HANDLED IN VISIONX"; /*no break*/                           \
            [[fallthrough]];                                                                                 \
        default:                                                                                             \
            ARMARX_ERROR << "Could not process point cloud, because format '"                                \
                         << _pointCloudProviderInfo.pointCloudFormat->type << "' is unknown";                \
    }                                                                                                        \
    do                                                                                                       \
    {                                                                                                        \
    } while (false)
 
using namespace armarx;
 
void
MaskFilterPointCloudProcessor::process()
{
    call_template_function(process);
}
 
void
MaskFilterPointCloudProcessor::onInitPointCloudAndImageProcessor()
{
    _pointCloudProviderName = getProperty<std::string>("ProviderName");
    _debugDrawerTopicName = getProperty<std::string>("DebugDrawerTopicName");
    _remoteGuiName = getProperty<std::string>("RemoteGuiName");
    _robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
    _maskImageProviderName = getProperty<std::string>("MaskImageProvider");
 
    usingPointCloudProvider(_pointCloudProviderName);
    usingImageProvider(_maskImageProviderName);
 
    offeringTopic(_debugDrawerTopicName);
    usingProxy(_remoteGuiName);
    usingProxy(_robotStateComponentName);
 
    _redHi = static_cast<std::uint8_t>(getProperty<int>("MaskRedHi"));
    _redLo = static_cast<std::uint8_t>(getProperty<int>("MaskRedLo"));
    _greenHi = static_cast<std::uint8_t>(getProperty<int>("MaskGreenHi"));
    _greenLo = static_cast<std::uint8_t>(getProperty<int>("MaskGreenLo"));
    _blueHi = static_cast<std::uint8_t>(getProperty<int>("MaskBlueHi"));
    _blueLo = static_cast<std::uint8_t>(getProperty<int>("MaskBlueLo"));
 
    _pointCloudReportFrame = getProperty<std::string>("PointCloudReportFrame");
}
 
void
MaskFilterPointCloudProcessor::onConnectPointCloudAndImageProcessor()
{
    //sync
    {
        _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
        _localRobot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent,
                                                            VirtualRobot::RobotIO::eStructure);
    }
    //debug visu
    {
        _debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(_debugDrawerTopicName);
    }
    //point cloud provider
    {
        enableResultPointClouds<pcl::PointXYZRGBA>(getName() + InMaskSuff);
        enableResultPointClouds<pcl::PointXYZRGBA>(getName() + OutOfMaskSuff);
        enableResultPointClouds<pcl::PointXYZRGBA>(getName() + OutOfMaskImgSuff);
        enableResultPointClouds<pcl::PointXYZRGBA>(getName() + MaskVisuSuff);
        enableResultPointClouds<pcl::PointXYZRGBA>(getName() + MaskAsCloudSuff);
 
        _pointCloudProviderInfo = getPointCloudProvider(_pointCloudProviderName, true);
        _pointCloudProvider = _pointCloudProviderInfo.proxy;
        _pointCloudProviderRefFrame =
            getProperty<std::string>("PointCloudProviderReferenceFrameOverride");
        if (_pointCloudProviderRefFrame.empty())
        {
            _pointCloudProviderRefFrame = "root";
            auto frameprov = visionx::ReferenceFrameInterfacePrx::checkedCast(_pointCloudProvider);
            if (frameprov)
            {
                _pointCloudProviderRefFrame = frameprov->getReferenceFrame();
            }
        }
        ARMARX_INFO << VAROUT(_pointCloudProviderRefFrame);
    }
    //guess some config params
    {
        _maskZRotation = -180;
        if (_pointCloudProviderRefFrame.size() >= 3 &&
            (_pointCloudProviderRefFrame.substr(_pointCloudProviderRefFrame.size() - 3) == "Sim" ||
             _pointCloudProviderRefFrame.substr(_pointCloudProviderRefFrame.size() - 3) == "sim"))
        {
            _maskZRotation = -90;
        }
    }
    //image provider
    {
        _maskImageProviderInfo = getImageProvider(_maskImageProviderName, true);
        _maskImageProvider = _maskImageProviderInfo.proxy;
        ARMARX_CHECK_EQUAL(_maskImageProviderInfo.imageFormat.type, visionx::ImageType::eRgb);
        _maskImageProviderImageOwner.clear();
        _maskImageProviderImages.clear();
        _maskImageProviderImageOwner.reserve(_maskImageProviderInfo.numberImages);
        ARMARX_CHECK_GREATER_EQUAL(_maskImageProviderInfo.numberImages, 1);
        _maskImageProviderImages.reserve(_maskImageProviderInfo.numberImages);
        for (int i = 0; i < _maskImageProviderInfo.numberImages; ++i)
        {
            _maskImageProviderImageOwner.emplace_back(
                visionx::tools::createByteImage(_maskImageProviderInfo));
            _maskImageProviderImages.emplace_back(
                static_cast<void*>(_maskImageProviderImageOwner.back()->pixels));
        }
 
 
        {
            _maskImageProviderRefFrame =
                getProperty<std::string>("MaskImageProviderReferenceFrameOverride");
            if (_maskImageProviderRefFrame.empty())
            {
                _maskImageProviderRefFrame = _pointCloudProviderRefFrame;
                auto frameprov =
                    visionx::ReferenceFrameInterfacePrx::checkedCast(_maskImageProvider);
                if (frameprov)
                {
                    _maskImageProviderRefFrame = frameprov->getReferenceFrame();
                }
            }
            ARMARX_INFO << VAROUT(_maskImageProviderRefFrame);
        }
 
        const std::string calibProviderName = getProperty<std::string>("CalibrationProviderName");
        if (calibProviderName.empty())
        {
            auto mcalibprov =
                visionx::MonocularCalibrationCapturingProviderInterfacePrx::checkedCast(
                    _maskImageProvider);
            auto scalibprov =
                visionx::StereoCalibrationInterfacePrx::checkedCast(_maskImageProvider);
            ARMARX_CHECK_EXPRESSION(mcalibprov || scalibprov);
            if (scalibprov)
            {
                _maskImageProviderFocalLengthX =
                    scalibprov->getStereoCalibration().calibrationLeft.cameraParam.focalLength.at(
                        0);
                _maskImageProviderFocalLengthY =
                    scalibprov->getStereoCalibration().calibrationLeft.cameraParam.focalLength.at(
                        1);
            }
            else if (mcalibprov)
            {
                _maskImageProviderFocalLengthX =
                    mcalibprov->getCalibration().cameraParam.focalLength.at(0);
                _maskImageProviderFocalLengthY =
                    mcalibprov->getCalibration().cameraParam.focalLength.at(1);
            }
        }
        else
        {
            auto calibProvider = getProxy<Ice::ObjectPrx>(calibProviderName);
            auto mcalibprov =
                visionx::MonocularCalibrationCapturingProviderInterfacePrx::checkedCast(
                    calibProvider);
            auto scalibprov = visionx::StereoCalibrationInterfacePrx::checkedCast(calibProvider);
            ARMARX_CHECK_EXPRESSION(mcalibprov || scalibprov);
            if (scalibprov)
            {
                _maskImageProviderFocalLengthX =
                    scalibprov->getStereoCalibration().calibrationLeft.cameraParam.focalLength.at(
                        0);
                _maskImageProviderFocalLengthY =
                    scalibprov->getStereoCalibration().calibrationLeft.cameraParam.focalLength.at(
                        1);
            }
            else if (mcalibprov)
            {
                _maskImageProviderFocalLengthX =
                    mcalibprov->getCalibration().cameraParam.focalLength.at(0);
                _maskImageProviderFocalLengthY =
                    mcalibprov->getCalibration().cameraParam.focalLength.at(1);
            }
        }
    }
    //gui
    {
        _remoteGui = getProxy<RemoteGuiInterfacePrx>(_remoteGuiName);
        RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
 
        auto makeSlider = [&](std::string name, int val, int min = 0, int max = 255)
        {
            rootLayoutBuilder.addChild(
                RemoteGui::makeHBoxLayout()
                    .addChild(RemoteGui::makeTextLabel(name))
                    .addChild(RemoteGui::makeIntSlider(name).min(min).max(max).value(val))
                    .addChild(
                        RemoteGui::makeIntSpinBox(name + "_spin").min(min).max(max).value(val)));
        };
        auto makeFSlider = [&](std::string name, int min, int max, float val)
        {
            rootLayoutBuilder.addChild(
                RemoteGui::makeHBoxLayout()
                    .addChild(RemoteGui::makeTextLabel(name))
                    .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(
                        static_cast<int>(max - min)))
                    .addChild(
                        RemoteGui::makeFloatSpinBox(name + "_spin").min(min).max(max).value(val)));
        };
 
        // ValueProxy<float> x1Slider = makeSlider ...
        makeSlider("red lo", _redLo);
        makeSlider("red hi", _redHi);
        makeSlider("green lo", _greenLo);
        makeSlider("green hi", _greenHi);
        makeSlider("blue lo", _blueLo);
        makeSlider("blue hi", _blueHi);
 
 
        rootLayoutBuilder.addChild(
            RemoteGui::makeHBoxLayout()
                .addChild(RemoteGui::makeTextLabel("mask match one color range"))
                .addChild(RemoteGui::makeCheckBox("ormode").value(_maskMatchOneRangeInsteadOfAll))
                .addChild(RemoteGui::makeTextLabel("flip x"))
                .addChild(RemoteGui::makeCheckBox("flip x").value(_flipX))
                .addChild(RemoteGui::makeTextLabel("flip y"))
                .addChild(RemoteGui::makeCheckBox("flip y").value(_flipY)));
 
        rootLayoutBuilder.addChild(
            RemoteGui::makeHBoxLayout()
                .addChild(RemoteGui::makeTextLabel("offset x"))
                .addChild(
                    RemoteGui::makeIntSpinBox("offset x").min(-2000).max(2000).value(_offsetX))
                .addChild(RemoteGui::makeTextLabel("offset y"))
                .addChild(
                    RemoteGui::makeIntSpinBox("offset y").min(-2000).max(2000).value(_offsetY)));
 
        _focalLengthAdjustment = 0;
        const int focaladjrange = 10000;
        makeFSlider(
            "focal length adjustment", -focaladjrange, focaladjrange, _focalLengthAdjustment);
        makeFSlider("Mask z rotation", -180, 180, _maskZRotation);
 
        rootLayoutBuilder.addChild(RemoteGui::makeHBoxLayout()
                                       .addChild(RemoteGui::makeTextLabel("report frame"))
                                       .addChild(RemoteGui::makeComboBox("report frame")
                                                     .options(_localRobot->getRobotNodeNames())
                                                     .addOptions({"Global", "root"})
                                                     .value("root")));
 
 
        auto checkboxBuilder = RemoteGui::makeCheckBox("Sync using timestamp");
        checkboxBuilder.value(_syncRobotWithTimestamp);
        checkboxBuilder.widget().label = "Sync using timestamp";
        rootLayoutBuilder.addChild(checkboxBuilder);
        rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
 
        _guiTask = new SimplePeriodicTask<>(
            [this]()
            {
                _guiTab.receiveUpdates();
 
                _redLo = static_cast<std::uint8_t>(_guiTab.getValue<int>("red lo").get());
                _guiTab.getValue<int>("red lo_spin").set(_redLo);
 
                _redHi = static_cast<std::uint8_t>(_guiTab.getValue<int>("red hi").get());
                _guiTab.getValue<int>("red hi_spin").set(_redHi);
 
                _greenLo = static_cast<std::uint8_t>(_guiTab.getValue<int>("green lo").get());
                _guiTab.getValue<int>("green lo_spin").set(_greenLo);
 
                _greenHi = static_cast<std::uint8_t>(_guiTab.getValue<int>("green hi").get());
                _guiTab.getValue<int>("green hi_spin").set(_greenHi);
 
                _blueLo = static_cast<std::uint8_t>(_guiTab.getValue<int>("blue lo").get());
                _guiTab.getValue<int>("blue lo_spin").set(_blueLo);
 
                _blueHi = static_cast<std::uint8_t>(_guiTab.getValue<int>("blue hi").get());
                _guiTab.getValue<int>("blue hi_spin").set(_blueHi);
 
                _offsetX = _guiTab.getValue<int>("offset x").get();
                _offsetY = _guiTab.getValue<int>("offset y").get();
 
                _maskZRotation = _guiTab.getValue<float>("Mask z rotation").get();
                _guiTab.getValue<float>("Mask z rotation_spin").set(_maskZRotation);
 
                _focalLengthAdjustment = _guiTab.getValue<float>("focal length adjustment").get();
                _guiTab.getValue<float>("focal length adjustment_spin").set(_focalLengthAdjustment);
 
                _flipX = _guiTab.getValue<bool>("flip x").get();
                _flipY = _guiTab.getValue<bool>("flip y").get();
                _maskMatchOneRangeInsteadOfAll = _guiTab.getValue<bool>("ormode").get();
 
                _guiTab.sendUpdates();
 
                std::lock_guard<std::mutex> guard{_pointCloudReportFrameMutex};
                _pointCloudReportFrame = _guiTab.getValue<std::string>("report frame").get();
                _guiParamUpdated = true;
                _syncRobotWithTimestamp = _guiTab.getValue<bool>("Sync using timestamp").get();
            },
            10);
 
        RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
 
        _remoteGui->createTab(getName(), rootLayout);
        _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
 
        _guiTask->start();
    }
}
 
void
MaskFilterPointCloudProcessor::onDisconnectPointCloudAndImageProcessor()
{
    _guiTask->stop();
    _guiTask = nullptr;
    _debugDrawerTopic->removeLayer(getName());
}
 
std::pair<float, float>
MaskFilterPointCloudProcessor::maskImageProviderFocalLength() const
{
    return {_maskImageProviderFocalLengthX + _focalLengthAdjustment,
            _maskImageProviderFocalLengthY + _focalLengthAdjustment};
}
 
std::array<float, 3>
MaskFilterPointCloudProcessor::uvz2xyz(int u, int v, float z, float /*fx*/, float /*fy*/) const
{
    const float alpha = -_maskZRotation / 180.0 * M_PI;
    const float s = std::sin(alpha);
    const float c = std::cos(alpha);
 
    const auto width = _maskImageProviderInfo.imageFormat.dimension.width;
    const auto height = _maskImageProviderInfo.imageFormat.dimension.height;
 
    //middle to 0/0 and add offset
    const float ushifted = u - +width / 2.f + _offsetX;
    const float vshifted = v - +height / 2.f + _offsetY;
 
    const float rotatedU = c * ushifted + s * vshifted;
    const float rotatedV = -s * ushifted + c * vshifted;
 
    const float x = (_flipX ? 1 : -1) * rotatedU;
    const float y = (_flipY ? 1 : -1) * rotatedV;
 
    return {x, y, z};
}
 
std::array<std::int64_t, 2>
MaskFilterPointCloudProcessor::xyz2uv(float x, float y, float z, float fx, float fy) const
{
    const float alpha = _maskZRotation / 180.0 * M_PI;
    const float s = std::sin(alpha);
    const float c = std::cos(alpha);
 
    //transform to uv
    const float u = (_flipX ? 1 : -1) * (x / z) * fx;
    const float v = (_flipY ? 1 : -1) * (y / z) * fy;
 
    const float rotatedU = c * u + -s * v;
    const float rotatedV = s * u + c * v;
 
    const auto width = _maskImageProviderInfo.imageFormat.dimension.width;
    const auto height = _maskImageProviderInfo.imageFormat.dimension.height;
 
    // 0/0 is currently the middle of the image -> shift it and sub offsets
    const std::int64_t imgx = static_cast<std::int64_t>(rotatedU + width / 2.f) - _offsetX;
    const std::int64_t imgy = static_cast<std::int64_t>(rotatedV + height / 2.f) - _offsetY;
 
    return {imgx, imgy};
}
 
armarx::PropertyDefinitionsPtr
MaskFilterPointCloudProcessor::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new MaskFilterPointCloudProcessorPropertyDefinitions(getConfigIdentifier()));
}
 
template <typename PointType>
void
MaskFilterPointCloudProcessor::process()
{
    //something new to do?
    {
        if (waitForPointClouds(_pointCloudProviderName, 10))
        {
            _newPointCloud = true;
        }
        if (waitForImages(_maskImageProviderName, 10))
        {
            _newMaskImage = true;
        }
 
        if (!_newMaskImage && !_newPointCloud && !_guiParamUpdated)
        {
            return;
        }
        _newMaskImage = false;
        _newPointCloud = false;
    }
 
    std::lock_guard<std::mutex> guard{_cloudMutex};
 
    //working data
    typename pcl::PointCloud<PointType>::Ptr inputCloud(new pcl::PointCloud<PointType>());
    //get new data
    {
        getPointClouds(_pointCloudProviderName, inputCloud);
 
        getImages(_maskImageProviderImages.data());
 
        auto format = getPointCloudFormat(_pointCloudProviderName);
 
        if (_syncRobotWithTimestamp)
        {
            RemoteRobot::synchronizeLocalCloneToTimestamp(
                _localRobot, _robotStateComponent, format->timeProvided);
        }
        else
        {
            RemoteRobot::synchronizeLocalClone(_localRobot, _robotStateComponent);
        }
    }
    Eigen::Matrix4f pclInCamFrame = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f camInReportFrame = Eigen::Matrix4f::Identity();
    {
        {
            std::lock_guard<std::mutex> guard{_pointCloudReportFrameMutex};
            if (_pointCloudReportFrame == "root")
            {
                _pointCloudReportFrame = _localRobot->getRootNode()->getName();
            }
            FramedPose fp{
                Eigen::Matrix4f::Identity(), _maskImageProviderRefFrame, _localRobot->getName()};
            fp.changeFrame(_localRobot, _pointCloudReportFrame);
            camInReportFrame = fp.toEigen();
        }
        {
            FramedPose fp{
                Eigen::Matrix4f::Identity(), _pointCloudProviderRefFrame, _localRobot->getName()};
            fp.changeFrame(_localRobot, _maskImageProviderRefFrame);
            pclInCamFrame = fp.toEigen();
        }
    }
 
    {
        typename pcl::PointCloud<PointType>::Ptr tmpCloud(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*inputCloud, *tmpCloud, pclInCamFrame);
        tmpCloud.swap(inputCloud);
    }
 
    const auto r = std::minmax(_redLo, _redHi);
    const auto g = std::minmax(_greenLo, _greenHi);
    const auto b = std::minmax(_blueLo, _blueHi);
    const CByteImage& img = *_maskImageProviderImageOwner.front();
    std::uint64_t pointsInImage = 0;
    std::uint64_t pointsOutOfImage = 0;
    _maskedCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>());
    _inputCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr unmaskedCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outOfImageCloud(
        new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr visuCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
    float fx, fy;
    std::tie(fx, fy) = maskImageProviderFocalLength();
    for (const auto& point : inputCloud->points)
    {
        pcl::PointXYZRGBA p;
        p.x = point.x;
        p.y = point.y;
        p.z = point.z;
        p.a = 255;
 
        static_assert(std::is_same<PointType, pcl::PointXYZRGBA>::value ||
                          std::is_same<PointType, pcl::PointXYZRGB>::value ||
                          std::is_same<PointType, pcl::PointXYZRGBL>::value ||
                          std::is_same<PointType, pcl::PointXYZRGBNormal>::value,
                      "");
        //        if constexpr(
        //            std::is_same_v<PointType, pcl::PointXYZRGBA> ||
        //            std::is_same_v<PointType, pcl::PointXYZRGB> ||
        //            std::is_same_v<PointType, pcl::PointXYZRGBL> ||
        //            std::is_same_v<PointType, pcl::PointXYZRGBNormal>
        //        )
        {
            p.r = point.r;
            p.g = point.g;
            p.b = point.b;
        }
        //        else
        //        {
        //            p.r = p.g = p.b = 0;
        //        }
        const auto uv = xyz2uv(p.x, p.y, p.z, fx, fy);
        const std::int64_t u = uv.at(0);
        const std::int64_t v = uv.at(1);
        if (u < 0 || u >= img.width || v < 0 || v >= img.height)
        {
            ++pointsOutOfImage;
            _inputCloud->push_back(p);
            p.r = p.b = p.a = 255;
            p.g = 0;
            outOfImageCloud->push_back(p);
            continue; // point out of image
        }
        ++pointsInImage;
 
        p.r = img.pixels[(u + v * img.width) * 3 + 0];
        p.g = img.pixels[(u + v * img.width) * 3 + 1];
        p.b = img.pixels[(u + v * img.width) * 3 + 2];
 
        _inputCloud->push_back(p);
 
        const bool okR = r.first <= p.r && r.second >= p.r;
        const bool okG = g.first <= p.g && g.second >= p.g;
        const bool okB = b.first <= p.b && b.second >= p.b;
 
        if (_maskMatchOneRangeInsteadOfAll ? okR || okG || okB : okR && okG && okB)
        {
            _maskedCloud->push_back(p);
        }
        else
        {
            unmaskedCloud->push_back(p);
        }
        visuCloud->push_back(p);
    }
 
    {
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tmpCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
        pcl::transformPointCloud(*_maskedCloud, *tmpCloud, camInReportFrame);
        provideResultPointClouds(tmpCloud, getName() + InMaskSuff);
 
        pcl::transformPointCloud(*unmaskedCloud, *tmpCloud, camInReportFrame);
        provideResultPointClouds(tmpCloud, getName() + OutOfMaskSuff);
 
        pcl::transformPointCloud(*outOfImageCloud, *tmpCloud, camInReportFrame);
        provideResultPointClouds(tmpCloud, getName() + OutOfMaskImgSuff);
 
        pcl::transformPointCloud(*visuCloud, *tmpCloud, camInReportFrame);
        provideResultPointClouds(tmpCloud, getName() + MaskVisuSuff);
 
        //reuse visuCloud
        visuCloud->clear();
        for (int u = 0; u < img.width; ++u)
        {
            for (int v = 0; v < img.height; ++v)
            {
                //visu the mastk at the image plane
                auto xyz = uvz2xyz(u, v, (fx + fy) / 2, fx, fy);
                pcl::PointXYZRGBA p;
                p.x = xyz.at(0);
                p.y = xyz.at(1);
                p.z = xyz.at(2);
                p.a = 255;
                p.r = img.pixels[(u + v * img.width) * 3 + 0];
                p.g = img.pixels[(u + v * img.width) * 3 + 1];
                p.b = img.pixels[(u + v * img.width) * 3 + 2];
                visuCloud->push_back(p);
            }
        }
        pcl::transformPointCloud(*visuCloud, *tmpCloud, camInReportFrame);
        provideResultPointClouds(tmpCloud, getName() + MaskAsCloudSuff);
    }
}
 
namespace armarx
{
    std::string
    MaskFilterPointCloudProcessor::GetDefaultName()
    {
        return "MaskFilterPointCloudProcessor";
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(
        ::armarx::MaskFilterPointCloudProcessor,
        ::armarx::MaskFilterPointCloudProcessor::GetDefaultName());
} // namespace armarx