Retrieve IMU data from svo2 recording

Hi,

In the SVO v2 recordings and with 4.1.4, you claim that the IMU (sensor data) is fully recorded at max frequency. Correct me if I’m wrong please.

I want to retrieve the IMU data at max frequency. Currently I am trying to collect the IMU data in a thread, becaue it is mandatory to call zed.grab(runtime_parameters) but that’s a bottleneck. I always fail to grab the full IMU data that way.

Can you please point out what am I doing wrong, or a documentation to retrieve the recorded IMU data from an SVO v2.

Thank you.

Hi @Dhiaeddine-Oussayed
Welcome to the Stereolabs community.

Please share your code to check for its correctness.

I also recommend upgrading to the latest ZED SDK v4.2.1 which introduced improvements on SVO2 reading.

Hi @Myzhar thank you for the warm welcom.

This is the code that I am using:

import sys
from typing import Tuple
from threading import Thread

import pyzed.sl as sl


SVO_SAVE_PATH = '/path/to/svo/file.svo2'
GRAB_FREQUENCY = 200  # in Hz
SEC_TO_NS = 10**9


def init_camera() -> Tuple[sl.Camera, sl.RuntimeParameters]:
    """Initialize camera and camera params."""
    zed = sl.Camera()

    init_params = sl.InitParameters()
    init_params.coordinate_units = sl.UNIT.METER
    init_params.set_from_svo_file(SVO_SAVE_PATH)
    init_params.svo_real_time_mode = False

    if zed.open(init_params) != sl.ERROR_CODE.SUCCESS:
        sys.exit()

    return zed


def grab_images(zed: sl.Camera) -> None:
    """Grab images to simulate camera grbbing in software without storing the image."""
    runtime_parameters = sl.RuntimeParameters()
    while zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
        pass

def record_imu(zed: sl.Camera, record_frequency: int) -> None:
    """Record IMU data in a csv file while camera is recording."""
    sensors_data = sl.SensorsData()
    sensor_rate_nanosec = (1 / record_frequency) * SEC_TO_NS
    last_ts_recorded = 0
    while zed.is_opened():
        if not zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.CURRENT) == sl.ERROR_CODE.SUCCESS:
            continue
        zed_imu = sensors_data.get_imu_data()
        ts = zed_imu.timestamp.get_nanoseconds()
        if ts > (last_ts_recorded + sensor_rate_nanosec):
            acceleration = zed_imu.get_linear_acceleration()
            angular_velocity = zed_imu.get_angular_velocity()
            er = 1 / ((ts - last_ts_recorded) / SEC_TO_NS)
            row = [round(er), ts, *acceleration, *angular_velocity, sensors_data.image_sync_trigger]
            last_ts_recorded = ts
            print(row)
                

if __name__ == '__main__':

    zed_camera = init_camera()
    sensor_thread_fn = Thread(target=record_imu, args=(zed_camera, GRAB_FREQUENCY))
    sensor_thread_fn.start()
    grab_images(zed_camera)
    zed_camera.close()
    sensor_thread_fn.join()

The first issue is that the frequency is not stable and fluctuate a lot.
Second when I use GRAB_FREQUENCY = 200 this line er = 1 / ((ts - last_ts_recorded) / SEC_TO_NS) returns an exact pattern of 200, 199, 147, 177, 165, 200, 199… But when I use GRAB_FREQUENCY = 300 I get exactly 200 each iteration.

Does v4.2.1 include frequency stability ?

Thank you for the help.

Hi @Dhiaeddine-Oussayed,

From the look of your script, it seems that the issue comes from the fact that you’ve placed the grab calls in a different thread.
Are you able to retrieve all 400Hz data when iterating over all IMU data, and then performing the call to grab sequentially?

Hi @mattrouss,

I don’t think the threading is the issue. I did change my code to grab the IMU the way you suggested, like this.

while zed.grab() == sl.ERROR_CODE.SUCCESS:
    while True:
        sensors_data = sl.SensorsData()
        if zed.get_sensors_data(sensors_data, sl.TIME_REFERENCE.CURRENT) == sl.ERROR_CODE.SUCCESS:
            zed_imu = sensors_data.get_imu_data()
            ts = zed_imu.timestamp.get_nanoseconds()
            er = 1 / Utils.ns_to_sec((ts - last_ts_recorded))
            last_ts_recorded = ts
            acceleration = zed_imu.get_linear_acceleration()
            angular_velocity = zed_imu.get_angular_velocity()
            row = [round(er), ts, *acceleration, *angular_velocity, sensors_data.image_sync_trigger]
            #writer.writerow(row)
            print(row)
        else:
            break

I am able to grab at full rate but sometimes there’s a drop in the grab rate like the following (first value is the rate, second value is timestamp in nanoseconds):

813,1728469381732366977
813,1728469381733596977
813,1728469381734826977
271,1728469381738516977
407,1728469381740976977
813,1728469381742206977

You can see the drop (i.e the lag between timestamps in some iterations). I already upgraded to 4.2.1 as suggested by the way.

Thank you for the help.

Would it be possible to send a small SVO containing the issues you are experiencing? This would help us analyze the problem and see if the issue happened during data recording or while reading.

This is a link to a small svo where there is some frequency drop while playback.

Thank you for assisting.