search

luxonis / depthai-ros

Official ROS Driver for DepthAI Sensors.
MIT License
259 stars 189 forks source link

[BUG] Dropouts in IMU data publishing on OAK-D PRO W #474

Open petrlmat opened 10 months ago

petrlmat commented 10 months ago

Issue description We are trying to run a VIO algorithm in ROS using the stereo IR images from OAK-D PRO W with mixed success. While analyzing the IMU data, we have found out that the data is published quite sporadically in batches. This is not ideal for VIO, which preintegrates IMU data between image frames to provide a stable high-frequency stream of state estimation. I'm not sure whether the problem is in the way the IMU messages are published to ROS or if the problem is in the reading of data from the sensor itself.

I will try to better explain the issue on these plots: image The OAK-D IMU data in this plot arrives in batches with periods in the tens of ms without any data. Notice the other IMU in the plot, which publishes evenly spaced data points on 100 Hz (OAK-D IMU runs on 400 Hz in this plot). That would be the desired behavior. Can it be achieved or is there something preventing getting evenly-spaced IMU data points from OAK-D?

Let's see the problem in rqt_bag: image The bins are 0.005s long, so each bin should contain 1 IMU message (IMU running at 200 Hz) but there seem to be a window without any message for over ~30 ms.

Solutions attempted We tried various settings of the IMU parameters to improve this behavior but so far have been unsuccessful in eliminating the problem entirely. The parameter having the biggest effect is the batch_report_threshold and potentially max_batch_reports. If I understand the parameters correctly, OAK waits until batch_report_threshold data points (IMU and camera images combined) before sending over USB and this batch can only have max_batch_reports IMU data points. So unless max_batch_reports < batch_report_threshold the max_batch_reports parameter should have no effect right? I included both parameter values in case I understand it wrong. Let's see how the parameters influence the consistency of IMU data frequency. The plots show the ROS time of message arrival on the X-axis and the message time stamp (from its header) on the Y-axis.

batch_report_threshold = 5; max_batch_reports = 10 (default) image I see periods of ~40 ms without IMU data. Also, the periodicity of the dropout is suspicious. Could something else be blocking the sending of IMU data?

batch_report_threshold = 20; max_batch_reports = 20 image Much worse but expected. OAK waits until 20 data points are available before sending. I included this test to show that changing the parameters in this direction won't help. Also, I saw somewhere in code comment that this setting might help in high-throughput applications, so I wanted to try it.

batch_report_threshold = 1; max_batch_reports = 10 image This is the best performance achieved so far. The messages are sent much more often but still, there are ~40ms dropouts. Seems that the dropout has a ~100ms period. Could the communication be blocked by sending camera images at these moments? I don't have access to the camera today but I will check if this happens also when running the IMU only.

The dropouts are periodic and seem to happen when images from all cameras arrive at the same time so indeed it seems that the communication is blocked by the images. Can IMU messages be somehow prioritized? image

System info

Serafadam commented 9 months ago

Hi, sorry for the delay and thanks for the detailed reports, we are currently discussing these issues internally and I will get back to you with the conclusions hopefully this/next week. Just one quick question, what IMU sync methods did you use here?

petrlmat commented 9 months ago

Thanks for looking into this! All plots in the report are with the acceleration interpolation IMU sync, which was the one with the most consistent data rate. The other methods were much worse based on my tests.

200 Hz acceleration interpolation

image

Derivative (dT = 0.005 s)

image

400 Hz, acceleration interpolation

image

Derivative (dT = 0.0025 s)

image

400 Hz copy

image

Derivative (dT = 0.0025 s)

image

400 Hz gyro interpolation

image

Derivative (dT = 0.0025 s)

image

petrlmat commented 7 months ago

Any updates on this issue?

themarpe commented 7 months ago

@petrlmat

Hi - just one thing to cross check, are you referring to When the reports arrive or What timestamp they hold? Due to comms channel we cannot have eg "realtime" IMU updates, but batched IMU readings should still hold correct individual timestamp per report. Those reports should be feed into the system with the timestamp component

petrlmat commented 6 months ago

Hi, I am referring to "When the reports arrive". Realtime IMU updates are necessary for running realtime state estimation algorithms. ~40 ms long period without data is just too much for state estimation of unstable systems such as drones. We would like to use your camera as a "do it all" camera on our drones but due to this issue we have to rely on our custom solution with realtime IMU. How can the time between batches be influenced? Would e.g. sending less camera streams through the comm channel increase the batch frequency?

themarpe commented 6 months ago

@petrlmat

How can the time between batches be influenced? Would e.g. sending less camera streams through the comm channel increase the batch frequency?

Yes, this is likely the biggest factor. You may quickly check it by only sending the IMU updates instead of camera stream as well

petrlmat commented 1 month ago

Any updates on this issue? Is there a plan or possibility to obtain the IMU measurements ASAP instead of getting them in batches or is it infeasible due to hardware limitations?