IntelRealSenseProvider.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::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
 */
 
#include "IntelRealSenseProvider.h"
 
#include <stdio.h>
 
#include <filesystem>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Image/ImageProcessor.h>
#include <librealsense2/h/rs_sensor.h>
#include <librealsense2/rs.hpp>
#include <librealsense2/rs_advanced_mode.hpp>
#include <librealsense2/rsutil.h>
 
using namespace armarx;
using namespace visionx;
 
void
IntelRealSenseProvider::onInitImageProvider()
{
    Eigen::Vector2f fov = getProperty<Eigen::Vector2f>("FieldOfView");
    depthUtils.setFieldOfView(fov(0), fov(1));
    setNumberImages(2);
    Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("Resolution");
    setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)),
                   visionx::eRgb,
                   visionx::eBayerPatternGr);
    depthImage.reset(visionx::tools::createByteImage(getImageFormat(), visionx::eRgb));
    pointcloud.reset(new pcl::PointCloud<CloudPointType>());
 
    alignFilter.reset(new rs2::align(RS2_STREAM_COLOR));
}
 
void
IntelRealSenseProvider::onInitCapturingPointCloudProvider()
{
}
 
void
IntelRealSenseProvider::onExitCapturingPointCloudProvider()
{
}
 
void
IntelRealSenseProvider::onStartCapture(float framesPerSecond)
{
    ARMARX_VERBOSE << "starting pipe";
 
    //Create a configuration for configuring the pipeline with a non default profile
    rs2::config cfg;
 
    Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("Resolution").getValue();
    //Add desired streams to configuration
    cfg.enable_stream(
        RS2_STREAM_COLOR, dimensions(0), dimensions(1), RS2_FORMAT_RGB8, framesPerSecond);
    cfg.enable_stream(
        RS2_STREAM_DEPTH, dimensions(0), dimensions(1), RS2_FORMAT_Z16, framesPerSecond);
    rs2::pipeline_profile profile = pipe.start(cfg);
 
    rs2::device dev = profile.get_device();
 
    rs2::depth_sensor ds = dev.query_sensors().front().as<rs2::depth_sensor>();
 
    if (getProperty<std::string>("ConfigFile").isSet())
    {
        using namespace rs2;
        context ctx;
        auto devices = ctx.query_devices();
        size_t device_count = devices.size();
        if (!device_count)
        {
            throw armarx::LocalException("No device detected. Is it plugged in?\n");
        }
 
        // Get the first connected device
        auto dev = devices[0];
 
        // Enter advanced mode
        if (dev.is<rs400::advanced_mode>())
        {
            // Get the advanced mode functionality
            auto advanced_mode_dev = dev.as<rs400::advanced_mode>();
 
            // Load and configure .json file to device
            std::string path = getProperty<std::string>("ConfigFile");
            std::string absPath;
            auto configFileFound = armarx::ArmarXDataPath::getAbsolutePath(path, absPath);
            ARMARX_CHECK_EXPRESSION(configFileFound) << path;
            ARMARX_INFO << "Loading realsense config from " << path;
            std::ifstream t(path);
            std::string str((std::istreambuf_iterator<char>(t)), std::istreambuf_iterator<char>());
            advanced_mode_dev.load_json(str);
        }
        else
        {
            throw armarx::LocalException() << "Current device doesn't support advanced-mode!\n";
        }
    }
    else
    {
        auto preset = getProperty<rs2_rs400_visual_preset>("Preset").getValue();
        ARMARX_INFO << "Setting preset to: " << rs2_rs400_visual_preset_to_string(preset);
        ds.set_option(RS2_OPTION_VISUAL_PRESET, preset);
    }
 
    depthScale = ds.get_depth_scale();
    ARMARX_VERBOSE << "pipe started";
    ARMARX_VERBOSE << "depthScale: " << depthScale;
    cloudFormat = getPointCloudFormat();
    calibration = createStereoCalibration(profile);
 
    setMetaInfo(
        "Baseline",
        new Variant(std::to_string(calibration.calibrationRight.cameraParam.translation[0]) + ", " +
                    std::to_string(calibration.calibrationRight.cameraParam.translation[1]) + ", " +
                    std::to_string(calibration.calibrationRight.cameraParam.translation[2])));
}
 
void
IntelRealSenseProvider::onStopCapture()
{
    pipe.stop();
}
 
bool
IntelRealSenseProvider::doCapture()
{
    rs2::frameset data;
    int timeout = 1000;
    try
    {
        data = pipe.wait_for_frames(timeout);
    }
    catch (...)
    {
        ARMARX_INFO << deactivateSpam(30) << "Did not get frame until timeout of " << timeout
                    << " ms";
        return false;
    }
 
    auto rstimestamp = data.get_timestamp();
    IceUtil::Time timestamp;
    if (data.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME)
    {
        timestamp = IceUtil::Time::milliSeconds(rstimestamp);
    }
    else
    {
        timestamp = armarx::TimeUtil::GetTime();
        ARMARX_WARNING << deactivateSpam(999999999)
                       << "Timestamp of realsense is not global - measuring time myself";
    }
    ARMARX_DEBUG
        << deactivateSpam(1) << "frame age: "
        << (IceUtil::Time::now() - IceUtil::Time::milliSeconds(rstimestamp)).toMilliSeconds()
        << " ms";
 
 
    if (getProperty<bool>("ApplyAlignmentFilter").getValue())
    {
        //        TIMING_START(alignment);
        data = alignFilter->process(data);
        //        TIMING_END(alignment);
    }
 
    auto depthFrame = data.get_depth_frame();
 
    const bool applyDisparityFilter = getProperty<bool>("ApplyDisparityFilter").getValue();
    if (applyDisparityFilter)
    {
        //        TIMING_START(disparityFilter);
        depthFrame = depth_to_disparity_filter.process(depthFrame);
        //        TIMING_END(disparityFilter);
    }
 
 
    if (getProperty<bool>("ApplySpatialFilter").getValue())
    {
        //        TIMING_START(filter);
        depthFrame = spat_filter.process(depthFrame);
        //        TIMING_END(filter);
    }
    if (getProperty<bool>("ApplyTemporalFilter").getValue())
    {
        //        TIMING_START(tempFilter);
        depthFrame = temporal_filter.process(depthFrame);
        //        TIMING_END(tempFilter);
    }
 
    if (applyDisparityFilter)
    {
        //        TIMING_START(disparityFilterBack);
        depthFrame = disparity_to_depth_filter.process(depthFrame);
        //        TIMING_END(disparityFilterBack);
    }
 
    rs2::frame color = data.get_color_frame();
 
 
    if (!color)
    {
        color = data.get_infrared_frame();
    }
 
 
    const int cw = color.as<rs2::video_frame>().get_width();
    const int ch = color.as<rs2::video_frame>().get_height();
    auto colorBuffer = reinterpret_cast<const unsigned char*>(color.get_data());
    CByteImage rgbImage(cw, ch, CByteImage::eRGB24, true);
    rgbImage.pixels = const_cast<unsigned char*>(colorBuffer);
 
 
    int index = 0;
    int index2 = 0;
    auto resultDepthBuffer = depthImage->pixels;
    auto depthBuffer = reinterpret_cast<const unsigned short*>(depthFrame.get_data());
    const int dw = depthFrame.as<rs2::video_frame>().get_width();
    const int dh = depthFrame.as<rs2::video_frame>().get_height();
 
    for (int y = 0; y < dh; ++y)
    {
        for (int x = 0; x < dw; ++x)
        {
            int depthValue = 1000.f * depthBuffer[index2] * depthScale;
            visionx::tools::depthValueToRGB(depthValue,
                                            resultDepthBuffer[index],
                                            resultDepthBuffer[index + 1],
                                            resultDepthBuffer[index + 2]);
            index += 3;
            index2++;
        }
    }
 
    CByteImage* images[2] = {&rgbImage, depthImage.get()};
    provideImages(images, timestamp);
 
 
    // Tell pointcloud object to map to this color frame
    pc.map_to(color);
 
    // Generate the pointcloud and texture mappings
 
    points = pc.calculate(depthFrame);
 
 
    //convert to pcl cloud
    {
        ARMARX_CHECK_EXPRESSION(pointcloud);
        pointcloud->width = dw;
        pointcloud->height = dh;
        pointcloud->is_dense = false;
        pointcloud->points.resize(points.size());
        auto ptr = points.get_vertices();
        auto texPtr = points.get_texture_coordinates();
        ARMARX_CHECK_EXPRESSION(ptr);
        ARMARX_CHECK_EXPRESSION(texPtr);
        float maxDepth = getProperty<float>("MaxDepth").getValue() / 1000.f; // in m
        if (ptr && texPtr)
        {
            for (auto& p : pointcloud->points)
            {
                p.x = ptr->x * 1000.f; // m to mm
                p.y = ptr->y * 1000.f;
                p.z = ptr->z <= maxDepth ? ptr->z * 1000.f : std::nan("");
                if (texPtr)
                {
                    int x = texPtr->u * cw;
                    int y = texPtr->v * ch;
                    if (x >= 0 && x < cw && y >= 0 && y < ch)
                    {
                        auto colorIndex = int((x + y * cw) * 3);
                        p.r = colorBuffer[colorIndex];
                        p.g = colorBuffer[colorIndex + 1];
                        p.b = colorBuffer[colorIndex + 2];
                    }
                    else
                    {
                        p.r = p.g = p.b = 255;
                    }
                }
                ptr++;
                texPtr++;
            }
        }
    }
 
    pointcloud->header.stamp = timestamp.toMicroSeconds();
    providePointCloud(pointcloud);
    return true;
}
 
std::string
IntelRealSenseProvider::getDefaultName() const
{
    return "IntelRealSenseProvider";
}
 
armarx::PropertyDefinitionsPtr
IntelRealSenseProvider::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new IntelRealSenseProviderPropertyDefinitions(getConfigIdentifier()));
}
 
void
IntelRealSenseProvider::onInitComponent()
{
    ImageProvider::onInitComponent();
    CapturingPointCloudProvider::onInitComponent();
}
 
void
IntelRealSenseProvider::onConnectComponent()
{
 
    ImageProvider::onConnectComponent();
    CapturingPointCloudProvider::onConnectComponent();
}
 
void
IntelRealSenseProvider::onDisconnectComponent()
{
    captureEnabled = false;
 
    ImageProvider::onDisconnectComponent();
    CapturingPointCloudProvider::onDisconnectComponent();
}
 
void
IntelRealSenseProvider::onExitComponent()
{
    ImageProvider::onExitComponent();
    CapturingPointCloudProvider::onExitComponent();
}
 
visionx::StereoCalibration
IntelRealSenseProvider::getStereoCalibration(const Ice::Current&)
{
    return calibration;
}
 
bool
IntelRealSenseProvider::getImagesAreUndistorted(const Ice::Current& c)
{
    return false;
}
 
std::string
IntelRealSenseProvider::getReferenceFrame(const Ice::Current& c)
{
    return getProperty<std::string>("frameName");
}
 
StereoCalibration
IntelRealSenseProvider::createStereoCalibration(const rs2::pipeline_profile& selection) const
{
 
 
    auto getIntrinsics =
        [](visionx::CameraParameters& params, rs2::video_stream_profile& streamProfile)
    {
        //        auto resolution = std::make_pair(streamProfile.width(), streamProfile.height());
        auto i = streamProfile.get_intrinsics();
        //        auto principal_point = std::make_pair(i.ppx, i.ppy);
        //        auto focal_length = std::make_pair(i.fx, i.fy);
        //        rs2_distortion model = i.model;
 
 
        params.focalLength.resize(2, 0.0f);
        params.principalPoint.resize(2, 0.0f);
        params.distortion.resize(4, 0.0f);
 
        //Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
        params.rotation = visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        params.translation = visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
 
        params.principalPoint[0] = i.ppx;
        params.principalPoint[1] = i.ppy;
        params.focalLength[0] = i.fx;
        params.focalLength[1] = i.fy;
        params.width = streamProfile.width();
        params.height = streamProfile.height();
    };
    rs2::video_stream_profile rgb_stream =
        selection.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
    rs2::video_stream_profile depth_stream =
        selection.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
 
    visionx::CameraParameters rgbParams;
    visionx::CameraParameters depthParams;
 
    getIntrinsics(rgbParams, rgb_stream);
    getIntrinsics(depthParams, depth_stream);
 
    StereoCalibration calibration;
    {
        auto depth_stream = selection.get_stream(RS2_STREAM_DEPTH);
        auto color_stream = selection.get_stream(RS2_STREAM_COLOR);
        rs2_extrinsics e = depth_stream.get_extrinsics_to(color_stream);
        // Apply extrinsics to the origin
        float origin[3]{0.f, 0.f, 0.f};
        float target[3];
        rs2_transform_point_to_point(target, &e, origin);
 
 
        calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationLeft.cameraParam = rgbParams;
        calibration.calibrationRight.cameraParam = depthParams;
        calibration.rectificationHomographyLeft =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
        calibration.rectificationHomographyRight =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
        calibration.calibrationRight.cameraParam.translation = {target[0], target[1], target[2]};
    }
    return calibration;
}