librealsense: Can't merge depth frame and color frame into one frameset

Required Info
Camera Model D435
Firmware Version 05.12.03.00
Operating System & Version Linux (Ubuntu 16)
Kernel Version (Linux Only) Linux 4.15.0-101-generic x86_64
Platform VMWare and Jetson TX2
SDK Version 2.34.0-0~realsense0.2250
Language C++
Segment others

Issue Description

I was trying to generate a frameset consisting of a depth frame and a color frame (RGB) based on the code from example software_device (https://github.com/IntelRealSense/librealsense/tree/master/examples/software-device).

The way I did it is as follows:

  1. I declared a software_device
  2. I added two software_sensors to the software_device
  3. I added a depth stream and a color stream separately to the two software_sensors
  4. I opened the streams and started a syncer
  5. I added the extrinsics to the depth stream
  6. I used cv::imread to read an image encoded in BGR8 and injected the image to the color sensor
  7. I used cv::imread to read an image encoded in MONO16 and injected the image to the depth sensor
  8. I called wait_for_frames on the syncer.

Now, the problem is that although I injected both a depth image and a color image to the device, the frameset that I received from the syncer always had a size of one. It turned out that the frameset never included a color frame!

My source code will be included in the end of this post for anyone interested.

What I want to do is to align the two images. As a matter of fact, I have previously collected depth and RGB images from the camera (their were not aligned) and I stored them as JPG/PNG files in the filesystem, and they are named by their Unix-time timestamps (e.g. 1591200967.895594238.jpg). Now I want to align the depth images’ viewport to that of the color images, so I am trying to use the software_device to re-merge these depth-color image pairs into framesets and use the align processing block to align them, and then save the aligned images.

In my code, I have set the timestamps of the frames to be [idx]/60, and the frame-numbers are set to be [idx], where idx=1…120. Given the fact that the timestamps of these depth-color frame pairs are perfectly synced (identical, to be precise), the syncer should have recognized it and grouped them into a single frameset. But I never see a color frame in my frameset! This is so perplexing, can anyone give me an explanation?

#include <iostream>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>

double W = 640;
double H = 480;


int main() {
    
    rs2::software_device dev;
    
    auto depth_sensor = dev.add_sensor("Depth");
    auto color_sensor = dev.add_sensor("Color");
    
    rs2_intrinsics depth_intrinsics{W, H, W / 2, H / 2, 383.859, 383.859,
                                    RS2_DISTORTION_BROWN_CONRADY,
                                    {0, 0, 0, 0, 0}};
    rs2_intrinsics color_intrinsics{W, H, W / 2, H / 2, 618.212, 618.463,
                                    RS2_DISTORTION_INVERSE_BROWN_CONRADY,
                                    {0, 0, 0, 0, 0}};
    
    auto depth_stream = depth_sensor.add_video_stream(
            {RS2_STREAM_DEPTH, 0, 0, W, H, 60, 2, RS2_FORMAT_Z16,
             depth_intrinsics});
    auto color_stream = color_sensor.add_video_stream(
            {RS2_STREAM_COLOR, 0, 1, W, H, 60, 3, RS2_FORMAT_BGR8,
             color_intrinsics}, true);
    
    dev.create_matcher(RS2_MATCHER_DEFAULT);
    rs2::syncer sync;
    
    depth_sensor.open(depth_stream);
    color_sensor.open(color_stream);
    
    depth_sensor.start(sync);
    color_sensor.start(sync);
    
    depth_stream.register_extrinsics_to(color_stream,
                                        {{1, 0, 0, 0, 1, 0, 0, 0, 1},
                                         {0, 0, 0}});
    
    rs2::frameset fs;
    
    cv::Mat color_image;
    cv::Mat depth_image;
    
    //inject the images to the sensors
    for (int idx = 0; idx < 120; ++idx) {
        //Purpose of this for loop: somebody said you start getting the 
        // color frames once you have received an ample amount of frames
        // (e.g., 30 frames), when the auto-exposure has converged.
        //But in my case, I have run wait_for_frames for 120 times, 
        // which given the 60Hz FPS, is equivalent to 2 seconds.
        //Yet I am still not receiving any color frames in the frameset.
        color_image = cv::imread("/home/nvidia/Desktop/images/color.jpg",
                                 cv::IMREAD_COLOR);
        depth_image = cv::imread("/home/nvidia/Desktop/images/depth.png",
                                 cv::IMREAD_UNCHANGED);
        
        color_sensor.on_video_frame({(void*) color_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     3 * 640, 3, // Stride and Bytes-per-pixel
                                     (double) (idx) / 60, RS2_TIMESTAMP_DOMAIN_COUNT,
                                     idx, // Timestamp, Frame# for potential sync services
                                     color_stream});
        depth_sensor.on_video_frame({(void*) depth_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     2 * 640, 2, // Stride and Bytes-per-pixel
                                     (double) (idx) / 60, RS2_TIMESTAMP_DOMAIN_COUNT,
                                     idx, // Timestamp, Frame# for potential sync services
                                     depth_stream});
        
        fs = sync.wait_for_frames();
    }
    
    rs2::frame depth_frame = fs.get_depth_frame();
    rs2::frame color_frame = fs.get_color_frame();
    
    return 0;
}

About this issue

  • Original URL
  • State: closed
  • Created 4 years ago
  • Reactions: 1
  • Comments: 23 (2 by maintainers)

Most upvoted comments

Here’s the code that worked out for me in the end – in case there’s s.b. who is interested to refer to it (and thanks to everybody who contributed to this issue):

#include <iostream>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>

double W = 640;
double H = 480;


int main() {
    
    rs2::software_device dev;
    
    auto depth_sensor = dev.add_sensor("Depth");
    auto color_sensor = dev.add_sensor("Color");
    
    rs2_intrinsics depth_intrinsics{W, H, 315.505, 239.526, 383.859, 383.859,
                                    RS2_DISTORTION_BROWN_CONRADY,
                                    {0, 0, 0, 0, 0}};
    rs2_intrinsics color_intrinsics{W, H, 320.243, 237.202, 618.212, 618.463,
                                    RS2_DISTORTION_INVERSE_BROWN_CONRADY,
                                    {0, 0, 0, 0, 0}};
    
    auto depth_stream = depth_sensor.add_video_stream(
            {RS2_STREAM_DEPTH, 0, 0, W, H, 60, 2, RS2_FORMAT_Z16,
             depth_intrinsics});
    auto color_stream = color_sensor.add_video_stream(
            {RS2_STREAM_COLOR, 0, 1, W, H, 60, 3, RS2_FORMAT_BGR8,
             color_intrinsics});

    depth_sensor.add_read_only_option(RS2_OPTION_DEPTH_UNITS, 0.001f);
    depth_sensor.add_read_only_option(RS2_OPTION_STEREO_BASELINE, 0.001f);
    
    depth_stream.register_extrinsics_to(color_stream,
                                        {{0.999924,  -0.0103129,   -0.00680374,
                                                 0.0103063, 0.999946, -0.00100439,
                                                 0.00681374, 0.000934194, 0.999976},
                                         {0.0146439, -8.40931e-05, 0.000575048}});
    
    dev.create_matcher(RS2_MATCHER_DEFAULT);
    rs2::syncer sync;
    
    depth_sensor.open(depth_stream);
    color_sensor.open(color_stream);
    
    depth_sensor.start(sync);
    color_sensor.start(sync);
    
    rs2::align align(RS2_STREAM_COLOR);
    
    rs2::frameset fs;
    rs2::frame depth_frame;
    rs2::frame color_frame;
    
    cv::Mat color_image;
    cv::Mat depth_image;
    
    int idx = 0;
    
    //inject the images
    for (idx = 0; idx < 3; ++idx) {
        color_image = cv::imread("/home/nvidia/Desktop/images/color.jpg",
                                 cv::IMREAD_COLOR);
        depth_image = cv::imread("/home/nvidia/Desktop/images/depth.png",
                                 cv::IMREAD_UNCHANGED);
        
        color_sensor.on_video_frame({(void*) color_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     3 * 640, 3, // Stride and Bytes-per-pixel
                                     double(idx * 16), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
                                     idx, // Timestamp, Frame# for potential sync services
                                     color_stream});
        depth_sensor.on_video_frame({(void*) depth_image.data, // Frame pixels from capture API
                                     [](void*) {}, // Custom deleter (if required)
                                     2 * 640, 2, // Stride and Bytes-per-pixel
                                     double(idx * 16), RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME,
                                     idx, // Timestamp, Frame# for potential sync services
                                     depth_stream});
        
        fs = sync.wait_for_frames();
        if (fs.size() == 2) {
            fs = align.process(fs);
            rs2::frame depth_frame = fs.get_depth_frame();
            rs2::frame color_frame = fs.get_color_frame();
            
            cv::Mat aligned_image(480, 640, CV_16UC1, (void*) (depth_frame.get_data()), 2 * 640);
            cv::imwrite("aligned" + std::to_string(idx) + ".png", aligned_image);
        }
        
    }
    
    return 0;
}

@kafan1986 Thanks so much for your great contributions of help to the RealSense community!

@FJShen I’m very glad that you achieved a solution.