Closed Liuyu010 closed 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;
}
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.
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.
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:
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 Team Comment] Hi @Liuyu010, If below answer have solved your question, we'd like to close this ticket. Thanks!
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 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.
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
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
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
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
the modified CMakeLists.txt(librealsense/examples/align/CMakeLists.txt) as following:
cmake_minimum_required(VERSION 3.1.0)
project(RealsenseExamplesAlign)
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)
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.
Thank you for your patience. I have tried what you suggested, but the output window is still dark.
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 Team Comment] Hi @Liuyu010,
we need to dig more into this issue. Thanks for your investigation and patience.
[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.
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
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
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
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
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 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 Team Comment] @Liuyu010 Did you get any chance to try what we suggested to get this through? Please update. Thanks!
[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 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?
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 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 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.
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!