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 <pcl/point_types.h>
#include <pcl/common/transforms.h>
 
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/tools/ImageUtil.h>
 
#include "MaskFilterPointCloudProcessor.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);
    }
}