IntelRealSenseProvider.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::IntelRealSenseProvider
 * @author     Simon Thelen ( urday at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#ifndef _ARMARX_COMPONENT_VisionX_IntelRealSenseProvider_H
#define _ARMARX_COMPONENT_VisionX_IntelRealSenseProvider_H
 
 
#include <Eigen/Core>
 
#include <opencv2/opencv.hpp>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
#include <VisionX/components/pointcloud_core/CapturingPointCloudProvider.h>
#include <VisionX/components/pointcloud_provider/ImageToPointCloud/DepthImageUtils.h>
#include <VisionX/core/CapturingImageProvider.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
 
#include <librealsense2/h/rs_option.h>
#include <librealsense2/rs.hpp>
 
namespace visionx
{
    /**
     * @class IntelRealSenseProviderPropertyDefinitions
     * @brief
     */
    class IntelRealSenseProviderPropertyDefinitions :
        public visionx::CapturingPointCloudProviderPropertyDefinitions
    {
    public:
        IntelRealSenseProviderPropertyDefinitions(std::string prefix) :
            visionx::CapturingPointCloudProviderPropertyDefinitions(prefix)
        {
            defineOptionalProperty<Eigen::Vector2i>(
                "Resolution", Eigen::Vector2i(1280, 720), "Resolution of the RGB and depth image.")
                .map("424x240", Eigen::Vector2i(424, 240))
                .map("480x270", Eigen::Vector2i(480, 270))
                .map("640x360", Eigen::Vector2i(640, 360))
                .map("640x480", Eigen::Vector2i(640, 480))
                .map("848x480", Eigen::Vector2i(848, 480))
                .map("960x540", Eigen::Vector2i(960, 540))
                .map("1280x720", Eigen::Vector2i(1280, 720));
 
            defineOptionalPropertyVector("FieldOfView",
                                         Eigen::Vector2f(69.4, 42.5),
                                         "The horizontal and vertical field of view.");
 
            defineOptionalProperty<std::string>(
                "ConfigFile",
                "",
                "JSON config file (e.g. from the realsense-viewer) to load. If property is not "
                "set, the preset property is used.");
            defineOptionalProperty<rs2_rs400_visual_preset>(
                "Preset",
                RS2_RS400_VISUAL_PRESET_MEDIUM_DENSITY,
                "RealSense preset. Only used if ConfigFile property is not set.")
                .setCaseInsensitive(true)
                .map("CUSTOM", RS2_RS400_VISUAL_PRESET_CUSTOM)
                .map("DEFAULT", RS2_RS400_VISUAL_PRESET_DEFAULT)
                .map("HAND", RS2_RS400_VISUAL_PRESET_HAND)
                .map("HIGH_ACCURACY", RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY)
                .map("HIGH_DENSITY", RS2_RS400_VISUAL_PRESET_HIGH_DENSITY)
                .map("MEDIUM_DENSITY", RS2_RS400_VISUAL_PRESET_MEDIUM_DENSITY)
                .map("REMOVE_IR_PATTERN", RS2_RS400_VISUAL_PRESET_REMOVE_IR_PATTERN);
 
            defineOptionalProperty<float>(
                "MaxDepth",
                6000.0f,
                "Max depth values allowed in Millimeters. Values above these are set to nan.",
                armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ApplySpatialFilter",
                                         false,
                                         "Apply a spatial filter to the depth image.",
                                         armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ApplyAlignmentFilter",
                                         true,
                                         "Align the depth image with the rgb image.",
                                         armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ApplyTemporalFilter",
                                         false,
                                         "Apply a temporal filter that smoothes over several "
                                         "frames. Leads to artifacts in combination with motion.",
                                         armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ApplyDisparityFilter",
                                         true,
                                         "Apply the disparity transformation filter.",
                                         armarx::PropertyDefinitionBase::eModifiable);
        }
    };
 
    /**
     * @defgroup Component-IntelRealSenseProvider IntelRealSenseProvider
     * @ingroup VisionX-Components
     * Provides support for Intel RealSense cameras for ArmarX.
     *
     * @class IntelRealSenseProvider
     * @ingroup Component-IntelRealSenseProvider
     * @brief Brief description of class IntelRealSenseProvider.
     *
     *
     */
    class IntelRealSenseProvider :
        virtual public visionx::RGBDPointCloudProviderInterface,
        virtual public visionx::CapturingPointCloudProvider,
        virtual public visionx::ImageProvider
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        virtual std::string getDefaultName() const override;
 
    protected:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
        // ManagedIceObject interface
    protected:
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
 
        void onExitComponent();
 
        // StereoCalibrationInterface interface
    public:
        visionx::StereoCalibration
        getStereoCalibration(const Ice::Current& c = Ice::emptyCurrent) override;
        bool getImagesAreUndistorted(const ::Ice::Current& c = Ice::emptyCurrent) override;
 
        std::string getReferenceFrame(const Ice::Current& c = Ice::emptyCurrent) override;
 
        visionx::StereoCalibration
        createStereoCalibration(const rs2::pipeline_profile& selection) const;
 
    protected:
        void onStartCapture(float framesPerSecond);
        void onStopCapture();
        void onInitImageProvider() override;
 
        void
        onExitImageProvider() override
        {
        }
 
        void onInitCapturingPointCloudProvider() override;
        void onExitCapturingPointCloudProvider() override;
        bool doCapture() override;
 
        bool
        hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
        MetaPointCloudFormatPtr
        getDefaultPointCloudFormat()
        {
            MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
            //info->frameId = getProperty<std::string>("frameId").getValue();
            info->type = PointContentType::eColoredPoints;
            info->capacity = 1280 * 720 * sizeof(ColoredPoint3D); // + info->frameId.size();
            info->size = info->capacity;
            return info;
        }
 
    private:
        rs2::pipeline pipe;
        rs2::colorizer color_map;
        visionx::CByteImageUPtr depthImage;
        using CloudPointType = pcl::PointXYZRGBA;
        pcl::PointCloud<CloudPointType>::Ptr pointcloud;
        armarx::DepthImageUtils depthUtils;
        MetaPointCloudFormatPtr cloudFormat;
 
        visionx::StereoCalibration calibration;
 
        // Declare pointcloud object, for calculating pointclouds and texture mappings
        rs2::pointcloud pc;
        // We want the points object to be persistent so we can display the last cloud when a frame drops
        rs2::points points;
 
        rs2::spatial_filter spat_filter;
        std::unique_ptr<rs2::align> alignFilter;
        rs2::temporal_filter temporal_filter;
        rs2::disparity_transform depth_to_disparity_filter,
            disparity_to_depth_filter = rs2::disparity_transform(false);
 
        float depthScale = 1;
    };
} // namespace visionx
 
#endif