2

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;
}
Stan
  • 21
  • 1

0 Answers0