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

Realsense D455 Processing Speed #260

Closed
myboyhood opened this issue Jul 16, 2022 · 12 comments
Closed

Realsense D455 Processing Speed #260

myboyhood opened this issue Jul 16, 2022 · 12 comments
Labels
user-platform User has trouble running on their own platform.

Comments

@myboyhood
Copy link

Thansk for the OpenVINS project, I have learnd it about 6 months!

The question is the time-consuming performance in feature delay, initialize and update stage is normal ?

My result is record as following [ Realsense D455 , mono color 640 * 480 30Hz + IMU 200Hz] [running on my laptop computer 6*2.6GHz CPU Core]:

[TIME]: 0.0032 seconds for tracking
[TIME]: 0.0136 seconds for propagation
[TIME]: 0.1252 seconds for MSCKF update (10 feats)
[TIME]: 0.1449 seconds for SLAM update (66 feats)
[TIME]: 0.1194 seconds for SLAM delayed init (4 feats)
[TIME]: 0.0101 seconds for re-tri & marg (11 clones in state)
[TIME]: 0.4165 seconds for total (camera 0)

If I change to stereo version, The time will increase up to even total 1.44s per period.

As I know, when we play the ETH euroc dataset to test OpenVINS, we always set the queue size in callback as 9999(as large as possible) in order to process all images one by one. we ignore the real-time performance.

But when we test in real-time environment, For example, 30Hz image stream request VIO system to spend below 30ms to process image and imu info. I find it is hard for OpenVINS system.

I also tried VINS-Mono ro VINS-Fusion before, The optimization backend is also time-costly.

Kind friends could help me ?
@yangyulin
Copy link
Member

yangyulin commented Jul 16, 2022 via email

@goldbattle
Copy link
Member

goldbattle commented Jul 16, 2022 via email

@myboyhood
Copy link
Author

myboyhood commented Jul 17, 2022 •
edited by goldbattle

Hi, If you want to speed up, 1) Only 10-15hz image might be enough 2) less slam features 3) less msckf features You can tune these parameters to speed up the system. best, Yulin

Hi, I am really appreciated to your suggestion.

  1. I would try 15Hz image stream on Realsense D455 later

  2. About the backend processing time, I carefully make statistics about each step.

    [tracking part]: using Fast + optiflow costs 3ms

    [propagation part]: The time is linear to the size of IMU integration. For example, 10 times IMU integration and state argument costs about 10ms. if image stream is set to 10Hz, IMU frequency keeps 200Hz, in each period, there will be 20 IMU data to integrate and state argument, about 20ms.

    [MSCKF update]: each MSCKF feature update would cost average 12.5ms. This process includes triangulation, state update.(here I use max_clones = 11, which means the number of camera pose used in MSCKF feats is less than 11)

    [SLAM update]: each SLAM feats update would cost 2.2ms, For example, if I set max_slam_feats = 20, it will cost 44ms in SLAM update.

    [SLAM delayed init]: each SLAM delayed init process would cost 30ms. This process includes feature triangulation and G-N optimization, Since the observation count is 11 times, triangulation would be a little time-consuming. if I set max_clones = 5, it will speed up but the accuracy of feats and reprojection in state update would suffer.

    [re-tri & marg]:This part costs stably 9~10ms.

As analysis above, three part [propagation part] [MSCKF update] [SLAM delayed init] take most time.

Firstly, I doubt the running speed of my computer, maybe the single 2.6GHz cpu core running OpenVINS is really laborious.

Secondly, I found Realsense D455 is not stable. When the image stream is set to 15Hz and IMU is set to 200Hz. After a while, the image frequency drops to 13 or 8Hz strangely, imu data is also unstable (the USB3.1 cable loose? or the device is overheat? I am really troubled with those unstable devices, sad :-( ) . So I really want to get help which device RPNG usually use. I see xsens MTi in your paper, but I have no idea about the camera.

Finally, I am PhD candidate at robotics institution of Shanghai Jiao Tong University. Our lab mainly focuses on uav perception, navigation and control. We hope to have more friendship with RPNG, here is my mentor's [bio link] and introduction [bilibili video]

@goldbattle goldbattle added the user-platform User has trouble running on their own platform. label Jul 17, 2022
@yangyulin
Copy link
Member

yangyulin commented Jul 17, 2022 via email

@myboyhood
Copy link
Author

Can you provide the config you are using? And if possible an example bag with the roslaunch command to run it.
…
On Sat, Jul 16, 2022, 11:30 AM myboyhood @.> wrote: Thansk for the OpenVINS project, I have learnd it about 6 months! The question is the time-consuming performance in feature delay, initialize and update stage is normal ? My result is record as following [ Realsense D455 , mono color 640 * 480 30Hz + IMU 200Hz] [running on my laptop computer 62.6GHz CPU Core]: [TIME]: 0.0032 seconds for tracking [TIME]: 0.0136 seconds for propagation [TIME]: 0.1252 seconds for MSCKF update (10 feats) [TIME]: 0.1449 seconds for SLAM update (66 feats) [TIME]: 0.1194 seconds for SLAM delayed init (4 feats) [TIME]: 0.0101 seconds for re-tri & marg (11 clones in state) [TIME]: 0.4165 seconds for total (camera 0) If I change to stereo version, The time will increase up to even total 1.44s per period. As I know, when we play the ETH euroc dataset to test OpenVINS, we always set the queue size in callback as 9999(as large as possible) in order to process all images one by one. we ignore the real-time performance. But when we test in real-time environment, For example, 30Hz image stream request VIO system to spend below 30ms to process image and imu info. I find it is hard for OpenVINS system. I also tried VINS-Mono ro VINS-Fusion before, The optimization backend is also time-costly. Kind friends could help me ? — Reply to this email directly, view it on GitHub <#260>, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYUJM24UWVXH6C5MKSLVULIPZANCNFSM53YH4RUA . You are receiving this because you are subscribed to this thread.Message ID: @.*>

Hi. Thanks for your reply.
I have to admit that OpenVINS is the first effective tutorial of my SLAM research.

I tried the rosbag again on my desktop (3.6GHz per core)as suggested by [yangyulin](https://github.com/yangyulin) . Get info as following:

[TIME]: 0.0047 seconds for tracking
[TIME]: 0.0081 seconds for propagation
[TIME]: 0.0386 seconds for MSCKF update (12 feats)
[TIME]: 0.0009 seconds for SLAM update (11 feats)
[TIME]: 0.0738 seconds for SLAM delayed init (7 feats)
[TIME]: 0.0044 seconds for re-tri & marg (11 clones in state)
[TIME]: 0.1305 seconds for total (camera 0)

I share my rosbag and launch file on [Amazon Drive]

Thanks for your kind help !

@goldbattle
Copy link
Member

goldbattle commented Jul 17, 2022 •
edited

Please use the newest version of OpenVINS (you launch file is for a very old version).
Better yet, you should use the develop_v2.6.1 #259 branch which will be merged into master next week.

You will need to create a config yaml file specific to your platform:
https://github.com/rpng/open_vins/tree/develop_v2.6.1/config

Additionally, you need to make sure to change the realsense launch to fuse the gyroscope and accel.

<arg name="unite_imu_method"          default="linear_interpolation"/>

EDIT: Also make sure you perform calibration of the camera and IMU using kalibr. You should not trust the values reported by intel from their driver for intrinsics and extrinsics between the IMU and camera. Here are some example results I have used in the past for the d455 in its stereo mode.

@myboyhood
Copy link
Author

Please use the newest version of OpenVINS (you launch file is for a very old version). Better yet, you should use the develop_v2.6.1 #259 branch which will be merged into master next week.

You will need to create a config yaml file specific to your platform: https://github.com/rpng/open_vins/tree/develop_v2.6.1/config

Additionally, you need to make sure to change the realsense launch to fuse the gyroscope and accel.

<arg name="unite_imu_method"          default="linear_interpolation"/>

EDIT: Also make sure you perform calibration of the camera and IMU using kalibr. You should not trust the values reported by intel from their driver for intrinsics and extrinsics between the IMU and camera. Here are some example results I have used in the past for the d455 in its stereo mode.

Yes! The latest version develop_v2.6.1 has been implemented on my computer. It is really faster!

UpdaterMSCKF.cpp:290 [MSCKF-UP]: 0.0004 seconds to triangulate
UpdaterMSCKF.cpp:291 [MSCKF-UP]: 0.0010 seconds create system (5 features)
UpdaterMSCKF.cpp:292 [MSCKF-UP]: 0.0007 seconds compress system
UpdaterMSCKF.cpp:293 [MSCKF-UP]: 0.0008 seconds update state (66 size)
UpdaterMSCKF.cpp:294 [MSCKF-UP]: 0.0028 seconds total
UpdaterSLAM.cpp:474 [SLAM-UP]: 0.0000 seconds to clean
UpdaterSLAM.cpp:475 [SLAM-UP]: 0.0000 seconds creating linear system
UpdaterSLAM.cpp:477 [SLAM-UP]: 0.0000 seconds to update (1 feats of 4 size)
UpdaterSLAM.cpp:478 [SLAM-UP]: 0.0001 seconds total
UpdaterSLAM.cpp:246 [SLAM-DELAY]: 0.0000 seconds to clean
UpdaterSLAM.cpp:247 [SLAM-DELAY]: 0.0007 seconds to triangulate
UpdaterSLAM.cpp:248 [SLAM-DELAY]: 0.0095 seconds initialize (11 features)
UpdaterSLAM.cpp:249 [SLAM-DELAY]: 0.0102 seconds total
VioManager.cpp:588 [TIME]: 0.0100 seconds for tracking
VioManager.cpp:589 [TIME]: 0.0002 seconds for propagation
VioManager.cpp:590 [TIME]: 0.0029 seconds for MSCKF update (5 feats)
VioManager.cpp:592 [TIME]: 0.0001 seconds for SLAM update (12 feats)
VioManager.cpp:593 [TIME]: 0.0102 seconds for SLAM delayed init (11 feats)
VioManager.cpp:595 [TIME]: 0.0009 seconds for re-tri & marg (11 clones in state)
VioManager.cpp:603 [TIME]: 0.0243 seconds for total (camera 0 1)
VioManager.cpp:631 q_GtoI = -0.729,0.059,-0.108,0.673 | p_IinG = -0.336,-0.510,0.266 | dist = 10.57 (meters)
VioManager.cpp:633 bg = -0.0005,-0.0032,-0.0009 | ba = 0.1138,0.2069,0.0717
VioManager.cpp:637 camera-imu timeoffset = -0.06152
ROS1Visualizer.cpp:417 [TIME]: 0.0246 seconds total (40.6 hz, 6.61 ms behind)

Is the speed improved by multi-thread, async or other method? It is now can run on my Realsense D455 at 30Hz infra stereo mode.
What's more, if the system allows, 30 Hz is more robust and take less time to acquire SLAM feats to stabilize the system.

@goldbattle goldbattle changed the title The slow running time in feature delay, initialize and update impacts the real-time performance Realsense D455 Processing Speed Jul 21, 2022
@goldbattle
Copy link
Member

Glad you were able to get it working. I am not sure what caused it originally to be slow, but likely was just a much older version of the project, or maybe different compile flags.

@goldbattle
Copy link
Member

Example D455 config file is now in the repo here: https://github.com/rpng/open_vins/tree/master/config/rs_d455

@myboyhood
Copy link
Author

myboyhood commented Jan 6, 2023 •
edited

Thank you very much. I am graceful for the testing.
Are you testing with raw d455 /imu/data? In my painful experience, the imu in D455 is noisy when the camera installed on the flying drone. see figure [1]. the /imu/data/ on the left is euroc_mav_dataset. The right is using xsens-MTi-630 on my flying drone. By my test, the imu in D455 is even worse than the xsens-MTi-630.
imu_data_eurocdataset_on_left_xsensMTi630_on_right-2022-12-05 21-10-03
Figure 1 /imu/data/ comparion.
The left data from euroc mav dataset is reasonable. But the right data from my own drone is very noisy.
Yes! as the figure shows, it gets very noisy because the shaking of rotors of the drone. Even though I change a more stable drone platform, the noise is still very big. And also, you can see the /imu/data is good when we stop the rotors and hold the drone to move (note the end time at the right part of rqt_plot).

The above problem confused me for a long time in the last year.
Then I add shock-absorbing sponge at the bottom of IMU, the performance gets better but it still sometimes get drifting at the beginning of OpenVINS.

Then, I use s-g filter to filter the raw imu data, the imu data become very stable and the time-delay is not obvious.
See figure2, figure3.
imu_data_filter_x_2023-01-06 15-41-37
Figure 2 /imu/data/acceleration/x filter comparion.
imu_data_filter_z_2023-01-06 15-37-41
Figure 3 /imu/data/acceleration/z filter comparion.

After this s-g filter, I can execute OpenVINS successful.
I don't know my preprocessing of imu data is right? Are most of imu data for UAV SLAM should preprcessed by low-pass or s-g filter?
I writing a paper based on OpenVINS now, I am hoping for your help！ Thanks advance !

@goldbattle
Copy link
Member

The UAV rotors will have an impact on how noisy the IMU readings are. I think that physically damping the sensor platform (e.g. rubber rings) or filtering the readings is reasonable to try to address this. Additionally inflating the IMU noises from the stationary ones. The question is by how much to increase it...

Do you have the exact filter you are using?
s.g refering to Low-cost_IMU_Data_Denoising_using_Savitzky-Golay_Filters.pdf this paper?

Are you able to share a bag with the noisy IMU data in it?

@myboyhood
Copy link
Author

Thank you very much. I did refer the paper Low-cost_IMU_Data_Denoising_using_Savitzky-Golay_Filters.pdf.
I upload my rosbag, which is named by FF4-odom-imu_2022-12-10-21-22-24.bag in amazon driver.

path:        FF4-odom-imu_2022-12-10-21-22-24.bag
version:     2.0
duration:    1:45s (105s)
start:       Dec 10 2022 21:22:24.30 (1670678544.30)
end:         Dec 10 2022 21:24:09.45 (1670678649.45)
size:        53.7 MB
messages:    91865
compression: none [71/71 chunks]
types:       nav_msgs/Odometry [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu   [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /T265_2/camera/odom/sample   21064 msgs    : nav_msgs/Odometry
             /imu/data                    21032 msgs    : sensor_msgs/Imu  
             /imu/data_filter             16938 msgs    : sensor_msgs/Imu  
             /ov_msckf/odomimu            16434 msgs    : nav_msgs/Odometry
             /ov_msckf/odomimu_filtered   16397 msgs    : nav_msgs/Odometry

I found OpenVINS is not smooth as T265, So I have to adopt s-g filte for /ov_msckf/odomimu. And the velocity output from OpenVINS is also shaking. Thus it is really hard to feedback flying PID control.

I also provide my s-g filter in gitee myboyhood/aggressive201lss
you can try this s-g filter code if you are interested. Thank you very much!

hoping for your reply !

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
user-platform User has trouble running on their own platform.
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@goldbattle @yangyulin @myboyhood