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 <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
#include <RobotAPI/interface/core/RobotState.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};
    };
} // namespace armarx