MaskFilterPointCloudProcessor.h

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
 */
 
#pragma once
 
#include <atomic>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
#include <RobotAPI/interface/core/RobotState.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <VisionX/components/pointcloud_core/PointCloudAndImageProcessor.h>
 
namespace armarx
{
    /**
     * @class MaskFilterPointCloudProcessorPropertyDefinitions
     * @brief
     */
    class MaskFilterPointCloudProcessorPropertyDefinitions:
        public visionx::PointCloudProcessorPropertyDefinitions
    {
    public:
        MaskFilterPointCloudProcessorPropertyDefinitions(std::string prefix):
            visionx::PointCloudProcessorPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
 
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui");
 
            defineRequiredProperty<std::string>("MaskImageProvider", "The Image Provider used to get a mask");
 
            defineOptionalProperty<std::string>("PointCloudProviderReferenceFrameOverride", "", "If set, the component will assume incoming point clouds to be in this frame");
            defineOptionalProperty<std::string>("MaskImageProviderReferenceFrameOverride", "", "If set, the component will assume incoming mask images to be in this frame");
 
            defineOptionalProperty<std::string>("CalibrationProviderName", "", "An alternative component to receive calibration data from (mono or stereo)");
 
            defineOptionalProperty<std::string>("PointCloudReportFrame", "root", "The frame used to report the point clouds");
 
            defineOptionalProperty<int>("MaskRedHi", 255, "Higher bound for the mask color's red channel");
            defineOptionalProperty<int>("MaskGreenHi", 255, "Higher bound for the mask color's green channel");
            defineOptionalProperty<int>("MaskBlueHi", 255, "Higher bound for the mask color's blue channel");
            defineOptionalProperty<int>("MaskRedLo", 0, "Lower bound for the mask color's red channel");
            defineOptionalProperty<int>("MaskGreenLo", 0, "Lower bound for the mask color's green channel");
            defineOptionalProperty<int>("MaskBlueLo", 128, "Lower bound for the mask color's blue channel");
        }
    };
 
    /**
     * @defgroup Component-MaskFilterPointCloudProcessor MaskFilterPointCloudProcessor
     * @ingroup VisionX-Components
     * A description of the component MaskFilterPointCloudProcessor.
     *
     * @class MaskFilterPointCloudProcessor
     * @ingroup Component-MaskFilterPointCloudProcessor
     * @brief Brief description of class MaskFilterPointCloudProcessor.
     *
     * Detailed description of class MaskFilterPointCloudProcessor.
     */
    class MaskFilterPointCloudProcessor :
        virtual public visionx::PointCloudAndImageProcessor
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "MaskFilterPointCloudProcessor";
        }
 
    protected:
        void process() override;
 
        void onInitPointCloudAndImageProcessor() override;
        void onConnectPointCloudAndImageProcessor() override;
        void onDisconnectPointCloudAndImageProcessor() override;
        void onExitPointCloudAndImageProcessor() override {}
 
        std::pair<float, float> maskImageProviderFocalLength() const;
 
        std::array<std::int64_t, 2> xyz2uv(float x, float y, float z, float fx, float fy) const;
        std::array<float, 3> uvz2xyz(int u, int v, float z, float fx, float fy) const;
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
    protected:
        template<typename PointType> void process();
        //robot sync
        std::string                                 _robotStateComponentName;
        RobotStateComponentInterfacePrx             _robotStateComponent;
        VirtualRobot::RobotPtr                      _localRobot;
        std::atomic_bool                            _syncRobotWithTimestamp{false};
        //visualize
        std::string                                 _debugDrawerTopicName;
        DebugDrawerInterfacePrx                     _debugDrawerTopic;
        std::string                                 _remoteGuiName;
        RemoteGuiInterfacePrx                       _remoteGui;
        SimplePeriodicTask<>::pointer_type          _guiTask;
        RemoteGui::TabProxy                         _guiTab;
        //filter settings
        std::atomic<std::uint8_t>                   _redLo;
        std::atomic<std::uint8_t>                   _redHi;
        std::atomic<std::uint8_t>                   _blueLo;
        std::atomic<std::uint8_t>                   _blueHi;
        std::atomic<std::uint8_t>                   _greenLo;
        std::atomic<std::uint8_t>                   _greenHi;
        std::atomic<float>                          _focalLengthAdjustment;
        std::atomic<float>                          _maskZRotation;
        std::mutex                                  _pointCloudReportFrameMutex;
        std::string                                 _pointCloudReportFrame;
        std::atomic_bool                            _flipX {false};
        std::atomic_bool                            _flipY {false};
        std::atomic_bool                            _maskMatchOneRangeInsteadOfAll {false};
        std::atomic<std::int64_t>                   _offsetX;
        std::atomic<std::int64_t>                   _offsetY;
        std::atomic<std::int64_t>                   _guiParamUpdated{false};
        //point clouds
        std::string                                 _pointCloudProviderName;
        visionx::PointCloudProviderInfo             _pointCloudProviderInfo;
        visionx::PointCloudProviderInterfacePrx     _pointCloudProvider;
        std::string                                 _pointCloudProviderRefFrame;
 
        std::mutex                                  _cloudMutex;
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr     _maskedCloud;
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr     _inputCloud;
        //mask image
        std::string                                 _maskImageProviderName;
        visionx::ImageProviderInfo                  _maskImageProviderInfo;
        visionx::ImageProviderInterfacePrx          _maskImageProvider;
        std::vector<visionx::CByteImageUPtr>        _maskImageProviderImageOwner;
        std::vector<void*>                          _maskImageProviderImages;
        std::string                                 _maskImageProviderRefFrame;
        float                                       _maskImageProviderFocalLengthX;
        float                                       _maskImageProviderFocalLengthY;
        //state
        bool _newMaskImage {false};
        bool _newPointCloud {false};
    };
}