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 <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/interface/components/RGBDImageProvider.h>
 
#include <Eigen/Core>
 
#include <opencv2/opencv.hpp>
 
#include <VisionX/core/CapturingImageProvider.h>
 
#include <librealsense2/rs.hpp>
#include <librealsense2/h/rs_option.h>
 
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;
 
    };
}
 
#endif