Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

D455 imu and color/depth sync in python #13139

Closed
jeezrick opened this issue Jul 10, 2024 · 6 comments
Closed

D455 imu and color/depth sync in python #13139

jeezrick opened this issue Jul 10, 2024 · 6 comments
Labels
d455 python

Comments

@jeezrick
Copy link

jeezrick commented Jul 10, 2024 •
edited
Loading

Required Info
Camera Model D455
Firmware Version 5.16.0.1
Operating System & Version {Win (8.1/10) / Linux (Ubuntu 14/16/17) / MacOS
Kernel Version (Linux Only) jetpack5.1.3
Platform NVIDIA Jetson orin nx
SDK Version 2.55.1
Language python

Issue Description

Accel: x: -0.755112, y: -9.5811, z: -0.519752 at 1720582802.4813268, Gyro: x: -0.00159698, y: -0.00106465, z: 0 at 1720582802.4836738, fps=427.20554084334896
Accel: x: -0.755112, y: -9.60071, z: -0.509946 at 1720582802.4861624, Gyro: x: -0.00159698, y: -0.00159698, z: 0.000532325 at 1720582802.4861088, fps=290.86712898751733
Accel: x: -0.755112, y: -9.60071, z: -0.509946 at 1720582802.4861624, Gyro: x: -0.00106465, y: -0.000532325, z: 0.00159698 at 1720582802.488544, fps=425.213300892133
Accel: x: -0.764919, y: -9.5909, z: -0.539366 at 1720582802.490998, Gyro: x: -0.00106465, y: 0.00159698, z: 0.0021293 at 1720582802.4909785, fps=425.213300892133
Accel: x: -0.764919, y: -9.5909, z: -0.539366 at 1720582802.490998, Gyro: x: -0.00159698, y: 0.0031957, z: 0.00266163 at 1720582802.493413, fps=424.99787212483534
Accel: x: -0.774725, y: -9.56148, z: -0.539366 at 1720582802.4958327, Gyro: x: -0.00159698, y: 0.0031957, z: 0.00266163 at 1720582802.493413, fps=425.60162354134957
Accel: x: -0.774725, y: -9.56148, z: -0.539366 at 1720582802.4958327, Gyro: x: -0.0021293, y: 0.00372802, z: 0.00266163 at 1720582802.4958475, fps=791.0795926065636
Accel: x: -0.774725, y: -9.56148, z: -0.539366 at 1720582802.4958327, Gyro: x: -0.00159698, y: 0.0031957, z: 0.00266163 at 1720582802.4982815, fps=777.8753709198813
Accel: x: -0.764919, y: -9.55168, z: -0.529559 at 1720582802.5006657, Gyro: x: -0.00159698, y: 0.0031957, z: 0.00266163 at 1720582802.4982815, fps=424.7396455696203
Accel: x: -0.764919, y: -9.55168, z: -0.529559 at 1720582802.5006657, Gyro: x: -0.00159698, y: 0.00266163, z: 0.0031957 at 1720582802.5007153, fps=5706.536054421768
Accel: x: -0.764919, y: -9.55168, z: -0.529559 at 1720582802.5006657, Gyro: x: -0.0021293, y: 0.00266163, z: 0.00266163 at 1720582802.503148, fps=427.1620327935635
Processed depth at 1720582802.539346 and color frames at 1720582802.5393534, fps=30.144704216646662
Accel: x: -0.764919, y: -9.5811, z: -0.549172 at 1720582802.505498, Gyro: x: -0.0021293, y: 0.00266163, z: 0.00266163 at 1720582802.503148, fps=425.7744391432342
Accel: x: -0.764919, y: -9.5811, z: -0.549172 at 1720582802.505498, Gyro: x: -0.00266163, y: 0.00266163, z: 0.00159698 at 1720582802.5055816, fps=5667.978378378379
Accel: x: -0.764919, y: -9.5811, z: -0.549172 at 1720582802.505498, Gyro: x: -0.0021293, y: 0.00159698, z: 0.00106465 at 1720582802.5080159, fps=427.2925835370823
Accel: x: -0.764919, y: -9.60071, z: -0.519752 at 1720582802.5103292, Gyro: x: -0.0021293, y: 0.00159698, z: 0.00106465 at 1720582802.5080159, fps=425.3426630159213
Accel: x: -0.764919, y: -9.60071, z: -0.519752 at 1720582802.5103292, Gyro: x: 0, y: 0.00159698, z: 0.000532325 at 1720582802.510448, fps=788.9962377727614
Accel: x: -0.764919, y: -9.60071, z: -0.519752 at 1720582802.5103292, Gyro: x: 0.000532325, y: 0.00159698, z: 0.000532325 at 1720582802.5128808, fps=425.2995335631718
Accel: x: -0.764919, y: -9.5811, z: -0.509946 at 1720582802.5151606, Gyro: x: 0, y: 0.00266163, z: 0.000532325 at 1720582802.5153134, fps=425.64481428861376
Accel: x: -0.764919, y: -9.5811, z: -0.509946 at 1720582802.5151606, Gyro: x: -0.0021293, y: 0.00159698, z: 0.00106465 at 1720582802.5177457, fps=425.47210387502537
Accel: x: -0.764919, y: -9.57129, z: -0.549172 at 1720582802.5199904, Gyro: x: -0.0031957, y: 0.00266163, z: 0.00159698 at 1720582802.520178, fps=425.5584415584416
Accel: x: -0.764919, y: -9.57129, z: -0.549172 at 1720582802.5199904, Gyro: x: -0.00372802, y: 0.00372802, z: 0.0021293 at 1720582802.5226102, fps=419.3046086174148
Accel: x: -0.764919, y: -9.55168, z: -0.529559 at 1720582802.5248203, Gyro: x: -0.00426035, y: 0.005325, z: 0.00106465 at 1720582802.525042, fps=425.256412856129
Accel: x: -0.764919, y: -9.55168, z: -0.529559 at 1720582802.5248203, Gyro: x: -0.00479267, y: 0.005325, z: 0.000532325 at 1720582802.5274734, fps=425.1701976685251
Accel: x: -0.784532, y: -9.57129, z: -0.549172 at 1720582802.5296485, Gyro: x: -0.00479267, y: 0.00426035, z: 0.00106465 at 1720582802.5299048, fps=425.04094041345763
Accel: x: -0.784532, y: -9.57129, z: -0.549172 at 1720582802.5296485, Gyro: x: -0.00372802, y: 0.0031957, z: 0.00159698 at 1720582802.532336, fps=414.3340906845797
Accel: x: -0.784532, y: -9.60071, z: -0.509946 at 1720582802.5344765, Gyro: x: -0.00372802, y: 0.00159698, z: 0.00266163 at 1720582802.5347672, fps=400.83180428134557
Processed depth at 1720582802.569421 and color frames at 1720582802.5694284, fps=30.645331930501367
Accel: x: -0.784532, y: -9.60071, z: -0.509946 at 1720582802.5344765, Gyro: x: -0.0021293, y: 0.00106465, z: 0.0021293 at 1720582802.5371978, fps=287.9121361889072
Accel: x: -0.745305, y: -9.57129, z: -0.529559 at 1720582802.539304, Gyro: x: -0.0021293, y: 0.00106465, z: 0.0021293 at 1720582802.5371978, fps=782.5194029850746
Accel: x: -0.745305, y: -9.57129, z: -0.529559 at 1720582802.539304, Gyro: x: -0.0021293, y: 0.0021293, z: 0.00106465 at 1720582802.5396285, fps=5607.358288770053
Accel: x: -0.745305, y: -9.57129, z: -0.529559 at 1720582802.539304, Gyro: x: -0.00159698, y: 0.00266163, z: 0 at 1720582802.5420587, fps=291.49377997081103
Accel: x: -0.745305, y: -9.5909, z: -0.529559 at 1720582802.5441294, Gyro: x: -0.00159698, y: 0.00266163, z: 0 at 1720582802.5420587, fps=788.8478465299982
Accel: x: -0.745305, y: -9.5909, z: -0.529559 at 1720582802.5441294, Gyro: x: -0.0021293, y: 0.00106465, z: -0.000532325 at 1720582802.5444887, fps=775.8608953015168
Accel: x: -0.745305, y: -9.5909, z: -0.529559 at 1720582802.5441294, Gyro: x: -0.00159698, y: -0.00106465, z: 0 at 1720582802.5469186, fps=425.38580121703853
Accel: x: -0.764919, y: -9.5909, z: -0.519752 at 1720582802.548954, Gyro: x: -0.000532325, y: -0.0021293, z: 0.0021293 at 1720582802.5493484, fps=424.78266153534537
Accel: x: -0.764919, y: -9.5909, z: -0.519752 at 1720582802.548954, Gyro: x: 0, y: -0.00159698, z: 0.0021293 at 1720582802.5517778, fps=424.5246963562753
Accel: x: -0.764919, y: -9.57129, z: -0.529559 at 1720582802.5537791, Gyro: x: -0.00106465, y: -0.000532325, z: 0.00159698 at 1720582802.5542073, fps=425.3426630159213
Accel: x: -0.764919, y: -9.57129, z: -0.529559 at 1720582802.5537791, Gyro: x: -0.00159698, y: 0, z: 0.00106465 at 1720582802.5566363, fps=424.9548125633232
Accel: x: -0.774725, y: -9.5811, z: -0.539366 at 1720582802.558602, Gyro: x: -0.00266163, y: 0.000532325, z: 0.000532325 at 1720582802.559065, fps=424.9548125633232
Accel: x: -0.774725, y: -9.5811, z: -0.539366 at 1720582802.558602, Gyro: x: -0.0031957, y: 0.000532325, z: 0.00106465 at 1720582802.5614939, fps=290.9478357380688
Accel: x: -0.764919, y: -9.55168, z: -0.519752 at 1720582802.5634253, Gyro: x: -0.0031957, y: 0.000532325, z: 0.00106465 at 1720582802.5614939, fps=778.3084060122471
Accel: x: -0.764919, y: -9.55168, z: -0.519752 at 1720582802.5634253, Gyro: x: -0.0031957, y: 0.000532325, z: 0.000532325 at 1720582802.5639224, fps=789.441746659138
Accel: x: -0.764919, y: -9.55168, z: -0.519752 at 1720582802.5634253, Gyro: x: -0.00266163, y: 0.000532325, z: 0.00106465 at 1720582802.5663507, fps=425.51526833722227
Processed depth at 1720582802.5993268 and color frames at 1720582802.5993342, fps=30.43386519805249

Hi, I am running a python code, I create 2 pipeline to get color/depth stream and imu stream seperately. And I print the log right after I get the data. As you can see, depth/color timestamp is ahead roughly 50 ms of accel/gyro timestamp.

So what should I do syncing the data? I want to use the current image data, how can I assign the correct imu data.

I read issues/11330, issues/2188, issues/4525, issues/3205, not really sure about the solution.

import pyrealsense2 as rs
import numpy as np
import threading
import time
import cv2


def configure_rs_device_img(width, height, depth_fps, color_fps):
    cntx = rs.context()
    device = cntx.devices[0]
    serial_number = device.get_info(rs.camera_info.serial_number)
    # print(f"Device Serial Number: {serial_number}")

    config = rs.config()
    config.enable_device(serial_number)

    config.enable_stream(rs.stream.depth, width, height, rs.format.z16, depth_fps)
    config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, color_fps)
    pipeline = rs.pipeline()
    profile = pipeline.start(config)
    return pipeline, profile


def configure_rs_device_imu(accel_fps, gyro_fps):
    cntx = rs.context()
    device = cntx.devices[0]
    serial_number = device.get_info(rs.camera_info.serial_number)
    print(f"Device Serial Number: {serial_number}")

    config = rs.config()
    config.enable_device(serial_number)

    config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, accel_fps)
    config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, gyro_fps)
    pipeline = rs.pipeline()
    profile = pipeline.start(config)
    return pipeline, profile



def process_frames(pipeline):
    try:
        i = 0
        last_check_t = time.time()
        last_i = 0
        while True:
            frames = pipeline.wait_for_frames() # blocking

            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()

            if not depth_frame or not color_frame:
                print("SOME IMAGE DATA NOT AVAILABLE")
                continue

            depth_timestamp = depth_frame.get_timestamp()
            color_timestamp = color_frame.get_timestamp()

            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())
            # print(depth_image.min(), depth_image.max())
            aa = cv2.convertScaleAbs(depth_image, alpha=30)
            # print(aa.min(), aa.max())


            if i % 1 == 0:
                cur_t = time.time()
                fps = (i - last_i) / (cur_t - last_check_t)
                last_i, last_check_t = i, cur_t
                print(f"Processed depth at {depth_timestamp / 1000} and color frames at {color_timestamp / 1000}, {fps=}")
            i += 1
    except Exception as e:
        print("error", e)
def process_imu(pipeline):
    try:
        i = 0
        last_check_t = time.time()
        last_i = 0
        while True:
            frames = pipeline.poll_for_frames() # non-blocking

            accel_frame = frames.first_or_default(rs.stream.accel)
            gyro_frame = frames.first_or_default(rs.stream.gyro)

            if not accel_frame and not gyro_frame:
                # print("both NOT")
                time.sleep(0.001)  # do we need to sleep
                continue

            if not accel_frame:
                # print("accel NOT")
                time.sleep(0.001)  # do we need to sleep
                continue
            elif not gyro_frame:
                # print("gyro NOT")
                time.sleep(0.001)  # do we need to sleep
                continue

            accel_timestamp = accel_frame.get_timestamp()
            gyro_timestamp = gyro_frame.get_timestamp()
            # use newest timestamp

            accel_data = accel_frame.as_motion_frame().get_motion_data()
            gyro_data = gyro_frame.as_motion_frame().get_motion_data()

            if i % 1 == 0:
                cur_t = time.time()
                fps = (i - last_i) / (cur_t - last_check_t)
                last_i, last_check_t = i, cur_t
                print(f"Accel: {accel_data} at {accel_timestamp / 1000}, Gyro: {gyro_data} at {gyro_timestamp / 1000}, {fps=}")
            i += 1
    except Exception as e:
        print(e)

def dummy_sim(): # dummy function to hold the program alive
    i = 0
    while True:
        i += 1
        time.sleep(0.1)


if __name__ == "__main__":
    pipeline_img, _ = configure_rs_device_img(640, 480, depth_fps=30, color_fps=30)
    pipeline_imu, _ = configure_rs_device_imu(accel_fps=200, gyro_fps=400)
    try:
        thread = threading.Thread(target=process_frames, args=(pipeline_img, ))
        thread.daemon = True
        thread.start()
        time.sleep(1)

        thread_2 = threading.Thread(target=process_imu, args=(pipeline_imu, ))
        thread_2.daemon = True
        thread_2.start()

        dummy_sim()

        thread.join()
        thread_2.join()
    finally:
        pipeline_imu.stop()
        pipeline_img.stop()
@jeezrick
Copy link
Author

I think the expected sync result for me is like this:
ts_prev_depth -> ts_cur_imu_0 -> ts_cur_imu_1 -> ... -> ts_cur_imu_n -> ts_cur_depth -> ...

Now, it's not the case. So what should I do about it?

@MartyG-RealSense
Copy link
Collaborator

Hi @jeezrick It is normal for there to be an offset between IMU and RGB, as advised at the #3205 (comment) case that you linked to.

In regard to IMU and depth, each IMU data packet is timestamped using the depth sensor hardware clock to allow temporal synchronization between gyro, accel and depth frames, as stated in the Core Capabilities section of Intel's guide for getting IMU data.

https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/

In order for data to be to be timestamped with the depth sensor hardware clock though, you need to have support for hardware metadata enabled in the librealsense SDK. Hardware metadata support will be enabled if you do one of the following:

  1. Install librealsense from Debian packages.

  2. Build librealsense from source code with CMake with the -DFORCE_RSUSB_BACKEND=TRUE flag included in the CMake build instruction.

  3. Build librealsense from source code with CMake and apply a kernel patch script to patch the kernel for hardware metadata support.

@jeezrick
Copy link
Author

Thanks, I'm aware that depth and imu has some offset, imu data suppose to be ahead of depth data about 40 ms, which is not the case when I run this python code.

But after experiment, it turns out, you just need to run this code for 4 ~ 5 seconds before it reachs the above pattern. So, all fine.
Also, I am interested that what does this code do? what's the effect on final data?

    for (rs2::sensor sensor : sensors)
        if (sensor.supports(RS2_CAMERA_INFO_NAME)) {
            ++index;
            if (index == 1) {
                sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
                // sensor.set_option(RS2_OPTION_AUTO_EXPOSURE_LIMIT,50000);
                sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1); // emitter on for depth information
            }
            // std::cout << "  " << index << " : " << sensor.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
            get_sensor_option(sensor);
            if (index == 2){
                // RGB camera
                sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE,1);

                // sensor.set_option(RS2_OPTION_EXPOSURE,80.f);
            }

            if (index == 3){
                sensor.set_option(RS2_OPTION_ENABLE_MOTION_CORRECTION,0);
            }

        }

@MartyG-RealSense
Copy link
Collaborator

It looks as though if the Index number is set to 1 then auto-exposure and the camera's IR Emitter component are enabled. Auto-exposure enables the camera to automatically adjust exposure as lighting conditions in the real-world scene change, whilst the IR emitter when enabled projects an invisible pattern of dots onto the surface of objects in the scene to aid depth analysis of those surfaces.

Index 2 enables auto-exposure only.

Index 3 disables IMU motion correction, meaning that raw IMU data will be used and the SDK will not attempt to 'fix' the data to reduce inaccuracies.

@MartyG-RealSense
Copy link
Collaborator

Hi @jeezrick Do you require further assistance with this case, please? Thanks!

@MartyG-RealSense
Copy link
Collaborator

Case closed due to no further comments received.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
d455 python
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@MartyG-RealSense @jeezrick