IntelRealSense / librealsense

Intel® RealSense™ SDK
https://www.intelrealsense.com/
Apache License 2.0
7.49k stars 4.8k forks source link

How to convert cv::mat to rs::frame? #2634

Closed Liuyu010 closed 5 years ago

Liuyu010 commented 5 years ago
Required Info
Camera Model { D400 }
Firmware Version (05.09.02.0)
Operating System & Version {Win 10
Platform PC
SDK Version {2.13.0 }
Language {C++/opencv }
Segment VR/AR }

Issue Description

Hello,

I have already seen this question in the realsense community but didn't get the answer I want.

In rs_align, I have converted the rs2:: frame to cv:: Mat for using the joint bilateral filter provided by OpenCV to improve the quality of the depth map. The depth map of the processed mat type that is obtained cannot be used to call _remove_background(rs2::video_frame& other, const rs::frame & depth_frame, float depth_scale, float clippingdist).

So, I just want to know how to convert cv:: Mat to rs2:: frame. If you can help me, I would be very grateful.

Thank you!

dorodnic commented 5 years ago

Hi @Liuyu010 Frame is not exactly equivalent to cv::Mat, as it carries a lot of camera specific information (link to the sensor, metadata, link to calibration, etc...) That's why there is no easy cv-->frame API. It is possible, of course. I made a modified version of im-show example demonstrating how to go about it:

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>   // Include OpenCV API

int main(int argc, char * argv[]) try
{
    // Declare depth colorizer for pretty visualization of depth data
    rs2::colorizer color_map;

    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Start streaming with default recommended configuration
    pipe.start();

    using namespace cv;
    const auto window_name = "Display Image";
    namedWindow(window_name, WINDOW_AUTOSIZE);

    // Create custom processing block and its output queue:
    rs2::frame_queue q;
    rs2::processing_block pb(
        [](rs2::frame f, rs2::frame_source& src)
    {
        // For each input frame f, do:

        const int w = f.as<rs2::video_frame>().get_width();
        const int h = f.as<rs2::video_frame>().get_height();

        // frame --> cv
        Mat image(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
        // do some (silly) processing
        cv::GaussianBlur(image, image, cv::Size(0, 0), 3);

        // Allocate new frame. Copy all missing data from f.
        // This assumes the output is same resolution and format
        // if not true, need to specify parameters explicitly
        auto res = src.allocate_video_frame(f.get_profile(), f);

        // copy from cv --> frame
        memcpy((void*)res.get_data(), image.data, w * h * 2);

        // Send the resulting frame to the output queue
        src.frame_ready(res);
    });
    pb.start(q); // Bind output of the processing block to be enqueued into the queue

    while (waitKey(1) < 0 && cvGetWindowHandle(window_name))
    {
        rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
        rs2::frame depth = data.get_depth_frame();

        // Send depth frame for processing
        pb.invoke(depth);
        // Wait for results
        depth = q.wait_for_frame();

        // Continue with the new frame
        depth = depth.apply_filter(color_map);

        // Query frame size (width and height)
        const int w = depth.as<rs2::video_frame>().get_width();
        const int h = depth.as<rs2::video_frame>().get_height();

        // Create OpenCV matrix of size (w,h) from the colorized depth data
        Mat image(Size(w, h), CV_8UC3, (void*)depth.get_data(), Mat::AUTO_STEP);

        // Update the window with new data
        imshow(window_name, image);
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}
Liuyu010 commented 5 years ago

Hi,dorodnic Thank you for your reply! Your advice sure makes sense and I have tried to use it in my project. But there are still two problems that bother me.

  1. In the processing_block pb(), not only do I want to pass depth_map into it but also include color_map. When I tried to modify it, I was often reminded that "this function will not be counted as accepting two parameters".How to transfer two parameters to processing_block pb()?
  2. All my project are based on rs-align. So, I tried to use the method you provided in rs_align to process depth_map with OpenCV to see if it could be used as a way to improve background segmentation, but it doesn't work. Didi do something wrong?
    • Here is the wrong result.

image

Finally, I would like to ask you whether there is a good background segmentation program or a good idea on the D400s realsense camera. Although I have tried all the filters you provided in rs_align(like deci_filter、spat_filter、temp_filter、hole_filter etc.), the result is still a serious situation of edge leakage. Looking forward to your reply.

dorodnic commented 5 years ago

Hi @Liuyu010

To pass two or more frames, you need to bundle them in a frameset. See an example here: rs-measure.cpp:191-233

Third-Person-VR seem to be a private repository, I couldn't access the code.

In general, I think that good background removal would leverage both color and depth data. In rs-grabcuts I tried to feed depth data as initial guess to purely RGB based graph-cuts background removal. This can show decent results, but performance is abysmal. I wanted to try a hybrid approach for a while now, but couldn't make it work yet.

It is obvious that depth data helps in the problem of background removal, but in terms of depth sensing solutions, we are not yet in a place where you could just take the raw depth map and use it as a threshold. Main issue is that most depth cameras available today are composed from several sensors, each at slightly different location. This means that you will always going to get pixels that one sensor sees but the second does not, resulting in black pixels.

That said, I'm a software engineer, not a computer vision expert, so I'm sure there are people here who have better ideas.

P.S. I tried to fine-tune the example a bit for the D415 and came up with two suggestions:

  1. You can fairly easily enable basic post-processing to get less holes:
    
    rs2::temporal_filter temp;
    rs2::hole_filling_filter hfill;

while (app) // Application still alive? { // Using the align object, we block the application until a frameset is available rs2::frameset frameset = pipe.wait_for_frames().apply_filter(temp).apply_filter(hfill);

2. The D415 can deliver imperfect RGB from the left IR sensor (potentially reducing calibration artifacts)
You can try it and compare to the results from RGB sensor: 
```cpp
    rs2::pipeline pipe;

    rs2::config config;
    config.enable_stream(RS2_STREAM_DEPTH);
    config.enable_stream(RS2_STREAM_INFRARED, RS2_FORMAT_RGB8);
    rs2::pipeline_profile profile = pipe.start(config);

    auto sensor = profile.get_device().query_sensors().front();
    sensor.set_option(RS2_OPTION_VISUAL_PRESET, 6.0);
    sensor.set_option(RS2_OPTION_EXPOSURE, 50000.0);
    sensor.set_option(RS2_OPTION_LASER_POWER, sensor.get_option_range(RS2_OPTION_LASER_POWER).max);
RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010, If below answer have solved your question, we'd like to close this ticket. Thanks!

Liuyu010 commented 5 years ago

Hi,dorodnic Sorry for reply so late! I have tried what you advised last week, but it seems that I made the project more Chaotic and can't get the features I want(Maybe because of my unfamiliarity with developing on Intel RealSense). In rs_align, all the features I want is that I can use the color_map and depth_map fetched from D415/D435 to add the Joint Bilateral Filter on the depth_map(This is a method I think can improve the edge effect of the map very well). But I am still stuck in unknowing how to use the depth_image processed by OpenCV to call remove_background(). My current problem is the same as the two problems mentioned earlier. I have tried the pb() and changing the format of the input of the depth map of remove_background(), but all failed, Here is my code. Sincerely hope to get your help!

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp>
#include "window.hpp"
#include <imgui.h>
#include "imgui_impl_glfw.h"

#include <sstream>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstring>

#include <opencv2/opencv.hpp>
#include <ximgproc.hpp>

void render_slider(rect location, float& clipping_dist);
void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist);
float get_depth_scale(rs2::device dev);
rs2_stream find_stream_to_align(const std::vector<rs2::stream_profile>& streams);
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev);

int main(int argc, char * argv[]) try
{
    // Create and initialize GUI related objects
    window app(1280, 720, "CPP - Align Example"); // Simple window handling
    ImGui_ImplGlfw_Init(app, false);      // ImGui library intializition
    rs2::colorizer c;                          // Helper to colorize depth images
    texture renderer;                     // Helper for renderig images

    // Create a pipeline to easily configure and start the camera
    rs2::pipeline pipe;
    //Calling pipeline's start() without any additional parameters will start the first device
    // with its default streams.
    //The start function returns the pipeline profile which the pipeline used to start the device
    rs2::pipeline_profile profile = pipe.start();

    // Each depth camera might have different units for depth pixels, so we get it here
    // Using the pipeline's profile, we can retrieve the device that the pipeline uses
    float depth_scale = get_depth_scale(profile.get_device());

    //Pipeline could choose a device that does not have a color stream
    //If there is no color stream, choose to align depth to another stream
    rs2_stream align_to = find_stream_to_align(profile.get_streams());

    // Create a rs2::align object.
    // rs2::align allows us to perform alignment of depth frames to others frames
    //The "align_to" is the stream type to which we plan to align depth frames.
    rs2::align align(align_to);

    // Define a variable for controlling the distance to clip
    float depth_clipping_distance = 1.f;

    // Create custom processing block and its output queue:
    rs2::frame_queue q;
    rs2::processing_block pb(
        [](rs2::frame f, rs2::frame_source& src)
        //[](rs2::frame f, rs2::frame color,rs2::frame_source& src)
    {

        // For each input frame f, do:
        const int w = f.as<rs2::video_frame>().get_width();
        const int h = f.as<rs2::video_frame>().get_height();

        // frame --> cv
        ////Transform rs::color_frame to the 8UC3 type of Mat
        //cv::Mat color1(cv::Size(w, h), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP);
        //cv::imshow("Display image", color1);

        //Transform rs::depth_frame to the 8U3C type of Mat
        cv::Mat Mat_depth = cv::Mat(h, w, CV_16UC1, (void*)f.get_data());
        cv::GaussianBlur(Mat_depth, Mat_depth, cv::Size(0, 0), 3);
        //cvtColor(Mat_depth, Mat_depth, CV_BGR2GRAY);
        //imshow("Depth", Mat_depth);

        //// Apply Histogram Equalization
        //cv::equalizeHist(Mat_depth, Mat_depth);
        //cv::applyColorMap(Mat_depth, Mat_depth, cv::COLORMAP_JET);
        ////cv::imshow("Display depth", depth);

        //ADD the Joint Bilateral Filter...
        //cv::Mat dst, dst1;
        //cv::ximgproc::jointBilateralFilter(color, Mat_depth, dst, -1, 3, 11);
        ////cv::ximgproc::jointBilateralFilter(color, dst, dst1, -1, 3, 11);
        //cv::imshow("Display dst", dst);

        // Allocate new frame. Copy all missing data from f.
        // This assumes the output is same resolution and format
        // if not true, need to specify parameters explicitly
        auto res = src.allocate_video_frame(f.get_profile(), f);

        // copy from cv --> frame
        memcpy((void*)res.get_data(), Mat_depth.data, w * h * 2);

        // Send the resulting frame to the output queue
        src.frame_ready(res);
    });
    pb.start(q); // Bind output of the processing block to be enqueued into the queue

    while (app) // Application still alive?
    {
        // Using the align object, we block the application until a frameset is available
        rs2::frameset frameset = pipe.wait_for_frames();

        // rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
        // Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
        //  after the call to wait_for_frames();
        if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
        {
            //If the profile was changed, update the align object, and also get the new device's depth scale
            profile = pipe.get_active_profile();
            align_to = find_stream_to_align(profile.get_streams());
            align = rs2::align(align_to);
            depth_scale = get_depth_scale(profile.get_device());
        }

        //Get processed aligned frame
        auto processed = align.process(frameset);

        // Trying to get both other and aligned depth frames
        rs2::video_frame other_frame = processed.first(align_to);
        rs2::frame aligned_depth_frame = processed.get_depth_frame();

        pb.invoke(aligned_depth_frame);
        // Wait for results
        aligned_depth_frame = q.wait_for_frame();

        // Query frame size (width and height)
        const int width = aligned_depth_frame.as<rs2::video_frame>().get_width();
        const int height = aligned_depth_frame.as<rs2::video_frame>().get_height();
        rs2::colorizer color;
        rs2::frame colorDepth = aligned_depth_frame.apply_filter(color);
        // Create OpenCV matrix of size (w,h) from the colorized depth data
        cv::Mat image(cv::Size(width, height), CV_8UC3, (void*)colorDepth.get_data(), cv::Mat::AUTO_STEP);

        // Update the window with new data
        imshow("CVshow", image);

        //If one of them is unavailable, continue iteration
        if (!aligned_depth_frame || !other_frame)
        {
            continue;
        }
        // Passing both frames to remove_background so it will "strip" the background
        // NOTE: in this example, we alter the buffer of the other frame, instead of copying it and altering the copy
        //       This behavior is not recommended in real application since the other frame could be used elsewhere
        remove_background(other_frame, aligned_depth_frame, depth_scale, depth_clipping_distance);

        // Taking dimensions of the window for rendering purposes
        float w = static_cast<float>(app.width());
        float h = static_cast<float>(app.height());

        // At this point, "other_frame" is an altered frame, stripped form its background
        // Calculating the position to place the frame in the window
        rect altered_other_frame_rect{ 0, 0, w, h };
        altered_other_frame_rect = altered_other_frame_rect.adjust_ratio({ static_cast<float>(other_frame.get_width()),static_cast<float>(other_frame.get_height()) });

        // Render aligned image
        renderer.render(other_frame, altered_other_frame_rect);

        // The example also renders the depth frame, as a picture-in-picture
        // Calculating the position to place the depth frame in the window
        rect pip_stream{ 0, 0, w / 5, h / 5 };
        pip_stream = pip_stream.adjust_ratio({ static_cast<float>(width),static_cast<float>(height) });
        pip_stream.x = altered_other_frame_rect.x + altered_other_frame_rect.w - pip_stream.w - (std::max(w, h) / 25);
        pip_stream.y = altered_other_frame_rect.y + altered_other_frame_rect.h - pip_stream.h - (std::max(w, h) / 25);

        // Render depth (as picture in pipcture)
        renderer.upload(c.process(aligned_depth_frame));
        renderer.show(pip_stream);

        // Using ImGui library to provide a slide controller to select the depth clipping distance
        ImGui_ImplGlfw_NewFrame(1);
        render_slider({ 5.f, 0, w, h }, depth_clipping_distance);
        ImGui::Render();

    }
    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception & e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

float get_depth_scale(rs2::device dev)
{
    // Go over the device's sensors
    for (rs2::sensor& sensor : dev.query_sensors())
    {
        // Check if the sensor if a depth sensor
        if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
        {
            return dpt.get_depth_scale();
        }
    }
    throw std::runtime_error("Device does not have a depth sensor");
}

void render_slider(rect location, float& clipping_dist)
{
    // Some trickery to display the control nicely
    static const int flags = ImGuiWindowFlags_NoCollapse
        | ImGuiWindowFlags_NoScrollbar
        | ImGuiWindowFlags_NoSavedSettings
        | ImGuiWindowFlags_NoTitleBar
        | ImGuiWindowFlags_NoResize
        | ImGuiWindowFlags_NoMove;
    const int pixels_to_buttom_of_stream_text = 25;
    const float slider_window_width = 30;

    ImGui::SetNextWindowPos({ location.x, location.y + pixels_to_buttom_of_stream_text });
    ImGui::SetNextWindowSize({ slider_window_width + 20, location.h - (pixels_to_buttom_of_stream_text * 2) });

    //Render the vertical slider
    ImGui::Begin("slider", nullptr, flags);
    ImGui::PushStyleColor(ImGuiCol_FrameBg, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
    ImGui::PushStyleColor(ImGuiCol_SliderGrab, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
    ImGui::PushStyleColor(ImGuiCol_SliderGrabActive, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
    auto slider_size = ImVec2(slider_window_width / 2, location.h - (pixels_to_buttom_of_stream_text * 2) - 20);
    ImGui::VSliderFloat("", slider_size, &clipping_dist, 0.0f, 6.0f, "", 1.0f, true);
    if (ImGui::IsItemHovered())
        ImGui::SetTooltip("Depth Clipping Distance: %.3f", clipping_dist);
    ImGui::PopStyleColor(3);

    //Display bars next to slider
    float bars_dist = (slider_size.y / 6.0f);
    for (int i = 0; i <= 6; i++)
    {
        ImGui::SetCursorPos({ slider_size.x, i * bars_dist });
        std::string bar_text = "- " + std::to_string(6-i) + "m";
        ImGui::Text("%s", bar_text.c_str());
    }
    ImGui::End();
}

void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist)
{
    const uint16_t* p_depth_frame = reinterpret_cast<const uint16_t*>(depth_frame.get_data());
    uint8_t* p_other_frame = reinterpret_cast<uint8_t*>(const_cast<void*>(other_frame.get_data()));

    int width = other_frame.get_width();
    int height = other_frame.get_height();
    int other_bpp = other_frame.get_bytes_per_pixel();

#pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
    for (int y = 0; y < height; y++)
    {
        auto depth_pixel_index = y * width;
        for (int x = 0; x < width; x++, ++depth_pixel_index)
        {
            // Get the depth value of the current pixel
            auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index];

            // Check if the depth value is invalid (<=0) or greater than the threashold
            if (pixels_distance <= 0.f || pixels_distance > clipping_dist)
            {
                // Calculate the offset in other frame's buffer to current pixel
                auto offset = depth_pixel_index * other_bpp;

                // Set pixel to "background" color (0x999999)
                std::memset(&p_other_frame[offset], 0x99, other_bpp);
            }
        }
    }
}
rs2_stream find_stream_to_align(const std::vector<rs2::stream_profile>& streams)
{
    //Given a vector of streams, we try to find a depth stream and another stream to align depth with.
    //We prioritize color streams to make the view look better.
    //If color is not available, we take another stream that (other than depth)
    rs2_stream align_to = RS2_STREAM_ANY;
    bool depth_stream_found = false;
    bool color_stream_found = false;
    for (rs2::stream_profile sp : streams)
    {
        rs2_stream profile_stream = sp.stream_type();
        if (profile_stream != RS2_STREAM_DEPTH)
        {
            if (!color_stream_found)         //Prefer color
                align_to = profile_stream;

            if (profile_stream == RS2_STREAM_COLOR)
            {
                color_stream_found = true;
            }
        }
        else
        {
            depth_stream_found = true;
        }
    }

    if(!depth_stream_found)
        throw std::runtime_error("No Depth stream available");

    if (align_to == RS2_STREAM_ANY)
        throw std::runtime_error("No stream found to align with Depth");

    return align_to;
}

bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev)
{
    for (auto&& sp : prev)
    {
        //If previous profile is in current (maybe just added another)
        auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); });
        if (itr == std::end(current)) //If it previous stream wasn't found in current
        {
            return true;
        }
    }
    return false;
}
RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010 ,

from your posts, your question is "how to use the depth_image processed by OpenCV to call remove_background()", could you reference following modified rs-align.cpp(librealsense/examples/align/rs-align.cpp):

// License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2017 Intel Corporation. All Rights Reserved.

include <librealsense2/rs.hpp>

include "../example.hpp"

include

include "imgui_impl_glfw.h"

include

include

include

include

include

include <opencv2/opencv.hpp> // Include OpenCV API

using namespace cv;

void render_slider(rect location, float& clipping_dist); void remove_background(rs2::video_frame& other, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist); float get_depth_scale(rs2::device dev); rs2_stream find_stream_to_align(const std::vector& streams); bool profile_changed(const std::vector& current, const std::vector& prev);

int main(int argc, char * argv[]) try { // Create and initialize GUI related objects window app(1280, 720, "CPP - Align Example"); // Simple window handling ImGui_ImplGlfw_Init(app, false); // ImGui library intializition rs2::colorizer c; // Helper to colorize depth images texture renderer; // Helper for renderig images

// Create a pipeline to easily configure and start the camera
rs2::pipeline pipe;
//Calling pipeline's start() without any additional parameters will start the first device
// with its default streams.
//The start function returns the pipeline profile which the pipeline used to start the device
rs2::pipeline_profile profile = pipe.start();

// Each depth camera might have different units for depth pixels, so we get it here
// Using the pipeline's profile, we can retrieve the device that the pipeline uses
float depth_scale = get_depth_scale(profile.get_device());

//Pipeline could choose a device that does not have a color stream
//If there is no color stream, choose to align depth to another stream
rs2_stream align_to = find_stream_to_align(profile.get_streams());

// Create a rs2::align object.
// rs2::align allows us to perform alignment of depth frames to others frames
//The "align_to" is the stream type to which we plan to align depth frames.
rs2::align align(align_to);

// Define a variable for controlling the distance to clip
float depth_clipping_distance = 1.f;

// Create custom processing block and its output queue: rs2::frame_queue q; rs2::processing_block pb( [](rs2::frame f, rs2::frame_source& src) { // For each input frame f, do:

    const int w = f.as<rs2::video_frame>().get_width();
    const int h = f.as<rs2::video_frame>().get_height();

    // frame --> cv
    Mat image(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
    // do some (silly) processing
    cv::GaussianBlur(image, image, cv::Size(0, 0), 3);

    // Allocate new frame. Copy all missing data from f.
    // This assumes the output is same resolution and format
    // if not true, need to specify parameters explicitly
    auto res = src.allocate_video_frame(f.get_profile(), f);

    // copy from cv --> frame
    memcpy((void*)res.get_data(), image.data, w * h * 2);

    // Send the resulting frame to the output queue
    src.frame_ready(res);
});
pb.start(q); // Bind output of the processing block to be enqueued into the queue

while (app) // Application still alive?
{
    // Using the align object, we block the application until a frameset is available
    rs2::frameset frameset = pipe.wait_for_frames();

    // rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
    // Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
    //  after the call to wait_for_frames();
    if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
    {
        //If the profile was changed, update the align object, and also get the new device's depth scale
        profile = pipe.get_active_profile();
        align_to = find_stream_to_align(profile.get_streams());
        align = rs2::align(align_to);
        depth_scale = get_depth_scale(profile.get_device());
    }

    //Get processed aligned frame
    auto processed = align.process(frameset);

    // Trying to get both other and aligned depth frames
    rs2::video_frame other_frame = processed.first(align_to);
    rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();

    pb.invoke(aligned_depth_frame);
    aligned_depth_frame = q.wait_for_frame();

    //If one of them is unavailable, continue iteration
    if (!aligned_depth_frame || !other_frame)
    {
        continue;
    }
    // Passing both frames to remove_background so it will "strip" the background
    // NOTE: in this example, we alter the buffer of the other frame, instead of copying it and altering the copy
    //      This behavior is not recommended in real application since the other frame could be used elsewhere
    remove_background(other_frame, aligned_depth_frame, depth_scale, depth_clipping_distance);

    // Taking dimensions of the window for rendering purposes
    float w = static_cast<float>(app.width());
    float h = static_cast<float>(app.height());

    // At this point, "other_frame" is an altered frame, stripped form its background
    // Calculating the position to place the frame in the window
    rect altered_other_frame_rect{ 0, 0, w, h };
    altered_other_frame_rect = altered_other_frame_rect.adjust_ratio({ static_cast<float>(other_frame.get_width()),static_cast<float>(other_frame.get_height()) });

    // Render aligned image
    renderer.render(other_frame, altered_other_frame_rect);

    // The example also renders the depth frame, as a picture-in-picture
    // Calculating the position to place the depth frame in the window
    rect pip_stream{ 0, 0, w / 5, h / 5 };
    pip_stream = pip_stream.adjust_ratio({ static_cast<float>(aligned_depth_frame.get_width()),static_cast<float>(aligned_depth_frame.get_height()) });
    pip_stream.x = altered_other_frame_rect.x + altered_other_frame_rect.w - pip_stream.w - (std::max(w, h) / 25);
    pip_stream.y = altered_other_frame_rect.y + altered_other_frame_rect.h - pip_stream.h - (std::max(w, h) / 25);

    // Render depth (as picture in pipcture)
    renderer.upload(c.process(aligned_depth_frame));
    renderer.show(pip_stream);

    // Using ImGui library to provide a slide controller to select the depth clipping distance
    ImGui_ImplGlfw_NewFrame(1);
    render_slider({ 5.f, 0, w, h }, depth_clipping_distance);
    ImGui::Render();

}
return EXIT_SUCCESS;

} catch (const rs2::error & e) { std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl; return EXIT_FAILURE; } catch (const std::exception & e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; }

float get_depth_scale(rs2::device dev) { // Go over the device's sensors for (rs2::sensor& sensor : dev.query_sensors()) { // Check if the sensor if a depth sensor if (rs2::depth_sensor dpt = sensor.as()) { return dpt.get_depth_scale(); } } throw std::runtime_error("Device does not have a depth sensor"); }

void render_slider(rect location, float& clipping_dist) { // Some trickery to display the control nicely static const int flags = ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove; const int pixels_to_buttom_of_stream_text = 25; const float slider_window_width = 30;

ImGui::SetNextWindowPos({ location.x, location.y + pixels_to_buttom_of_stream_text });
ImGui::SetNextWindowSize({ slider_window_width + 20, location.h - (pixels_to_buttom_of_stream_text * 2) });

//Render the vertical slider
ImGui::Begin("slider", nullptr, flags);
ImGui::PushStyleColor(ImGuiCol_FrameBg, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
ImGui::PushStyleColor(ImGuiCol_SliderGrab, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
ImGui::PushStyleColor(ImGuiCol_SliderGrabActive, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
auto slider_size = ImVec2(slider_window_width / 2, location.h - (pixels_to_buttom_of_stream_text * 2) - 20);
ImGui::VSliderFloat("", slider_size, &clipping_dist, 0.0f, 6.0f, "", 1.0f, true);
if (ImGui::IsItemHovered())
    ImGui::SetTooltip("Depth Clipping Distance: %.3f", clipping_dist);
ImGui::PopStyleColor(3);

//Display bars next to slider
float bars_dist = (slider_size.y / 6.0f);
for (int i = 0; i <= 6; i++)
{
    ImGui::SetCursorPos({ slider_size.x, i * bars_dist });
    std::string bar_text = "- " + std::to_string(6-i) + "m";
    ImGui::Text("%s", bar_text.c_str());
}
ImGui::End();

}

void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist) { const uint16_t p_depth_frame = reinterpret_cast<const uint16_t>(depth_frame.get_data()); uint8_t p_other_frame = reinterpret_cast<uint8_t>(const_cast<void*>(other_frame.get_data()));

int width = other_frame.get_width();
int height = other_frame.get_height();
int other_bpp = other_frame.get_bytes_per_pixel();

#pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
for (int y = 0; y < height; y++)
{
    auto depth_pixel_index = y * width;
    for (int x = 0; x < width; x++, ++depth_pixel_index)
    {
        // Get the depth value of the current pixel
        auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index];

        // Check if the depth value is invalid (<=0) or greater than the threashold
        if (pixels_distance <= 0.f || pixels_distance > clipping_dist)
        {
            // Calculate the offset in other frame's buffer to current pixel
            auto offset = depth_pixel_index * other_bpp;

            // Set pixel to "background" color (0x999999)
            std::memset(&p_other_frame[offset], 0x99, other_bpp);
        }
    }
}

}

rs2_stream find_stream_to_align(const std::vector& streams) { //Given a vector of streams, we try to find a depth stream and another stream to align depth with. //We prioritize color streams to make the view look better. //If color is not available, we take another stream that (other than depth) rs2_stream align_to = RS2_STREAM_ANY; bool depth_stream_found = false; bool color_stream_found = false; for (rs2::stream_profile sp : streams) { rs2_stream profile_stream = sp.stream_type(); if (profile_stream != RS2_STREAM_DEPTH) { if (!color_stream_found) //Prefer color align_to = profile_stream;

        if (profile_stream == RS2_STREAM_COLOR)
        {
            color_stream_found = true;
        }
    }
    else
    {
        depth_stream_found = true;
    }
}

if(!depth_stream_found)
    throw std::runtime_error("No Depth stream available");

if (align_to == RS2_STREAM_ANY)
    throw std::runtime_error("No stream found to align with Depth");

return align_to;

}

bool profile_changed(const std::vector& current, const std::vector& prev) { for (auto&& sp : prev) { //If previous profile is in current (maybe just added another) auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); }); if (itr == std::end(current)) //If it previous stream wasn't found in current { return true; } } return false; }

the modified CMakeLists.txt(librealsense/examples/align/CMakeLists.txt) as following:

minimum required cmake version: 3.1.0

cmake_minimum_required(VERSION 3.1.0)

project(RealsenseExamplesAlign)

Save the command line compile commands in the build output

set(CMAKE_EXPORT_COMPILE_COMMANDS 1)

find_package(OpenCV REQUIRED) message(STATUS "OpenCV library status:") message(STATUS " version: ${OpenCV_VERSION}") message(STATUS " libraries: ${OpenCV_LIBS}") message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}")

include_directories(${OpenCV_INCLUDE_DIRS})

include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") endif()

if(BUILD_GRAPHICAL_EXAMPLES)

align

add_executable(rs-align rs-align.cpp ../../third-party/imgui/imgui.cpp ../../third-party/imgui/imgui_draw.cpp ../../third-party/imgui/imgui_impl_glfw.cpp)
#    target_link_libraries(rs-align ${DEPENDENCIES})
target_link_libraries(rs-align ${DEPENDENCIES} ${OpenCV_LIBS})
include_directories(rs-align ../../common ../../third-party/imgui)
set_target_properties (rs-align PROPERTIES
    FOLDER Examples
)

install(
    TARGETS

    rs-align

    RUNTIME DESTINATION
    ${CMAKE_INSTALL_PREFIX}/bin
)

endif()

we'd like to hear from you later.

Liuyu010 commented 5 years ago

Thank you for your patience. I have tried what you suggested, but the output window is still dark.

image

After a step-by-step debugging, the preliminary judgment is aligned_depth_frame = q.wait_for_frame() cannot assign depth_frame from the queue to aligned_depth_frame.

Have you encountered any similar problems here? Looking forward to your reply.

Solved this problem. After commenting out this line, it will display normally.

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010,

we need to dig more into this issue. Thanks for your investigation and patience.

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010 ,

hope the following new rs-align.cpp meet your requirement:

// License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2017 Intel Corporation. All Rights Reserved.

include <librealsense2/rs.hpp>

include "../example.hpp"

include

include "imgui_impl_glfw.h"

include

include

include

include

include

include <opencv2/opencv.hpp> // Include OpenCV API

using namespace cv;

void render_slider(rect location, float& clipping_dist); void remove_background(rs2::video_frame& other, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist); float get_depth_scale(rs2::device dev); rs2_stream find_stream_to_align(const std::vector& streams); bool profile_changed(const std::vector& current, const std::vector& prev);

int main(int argc, char * argv[]) try { // Create and initialize GUI related objects window app(1280, 720, "CPP - Align Example"); // Simple window handling ImGui_ImplGlfw_Init(app, false); // ImGui library intializition rs2::colorizer c; // Helper to colorize depth images texture renderer; // Helper for renderig images

// Create a pipeline to easily configure and start the camera
rs2::pipeline pipe;
//Calling pipeline's start() without any additional parameters will start the first device
// with its default streams.
//The start function returns the pipeline profile which the pipeline used to start the device
rs2::pipeline_profile profile = pipe.start();

// Each depth camera might have different units for depth pixels, so we get it here
// Using the pipeline's profile, we can retrieve the device that the pipeline uses
float depth_scale = get_depth_scale(profile.get_device());

//Pipeline could choose a device that does not have a color stream
//If there is no color stream, choose to align depth to another stream
rs2_stream align_to = find_stream_to_align(profile.get_streams());

// Create a rs2::align object.
// rs2::align allows us to perform alignment of depth frames to others frames
//The "align_to" is the stream type to which we plan to align depth frames.
rs2::align align(align_to);

// Define a variable for controlling the distance to clip
float depth_clipping_distance = 1.f;

// Create custom processing block and its output queue: rs2::frame_queue q; rs2::processing_block pb( [&](rs2::frameset data, rs2::frame_source& src) {

    rs2::frame f = data.get_depth_frame();
    // For each input frame f, do:

    const int w = f.as<rs2::video_frame>().get_width();
    const int h = f.as<rs2::video_frame>().get_height();

    // frame --> cv
    Mat image(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
    // do some (silly) processing
    cv::GaussianBlur(image, image, cv::Size(0, 0), 3);

    // Allocate new frame. Copy all missing data from f.
    // This assumes the output is same resolution and format
    // if not true, need to specify parameters explicitly
    auto res = src.allocate_video_frame(f.get_profile(), f);

    // copy from cv --> frame
    memcpy((void*)res.get_data(), image.data, w * h * 2);

    std::cout << "enqueue" << std::endl;
    q.enqueue(f);

    // Send the resulting frame to the output queue
    src.frame_ready(res);
});
//pb.start(q); // Bind output of the processing block to be enqueued into the queue

//pb >> q;

while (app) // Application still alive?
{
    // Using the align object, we block the application until a frameset is available
    rs2::frameset frameset = pipe.wait_for_frames();

    // rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
    // Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
    //  after the call to wait_for_frames();
    if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
    {
        //If the profile was changed, update the align object, and also get the new device's depth scale
        profile = pipe.get_active_profile();
        align_to = find_stream_to_align(profile.get_streams());
        align = rs2::align(align_to);
        depth_scale = get_depth_scale(profile.get_device());
    }

    //Get processed aligned frame
    auto processed = align.process(frameset);

    // Trying to get both other and aligned depth frames
    rs2::video_frame other_frame = processed.first(align_to);
    //rs2::depth_frame aligned_depth_frame = processed.get_depth_frame();
    //rs2::depth_frame aligned_depth_frame;// = processed.get_depth_frame();

    pb.invoke(processed);

    rs2::frame depth = q.wait_for_frame();
    rs2::depth_frame aligned_depth_frame = depth.as<rs2::video_frame>();

    //If one of them is unavailable, continue iteration
    if (!aligned_depth_frame || !other_frame)
    {
        continue;
    }
    // Passing both frames to remove_background so it will "strip" the background
    // NOTE: in this example, we alter the buffer of the other frame, instead of copying it and altering the copy
    //      This behavior is not recommended in real application since the other frame could be used elsewhere
    remove_background(other_frame, aligned_depth_frame, depth_scale, depth_clipping_distance);

    // Taking dimensions of the window for rendering purposes
    float w = static_cast<float>(app.width());
    float h = static_cast<float>(app.height());

    // At this point, "other_frame" is an altered frame, stripped form its background
    // Calculating the position to place the frame in the window
    rect altered_other_frame_rect{ 0, 0, w, h };
    altered_other_frame_rect = altered_other_frame_rect.adjust_ratio({ static_cast<float>(other_frame.get_width()),static_cast<float>(other_frame.get_height()) });

    // Render aligned image
    renderer.render(other_frame, altered_other_frame_rect);

    // The example also renders the depth frame, as a picture-in-picture
    // Calculating the position to place the depth frame in the window
    rect pip_stream{ 0, 0, w / 5, h / 5 };
    pip_stream = pip_stream.adjust_ratio({ static_cast<float>(aligned_depth_frame.get_width()),static_cast<float>(aligned_depth_frame.get_height()) });
    pip_stream.x = altered_other_frame_rect.x + altered_other_frame_rect.w - pip_stream.w - (std::max(w, h) / 25);
    pip_stream.y = altered_other_frame_rect.y + altered_other_frame_rect.h - pip_stream.h - (std::max(w, h) / 25);

    // Render depth (as picture in pipcture)
    renderer.upload(c.process(aligned_depth_frame));
    renderer.show(pip_stream);

    // Using ImGui library to provide a slide controller to select the depth clipping distance
    ImGui_ImplGlfw_NewFrame(1);
    render_slider({ 5.f, 0, w, h }, depth_clipping_distance);
    ImGui::Render();

}
return EXIT_SUCCESS;

} catch (const rs2::error & e) { std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl; return EXIT_FAILURE; } catch (const std::exception & e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; }

float get_depth_scale(rs2::device dev) { // Go over the device's sensors for (rs2::sensor& sensor : dev.query_sensors()) { // Check if the sensor if a depth sensor if (rs2::depth_sensor dpt = sensor.as()) { return dpt.get_depth_scale(); } } throw std::runtime_error("Device does not have a depth sensor"); }

void render_slider(rect location, float& clipping_dist) { // Some trickery to display the control nicely static const int flags = ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove; const int pixels_to_buttom_of_stream_text = 25; const float slider_window_width = 30;

ImGui::SetNextWindowPos({ location.x, location.y + pixels_to_buttom_of_stream_text });
ImGui::SetNextWindowSize({ slider_window_width + 20, location.h - (pixels_to_buttom_of_stream_text * 2) });

//Render the vertical slider
ImGui::Begin("slider", nullptr, flags);
ImGui::PushStyleColor(ImGuiCol_FrameBg, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
ImGui::PushStyleColor(ImGuiCol_SliderGrab, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
ImGui::PushStyleColor(ImGuiCol_SliderGrabActive, ImColor(215.f / 255, 215.0f / 255, 215.0f / 255));
auto slider_size = ImVec2(slider_window_width / 2, location.h - (pixels_to_buttom_of_stream_text * 2) - 20);
ImGui::VSliderFloat("", slider_size, &clipping_dist, 0.0f, 6.0f, "", 1.0f, true);
if (ImGui::IsItemHovered())
    ImGui::SetTooltip("Depth Clipping Distance: %.3f", clipping_dist);
ImGui::PopStyleColor(3);

//Display bars next to slider
float bars_dist = (slider_size.y / 6.0f);
for (int i = 0; i <= 6; i++)
{
    ImGui::SetCursorPos({ slider_size.x, i * bars_dist });
    std::string bar_text = "- " + std::to_string(6-i) + "m";
    ImGui::Text("%s", bar_text.c_str());
}
ImGui::End();

}

void remove_background(rs2::video_frame& other_frame, const rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist) { const uint16_t p_depth_frame = reinterpret_cast<const uint16_t>(depth_frame.get_data()); uint8_t p_other_frame = reinterpret_cast<uint8_t>(const_cast<void*>(other_frame.get_data()));

int width = other_frame.get_width();
int height = other_frame.get_height();
int other_bpp = other_frame.get_bytes_per_pixel();

#pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
for (int y = 0; y < height; y++)
{
    auto depth_pixel_index = y * width;
    for (int x = 0; x < width; x++, ++depth_pixel_index)
    {
        // Get the depth value of the current pixel
        auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index];

        // Check if the depth value is invalid (<=0) or greater than the threashold
        if (pixels_distance <= 0.f || pixels_distance > clipping_dist)
        {
            // Calculate the offset in other frame's buffer to current pixel
            auto offset = depth_pixel_index * other_bpp;

            // Set pixel to "background" color (0x999999)
            std::memset(&p_other_frame[offset], 0x99, other_bpp);
        }
    }
}

}

rs2_stream find_stream_to_align(const std::vector& streams) { //Given a vector of streams, we try to find a depth stream and another stream to align depth with. //We prioritize color streams to make the view look better. //If color is not available, we take another stream that (other than depth) rs2_stream align_to = RS2_STREAM_ANY; bool depth_stream_found = false; bool color_stream_found = false; for (rs2::stream_profile sp : streams) { rs2_stream profile_stream = sp.stream_type(); if (profile_stream != RS2_STREAM_DEPTH) { if (!color_stream_found) //Prefer color align_to = profile_stream;

        if (profile_stream == RS2_STREAM_COLOR)
        {
            color_stream_found = true;
        }
    }
    else
    {
        depth_stream_found = true;
    }
}

if(!depth_stream_found)
    throw std::runtime_error("No Depth stream available");

if (align_to == RS2_STREAM_ANY)
    throw std::runtime_error("No stream found to align with Depth");

return align_to;

}

bool profile_changed(const std::vector& current, const std::vector& prev) { for (auto&& sp : prev) { //If previous profile is in current (maybe just added another) auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); }); if (itr == std::end(current)) //If it previous stream wasn't found in current { return true; } } return false; }

and our expert @dorodnic had been pointed out that you could pass frameset(contain color_frame and depth_frame and ir_frame...) to the pb.

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010,

we had been validated the latest modified rs-align.cpp, and it works find as expected.

we'd like to close this ticket if it could solved your question.

we'd like to hear from you again.

Thanks!

Best regards

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] @Liuyu010 Did you get any chance to try what we suggested to get this through? Please update. Thanks!

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] Hi @Liuyu010,

any progress with this issue? we'd like to get a positive response from you.

Thanks.

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] @Liuyu010 Sorry I'm not very clear about what you mentioned about "the map seems to be very confusing". Could you please explain more about it?

Liuyu010 commented 5 years ago

Ummm...it means that the depth map's precision does not support the background removal very well. We are looking for other possible solutions. Thank you for your sincere help.

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] @liuyu010 Thanks for the feedback! We have one such example in the link https://github.com/IntelRealSense/librealsense/tree/master/wrappers/opencv/grabcuts You can have a try. Thanks!

RealSense-Customer-Engineering commented 5 years ago

[Realsense Customer Engineering Team Comment] @liuyu010 Thanks for your using Realsense camera! Close this ticket. If you still have other questions, please feel free to open new ticket.