Open MingyuPan2 opened 1 month ago
Hi @MingyuPan2 Instead of multiplying the 16-bit pixel depth value by the camera's depth scale of 0.001, do you get more stable results if you change the line to use the alternative method of the get_distance() instruction instead? This instruction outputs the depth value in real-world meters without needing to multiply by the depth scale. For example:
distance = depth.get_distance(x,y);
Also, your code suggests that the full script is using post-processing filters such as hole-filling and spatial. If your script is using these filters, the .process lines of these filters that apply them should ideally be placed before the align.process instruction, as it is recommended that alignment is applied after post-processing.
Hi @MartyG-RealSense, thanks for the response! Yes, i do get stable results with get_distance(). The filter also don't seem to interfere with the results, since if i turn off all the filters, i still get random distance outputs when the depth stream is live. I'll try to implement the get_depth method in the onMouse function to see if i can get a stable result.
Here's my entire code, if it helps at all :/
#include <opencv2/highgui/highgui.hpp>
#include <librealsense2/rs.hpp>
#include <iostream>
#include <chrono>
using namespace cv;
using namespace rs2;
Mat latest_ir_frame, latest_rgb_frame, latest_depth_frame, combined_window; //Variables
std::string output_text = " ", latency_text_output = " ";
int button_width = 150, button_height = 40, button_x_pos = 10, button_y_pos = 530, frame_count = 0, fps = 0;
const Scalar button_on_color = Scalar(0, 255, 0), button_off_color = Scalar(0, 0, 255), button_text_color = Scalar(0, 0, 0);
bool depth_stream_enabled = true, hole_filling_enabled = false, spatial_enabled = false;
// Convert depth frame to an 8-bit image for display. TF..
Mat depth_frame_to_displayable(const Mat& depth_frame)
{
Mat depth_display;
double min, max;
minMaxLoc(depth_frame, &min, &max);
depth_frame.convertTo(depth_display, CV_8UC1, 255.0 / max);
applyColorMap(depth_display, depth_display, COLORMAP_JET);
return depth_display;
}
void drawButton(Mat& window, bool depth_on, bool hole_fill_on, bool spatial_on)
{
// Draw Depth button
Scalar depth_button_color = depth_on ? button_on_color : button_off_color;
rectangle(window, Point(button_x_pos, button_y_pos), Point(button_x_pos + button_width, button_y_pos + button_height), depth_button_color, FILLED);
putText(window, depth_on ? "Depth ON" : "Depth OFF", Point(button_x_pos + 20, button_y_pos + button_height / 2 + 10), FONT_HERSHEY_SIMPLEX, 0.7, button_text_color, 2);
// Hole Filling button
Scalar hole_fill_button_color = hole_fill_on ? button_on_color : button_off_color;
int hole_button_x_pos = button_x_pos + button_width + 20; // New button position
rectangle(window, Point(hole_button_x_pos, button_y_pos), Point(hole_button_x_pos + button_width, button_y_pos + button_height), hole_fill_button_color, FILLED);
putText(window, hole_fill_on ? "Hole Fill ON" : "Hole Fill OFF", Point(hole_button_x_pos + 5, button_y_pos + button_height / 2 + 10), FONT_HERSHEY_SIMPLEX, 0.7, button_text_color, 2);
//Spatial button
Scalar spatial_button_color = spatial_on ? button_on_color : button_off_color;
int spatial_button_x_pos = button_x_pos + button_width + 200; // New button position
rectangle(window, Point(spatial_button_x_pos, button_y_pos), Point(spatial_button_x_pos + button_width, button_y_pos + button_height), spatial_button_color, FILLED);
putText(window, spatial_on ? "Spatial ON" : "Spatial OFF", Point(spatial_button_x_pos + 10, button_y_pos + button_height / 2 + 10), FONT_HERSHEY_SIMPLEX, 0.7, button_text_color, 2);
}
void onMouse(int event, int x, int y, int flags, void* ustg)
{
// Mouse left click events
if (event == EVENT_LBUTTONDOWN)
{
if (x >= button_x_pos && x <= button_x_pos + button_width && y >= button_y_pos && y <= button_y_pos + button_height) //check if mouse cursor is on button
{
depth_stream_enabled = !depth_stream_enabled; // Toggle depth stream
}
else if (x >= button_x_pos + button_width + 20 && x <= button_x_pos + 2 * button_width + 20 && y >= button_y_pos && y <= button_y_pos + button_height)
{
hole_filling_enabled = !hole_filling_enabled; // Toggle hole filling filter
}
else if (x >= button_x_pos + button_width + 190 && x <= button_x_pos + 2 * button_width + 190 && y >= button_y_pos && y <= button_y_pos + button_height)
{
spatial_enabled = ! spatial_enabled; // Toggle hole filling filter
}
else if (x >= latest_rgb_frame.cols && y >= latest_ir_frame.rows && y < latest_ir_frame.rows + latest_depth_frame.rows)
{
int depth_x = x - latest_rgb_frame.cols, depth_y = y - latest_ir_frame.rows;
if (depth_x >= 0 && depth_x < latest_depth_frame.cols && depth_y >= 0 && depth_y < latest_depth_frame.rows)
{
ushort depth_value = latest_depth_frame.at<ushort>(depth_y, depth_x);
float distance = depth_value * 0.001f;
std::cout << "Last clicked Depth stream at (" << depth_x << ", " << depth_y << "), Distance: " << distance * 100 << " centimeters" << std::endl;
}
}
return;
}
// Mouse move events
if (event == EVENT_MOUSEMOVE)
{
if (x < latest_rgb_frame.cols && y < latest_rgb_frame.rows)
{
output_text = "Mouse over RGB image at (" + std::to_string(x) + ", " + std::to_string(y) + ")";
}
else if (x >= latest_rgb_frame.cols && y < latest_ir_frame.rows)
{
int ir_x = x - latest_rgb_frame.cols;
if (ir_x >= 0 && ir_x < latest_ir_frame.cols)
{
int pixel_value = latest_ir_frame.at<uchar>(y, ir_x);
output_text = "Mouse over IR image at (" + std::to_string(ir_x) + ", " + std::to_string(y) + "), Pixel value: " + std::to_string(pixel_value);
}
}
else if (x >= latest_rgb_frame.cols && y >= latest_ir_frame.rows && y < latest_ir_frame.rows + latest_depth_frame.rows)
{
int depth_x = x - latest_rgb_frame.cols, depth_y = y - latest_ir_frame.rows;
if (depth_x >= 0 && depth_x < latest_depth_frame.cols && depth_y >= 0 && depth_y < latest_depth_frame.rows)
{
ushort depth_value = latest_depth_frame.at<ushort>(depth_y, depth_x);
float distance = depth_value * 0.001f;
output_text = "Mouse over Depth image at (" + std::to_string(depth_x) + ", " + std::to_string(depth_y) + "), Distance: " + std::to_string(distance * 100) + "cm";
}
}
}
}
auto fps_last_time = std::chrono::steady_clock::now();
void calculateFPS()
{
frame_count++;
auto fps_current_time = std::chrono::steady_clock::now();
double elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(fps_current_time - fps_last_time).count();
if (elapsed_time >= 250.0)
{
fps = frame_count / (elapsed_time / 1000.0);
fps_last_time = fps_current_time;
frame_count = 0;
}
}
int main()
{
try
{
// Streaming initialization
rs2::pipeline pipe;
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_INFRARED, 848, 480, RS2_FORMAT_Y8, 60);
cfg.enable_stream(RS2_STREAM_COLOR, 848, 480, RS2_FORMAT_RGB8, 60);
cfg.enable_stream(RS2_STREAM_DEPTH, 848, 480, RS2_FORMAT_Z16, 60);
std::cout << "Starting pipeline..." << std::endl;
// Set emitter power to MAX based on https://dev.intelrealsense.com/docs/api-how-to. Simplify it?
rs2::pipeline_profile selection = pipe.start(cfg);
rs2::device selected_device = selection.get_device();
auto depth_sensor = selected_device.first<rs2::depth_sensor>();
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max);
}
rs2::align align_to_color(RS2_STREAM_COLOR);
namedWindow("D435i Output", WINDOW_AUTOSIZE);
setMouseCallback("D435i Output", onMouse);
// Filters
rs2::decimation_filter dec_filter;
rs2::spatial_filter spa_filter;
rs2::temporal_filter temp_filter;
rs2::threshold_filter thre_filter;
rs2::sequence_id_filter seq_filter;
rs2::hole_filling_filter hole_filter;
// Filter params
dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 1); // Higher = lower resolution
spa_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2); // 30fps if enabled
spa_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.5);
spa_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20);
spa_filter.set_option(RS2_OPTION_HOLES_FILL, 2);
temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.4); // Best for static scenes
temp_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20);
temp_filter.set_option(RS2_OPTION_HOLES_FILL, 3);
thre_filter.set_option(RS2_OPTION_MIN_DISTANCE, 0); // Max min distance
thre_filter.set_option(RS2_OPTION_MAX_DISTANCE, 4);
hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1);
while (true)
{
try
{
auto processing_start_time = std::chrono::high_resolution_clock::now();
auto data = pipe.wait_for_frames();
auto ir_frame = data.get_infrared_frame();
auto rgb_frame = data.get_color_frame();
auto filtered = data.get_depth_frame();
// Filter implementation
auto depth_frame = filtered;
depth_frame = dec_filter.process(depth_frame);
if (spatial_enabled)
{
depth_frame = spa_filter.process(depth_frame);
}
depth_frame = temp_filter.process(depth_frame);
depth_frame = thre_filter.process(depth_frame);
if (hole_filling_enabled)
{
depth_frame = hole_filter.process(depth_frame);
}
// Error check
if (!ir_frame || !rgb_frame || !depth_frame)
{
std::cerr << "One or more frames are not available!" << std::endl;
continue;
}
latest_ir_frame = Mat(Size(ir_frame.get_width(), ir_frame.get_height()), CV_8UC1, (void*)ir_frame.get_data(), Mat::AUTO_STEP);
latest_rgb_frame = Mat(Size(rgb_frame.get_width(), rgb_frame.get_height()), CV_8UC3, (void*)rgb_frame.get_data(), Mat::AUTO_STEP);
latest_depth_frame = Mat(Size(depth_frame.get_width(), depth_frame.get_height()), CV_16UC1, (void*)depth_frame.get_data(), Mat::AUTO_STEP);
cvtColor(latest_rgb_frame, latest_rgb_frame, COLOR_BGR2RGB);
cvtColor(latest_ir_frame, latest_ir_frame, COLOR_GRAY2BGR);
if (depth_stream_enabled)
{
latest_depth_frame = depth_frame_to_displayable(latest_depth_frame);
}
if (latest_ir_frame.empty() || latest_rgb_frame.empty() || (depth_stream_enabled && latest_depth_frame.empty()))
{
std::cerr << "Failed to convert frames to Mat!" << std::endl;
continue;
}
// float width = filtered.get_width();
// float height = filtered.get_height();
// float dist_to_center = filtered.get_distance(width/2, height/2);
// std::cout << "Camera to object: " << dist_to_center << "m \r";
int combined_window_height = std::max(latest_rgb_frame.rows, latest_ir_frame.rows) + (depth_stream_enabled ? latest_depth_frame.rows : 0) + 50;
int combined_window_width = latest_rgb_frame.cols + latest_ir_frame.cols;
if (combined_window.empty() || combined_window.cols < combined_window_width || combined_window.rows < combined_window_height)
{
combined_window = Mat::zeros(combined_window_height, combined_window_width, CV_8UC3);
}
latest_rgb_frame.copyTo(combined_window(Rect(0, 0, latest_rgb_frame.cols, latest_rgb_frame.rows)));
latest_ir_frame.copyTo(combined_window(Rect(latest_rgb_frame.cols, 0, latest_ir_frame.cols, latest_ir_frame.rows)));
if (depth_stream_enabled)
{
latest_depth_frame.copyTo(combined_window(Rect(latest_rgb_frame.cols, latest_ir_frame.rows, latest_depth_frame.cols, latest_depth_frame.rows)));
}
drawButton(combined_window, depth_stream_enabled, hole_filling_enabled, spatial_enabled);
int text_area_y_position = combined_window_height - 100, text_area_width = 848;
Rect text_area(0, text_area_y_position, text_area_width, 50);
Mat text_window = combined_window(text_area);
text_window.setTo(Scalar(0));
putText(text_window, output_text, Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255,255,255), 2);
calculateFPS();
auto processing_end_time = std::chrono::high_resolution_clock::now();
long long total_processing_latency = std::chrono::duration_cast<std::chrono::milliseconds>(processing_end_time - processing_start_time).count();
int text_latency_y_position = combined_window_height - 50, text_latency_width = 848;
Rect text_latency(0, text_latency_y_position, text_latency_width, 50);
Mat text_window2 = combined_window(text_latency);
text_window2.setTo(Scalar(0));
latency_text_output = "FPS: " + std::to_string(fps) + ", Software Processing Latency: " + std::to_string(total_processing_latency) + "ms";
putText(text_window2, latency_text_output, Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255,255,255), 2);
imshow("D435i Output", combined_window);
if (waitKey(1) >= 0)
{
break;
}
}
catch (const rs2::error& e)
{
std::cerr << "RealSense error: " << e.what() << std::endl;
break;
}
catch (const std::exception& e)
{
std::cerr << "Standard error: " << e.what() << std::endl;
break;
}
}
}
catch (const rs2::error& e)
{
std::cerr << "RealSense error: " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception& e)
{
std::cerr << "Standard error: " << e.what() << std::endl;
return EXIT_FAILURE;
}
return 0;
}
If you are using a USB mouse then you could test whether the EVENT_MOUSEMOVE code is making depth_x and depth_y fluctuate when the depth stream is live by unplugging the mouse.
If the mouse is a wireless one then the infrared of RealSense cameras can cause interference with the wireless mouse signal.
I only use a wired USB mouse. Unplugging the mouse does not fix the problem when the stream is live. I also tried getting the output by clicking the live stream before and the output is interesting as seen below:
I clicked on different areas of the the stream and some of the output is pretty similar (the actual distance is around 240cm). Also, i noticed that the live depth stream flashes a lot.I can't seem to record my screen so i included two screenshots:
These screenshots were take probably a quarter of a second apart, and the stream looks completely different even though i set the threshold filter distance to 0 - 4m. The stream actually constantly flash much faster (like every 10ms) and the flashing is worse when i disable all the filters. Again, I don't understand why this is happening since when i visualized the depth stream in RVIZ for ROS, the depth stream also had this exact same flashing problem.
I'll try to investigate further and any help you give is greatly appreciated!!
The lower multi-colored image looks as though it is the correct one of the two, because the depth colorization correctly shifts between blue in the foreground near the camera and red in the far background. In the upper all-blue image it is as though all coordinates have approximately the same depth no matter how far from the camera they are.
Could you try disabling auto exposure by setting it to '0' to see whether rapidly changing exposure values could be affecting your depth? Code for doing so can be found at https://github.com/IntelRealSense/librealsense/issues/10239
depth_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
Disabling the auto exposure still does not solve the problem. I can still see visable flashing and it actually introduces an area with no depth value at the center of the depth stream as seen below for some reason.... I changed around the other EXPOSURE settings but it doesn't seem to affect the image.
Interestingly, if i freeze the depth stream, the image it produces will give an accurate depth value, no matter if the depth colorization is correct or not. So currently, i can only get accurate depth value from a still image, but not the live stream.
Is depth more stable if you change the value of the temporal filter's RS2_OPTION_FILTER_SMOOTH_ALPHA setting from 0.4 to 0.1 so that the depth image still updates live but updates less frequently?
Yes, I tried changing all the filter parameters but it is still the same.
Screencast from 09-18-2024 10:35:42 AM.webm
Here's what i see on my screen. Sorry for the laggy video. Screen recording was not smooth at all while the camera was in use. However , you can still see the depth stream flash clearly, either when i move the camera slightly or keep the camera still.
Thank you very much for the video.
Do you have access to the realsense-viewer tool to test whether the flashing also occurs there when depth is streaming in real-time?
There's no flashing in the realsense-viewer tool. The depth stream output is very stable and the distance it gives is accurate too. I've also seen the same flashing problem when visualizing the depth stream in RVIZ with ROS2 iron. Are there any filters that was not shown in the control panel that might've made the depth stream more stable? Thank you!!!
All post-processing filters that are enabled are shown in the Post-Processing section. There are not hidden ones.
The realsense-viewer does also apply a range of default depth colorization settings in the Depth Visualization category of the side-panel.
It may also be worth trying to load one of the realsense-viewer's built-in Visual Presets into your C++ script to apply a range of configuration settings automatically.
I loaded the presets. There's still flashing when the camera points at a specific distance, but the flashing was reduced abit in my opinion. The output distance is still wrong, basically the same as before.
How does the ROS2 Iron depth image perform if you enable filters in the ros2 launch instruction?
ros2 launch realsense2_camera rs_launch.py temporal_filter.enable:=true spatial_filter.enable:=true
Hi @MingyuPan2 Do you have an update about this case that you can provide, please? Thanks!
I used the above method in ROS2 but the problem is still there. I am also currently working on other projects, so I may put this problem on hold for awhile. I'll let you know when i try something new in a few days! Thank you!!
You are very welcome. I look forward to your next report. Good luck!
Hi @MingyuPan2 Do you wish to put this issue on hold as you are working on other projects, please? Thanks!
Yes, can you please put this question oh hold? Thank you! I'll start working on this project again very soon.
That's no problem at all, I have added an Enhancement label to this case as a reminder to keep it open. Thanks!
| Camera Model | D435i | | Firmware Version | 5.16.01 | | Operating System & Version | Ubuntu 22.04.4 LTS | | Kernel Version (Linux Only) | 5.10.0-1012-rockchip | | Platform | OrangePi 5 Plus | | SDK Version | 2.55.1.0 | | Language | C++ for all code & OpenCV | | Segment | Others |
Issue Description
Currently, i am trying to write some code in C++ to realize some of the camera's functions in Opencv's output window, as seen below:
As you can see at the bottom of the window where i try to get the distance, the output is not correct, since it gives a seemingly random value between 0 to 6000cm. However, if i turn off the depth stream and clicked on the frozen image of the stream, the output distance will be correct.
I've attached the sections of code where i output the distance below:
I assume that somehow, when the stream is live, the distance value is not properly aligned with the stream (but is aligned when the stream is paused)? Or the way i output the distance value when the stream is live is not correct somehow. I'm not good at coding so i did let chatgpt do a few passes on my code.
Can you help me with this issue in my code? Thank you so much!!!