Adding color or infrared stream to pipeline config limits IMU data frequency

[newpavlov]

Required Info
Camera Model	D435i
Firmware Version	05.12.13.50
Operating System & Version	Linux (Debian 10)
Kernel Version (Linux Only)	4.19.181-1
Platform	PC
SDK Version	2.45.0
Language	Rust/C
Segment	Robot

Issue Description

I have the following test code written in Rust, which uses low-level realsense-sys bindings:

use realsense_sys as sys;

let mut err = std::ptr::null_mut::<sys::rs2_error>();
let cfg_ptr = sys::rs2_create_config(&mut err);
check_err!(err);

let serial = std::ffi::CStr::from_bytes_with_nul(SERIAL).unwrap();
sys::rs2_config_enable_device(cfg_ptr, serial.as_ptr(), &mut err);
check_err!(err);

sys::rs2_config_enable_stream(
    cfg_ptr,
    sys::rs2_stream_RS2_STREAM_ACCEL,
    -1,
    0,
    0,
    sys::rs2_format_RS2_FORMAT_MOTION_XYZ32F,
    250,
    &mut err,
);
check_err!(err);

sys::rs2_config_enable_stream(
    cfg_ptr,
    sys::rs2_stream_RS2_STREAM_GYRO,
    -1,
    0,
    0,
    sys::rs2_format_RS2_FORMAT_MOTION_XYZ32F,
    400,
    &mut err,
);
check_err!(err);

sys::rs2_config_enable_stream(
    cfg_ptr,
    sys::rs2_stream_RS2_STREAM_COLOR,
    -1,
    1920,
    1080,
    sys::rs2_format_RS2_FORMAT_RGB8,
    30,
    &mut err,
);
check_err!(err);

let ctx_ptr = sys::rs2_create_context(sys::RS2_API_VERSION as i32, &mut err);
check_err!(err);
let pipeline_ptr = sys::rs2_create_pipeline(ctx_ptr, &mut err);
check_err!(err);

let can_resolve = sys::rs2_config_can_resolve(
    cfg_ptr,
    pipeline_ptr,
    &mut err,
);
check_err!(err);
if can_resolve == 0 {
    panic!("can't resolve")
}

let profile_ptr = sys::rs2_pipeline_start_with_config(
    pipeline_ptr,
    cfg_ptr,
    &mut err,
);
check_err!(err);

let mut frame_ptr = std::ptr::null_mut::<sys::rs2_frame>();

let mut t = std::time::Instant::now();
for i in 0..30 {
    let did_get_frame = sys::rs2_pipeline_try_wait_for_frames(
        pipeline_ptr,
        &mut frame_ptr,
        sys::RS2_DEFAULT_TIMEOUT,
        &mut err,
    );
    check_err!(err);

    let dt = t.elapsed();
    t = std::time::Instant::now();
    println!("{:?}", dt);
}

This code is executed with a D435i sensor. It should be fairly easy to understand even for those who are not familiar with Rust and it can be trivially translated to C. Even though the config clearly specifies that I want to get IMU data with maximum frequencies, I get the following results:

474.640582ms
13.325282ms
34.97394ms
39.268458ms
28.408355ms
33.75342ms
32.724463ms
38.969229ms
32.107605ms
34.686522ms
32.931375ms
32.310468ms
32.075062ms
31.486505ms
32.396584ms

In other words, IMU measurements are limited by frequency of the color stream (same applies to infrared streams as well). By disabling it, I get expected numbers:

802.201µs
33.364µs
293.8µs
75.334µs
142.012µs
242.76µs
196.658µs
187.745µs
190.266µs
211.006µs
120.042µs
448.655µs
106.252µs
400.98µs
111.481µs
380.925µs

Is it possible to get complete IMU data and images using one pipeline?

[MartyG-RealSense]

Hi @newpavlov If you need to improve performance on a three-stream setup of IMU and two others (e.g depth and color) then there are two main approaches that can be taken. The first is to create separate pipelines (the IMU on one pipeline on its own and the other two streams on the second pipeline.

An excellent Python script example for separate pipelines is in the link below. Typically there are not equivalent scripts for doing the same in C++.

#5628 (comment)

If you would rather not use separate pipelines, the other popular approach is to use callbacks. The link below has a Python callback script that streams IMU, depth and color.

#6370 (comment)

This link discusses callbacks for IMU in terms of C++:

#6404

[newpavlov]

@MartyG-RealSense
Thank you for the proposed solutions! I've tried the callback solution (for IMU and color streams) by replacing the code after sys::rs2_pipeline_start_with_config with the following one:

let mut user_data = (0u64, 0u64);

unsafe extern "C" fn on_frame(frame_ptr: *mut sys::rs2_frame, user_ptr: *mut c_void) {
    let user_data = &mut *(user_ptr as *mut (u64, u64));

    let mut err = std::ptr::null_mut::<sys::rs2_error>();
    let count = sys::rs2_embedded_frames_count(frame_ptr, &mut err);
    if err.is_null() {
        user_data.0 += count as u64;
    } else {
        user_data.1 += 1;
        sys::rs2_free_error(err);
    }

    sys::rs2_release_frame(frame_ptr);
}

let _profile_ptr = sys::rs2_pipeline_start_with_config_and_callback(
    pipeline_ptr,
    cfg_ptr,
    Some(on_frame),
    &mut user_data as *mut (u64, u64) as *mut c_void,
    &mut err,
);
check_err!(err);

std::thread::sleep(Duration::from_secs(100));

sys::rs2_pipeline_stop(
    pipeline_ptr,
    &mut err,
);
check_err!(err);

println!("frames: {}, errors: {}", user_data.0, user_data.1);

But I get the following result:

frames: 2984, errors: 65056

It looks like rs2_embedded_frames_count works as expected for color frames and returns an error for IMU frames. How a proper code for distinguishing between those frame types should look like?

[MartyG-RealSense]

It looks as though the above script is based on Java because it is for use with Rust, in particular realsense-rust. Is that correct, please?

https://github.com/Tangram-Vision/realsense-rust

If it is correct: given that Rust has a compatibility wrapper for librealsense but it is not an integrated part of the RealSense SDK, it is difficult to provide programming advice for Rust.

The maintainers of realsense-rust suggest raising an issue on their Gitlab site if you have a question about the workings of it.

https://gitlab.com/tangram-vision-oss/realsense-rust/-/issues

[newpavlov]

realsense-sys (on which the examples are built) is a thin wrapper around librealsense C API, realsense-rust is built on top of it and provides higher level idiomatic API. For a number of reasons I am using the low-level C API.

I have found that I should use rs2_get_frame_stream_profile to check frame kind, so I think we can close this issue.

[MartyG-RealSense]

Thanks very much @newpavlov for the update of your progress and for the sharing of information about realsense-sys, which should be of benefit to realsense-rust users reading this case in future.