I want to apply RealSense library's depth filtering (rs2::spatial_filter
) on an OpenCV Mat
, but it seems like the filter is not being applied. The original depth image and the supposedly filtered depth image look exactly the same.
To load raw depth data into rs2::frame
, I used modified @Groch88's answer. One of the changes I made was changing the depth format from RS2_FORMAT_Z16
to RS2_FORMAT_DISTANCE
(to be able to load a float
depth map) and not loading the RGB part of the frame. The whole source is below.
Why do the original and the filtered images look exactly the same? Am I missing something obvious?
main.cpp:
#include <iostream>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include "rsImageConverter.h"
int main()
{
cv::Mat rawDepthImg = load_raw_depth("/path/to/depth/image"); // loads float depth image
rsImageConverter ic{rawDepthImg.cols, rawDepthImg.rows, sizeof(float)};
if ( !ic.convertFrame(rawDepthImg.data, rawDepthImg.data) )
{
fprintf(stderr, "Could not load depth.\n");
exit(1);
}
rs2::frame rsDepthFrame = ic.getDepth();
// Filter
// https://dev.intelrealsense.com/docs/post-processing-filters
rs2::spatial_filter spat_filter;
spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2.0f);
spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.5f);
spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20.0f);
// Apply filtering
rs2::frame rsFilteredDepthFrame;
rsFilteredDepthFrame = spat_filter.process(rsDepthFrame);
// Copy filtered depth to OpenCV Mat
cv::Mat filteredDepth = cv::Mat::zeros(rawDepthImg.size(), CV_32F);
memcpy(filteredDepth.data, rsFilteredDepthFrame.get_data(), rawDepthImg.cols * rawDepthImg.rows * sizeof(float));
// Display (un)filtered images
cv::imshow("Original depth", rawDepthImg); // Original image is being shown
cv::imshow("Filtered depth", filteredDepth); // A depth image that looks exactly like the original unfiltered depth map is shown
cv::imshow("Diff", filteredDepth - rawDepthImg); // A black image is being shown
cv::waitKey(0);
return 0;
}
rsImageConverter.h (edited version of @Doch88's code):
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>
class rsImageConverter
{
public:
rsImageConverter(int w, int h, int bppDepth);
bool convertFrame(uint8_t* depth_data, uint8_t* color_data);
rs2::frame getDepth() const;
private:
int w = 640;
int h = 480;
int bppDepth = sizeof(float);
rs2::software_device dev;
rs2::software_sensor depth_sensor;
rs2::stream_profile depth_stream;
rs2::syncer syncer;
rs2::frame depth;
int ind = 0;
};
rsImageConverter.cpp (edited version of @Doch88's code):
#include "rsImageConverter.h"
rsImageConverter::rsImageConverter(int w, int h, int bppDepth) :
w(w),
h(h),
bppDepth(bppDepth),
depth_sensor(dev.add_sensor("Depth")) // initializing depth sensor
{
rs2_intrinsics depth_intrinsics{ w, h, (float)(w / 2), (float)(h / 2), (float) w , (float) h , RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } };
depth_stream = depth_sensor.add_video_stream({ RS2_STREAM_DEPTH, 0, 0,
w, h, 60, bppDepth,
RS2_FORMAT_DISTANCE, depth_intrinsics });
depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, 1.f); // setting depth units option to the virtual sensor
depth_sensor.open(depth_stream);
depth_sensor.start(syncer);
}
bool rsImageConverter::convertFrame(uint8_t* depth_data, uint8_t* color_data)
{
depth_sensor.on_video_frame({ depth_data, // Frame pixels
[](void*) {}, // Custom deleter (if required)
w*bppDepth, bppDepth, // Stride and Bytes-per-pixel
(rs2_time_t)ind * 16, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, ind, // Timestamp, Frame# for potential sync services
depth_stream });
ind++;
rs2::frameset fset = syncer.wait_for_frames();
depth = fset.first_or_default(RS2_STREAM_DEPTH);
return depth;
}
rs2::frame rsImageConverter::getDepth() const
{
return depth;
}