Huge delay in Point Cloud

[bhaskar-glitch]

Hi, I'm using the depth image from Realsense d435i camera for the point cloud(/camera/depth/img_rect_raw) in Fast planner but the point cloud is having a huge delay of 5-6 seconds,it's a big delay for drones and it may crash.Is this a problem of fast planner(converting depth to pcl) or rviz(publish rate)? PLease help regarding the issue The Point cloud is like this rqt graph

[bigsuperZZZX]

This repository is ego-planner not Fast planner where this problem is encountered. Maybe try ego-planner and check if this delay happens and if it still exists, then rasie the issue again.

[bhaskar-glitch]

This repository is ego-planner not Fast planner where this problem is encountered. Maybe try ego-planner and check if this delay happens and if it still exists, then rasie the issue again.

Hi, I tried the Ego planner too but the delay in Depth image to Grid is extremely slow as in fast planner.

Now let me explain you everything I'm using:- 1.) Realsense d435i camera 2.) Jetson Xavier NX onboard Computer 3.) Vins Fusion for localization

the depth image from the camera is working perfect(no delay in it) also i tried increasing decreasing the fps but nothing helped..the huge delay remained same. The vins fusion I'm using is working perfect.

You may see the screenrecording of delay here https://photos.app.goo.gl/VxJfWRStFdoPvqpn8

[bigsuperZZZX]

It's difficult to determine what's going wrong from your video, maybe you can first check the callback frequency of one of the functions depthPoseCallback or depthOdomCallback depending on which input method you choose. The callback frequency should be identical to your input message frequency.

If they are not identical, please leave here the function you use on your platform and upload the bag file containing depth and camera_pose/odometry message to google drive, I will check it.

[bigsuperZZZX]

By the way, what onboard computer do you use?

[bhaskar-glitch]

By the way, what onboard computer do you use?

I'm using Jetson Xavier NX

[bigsuperZZZX]

Jetson Xavier NX can run 10 ego-planners simultaneously, so performance is not the bottleneck.

[bhaskar-glitch]

It's difficult to determine what's going wrong from your video, maybe you can first check the callback frequency of one of the functions depthPoseCallback or depthOdomCallback depending on which input method you choose. The callback frequency should be identical to your input message frequency.

If they are not identical, please leave here the function you use on your platform and upload the bag file containing depth and camera_pose/odometry message to google drive, I will check it.

How to check the callback frequency of the functions you told?? i didn't understand can you please elaborate

[FPSychotic]

This repository is ego-planner not Fast planner where this problem is encountered. Maybe try ego-planner and check if this delay happens and if it still exists, then rasie the issue again.

Hi, I tried the Ego planner too but the delay in Depth image to Grid is extremely slow as in fast planner.

Now let me explain you everything I'm using:- 1.) Realsense d435i camera 2.) Jetson Xavier NX onboard Computer 3.) Vins Fusion for localization

the depth image from the camera is working perfect(no delay in it) also i tried increasing decreasing the fps but nothing helped..the huge delay remained same. The vins fusion I'm using is working perfect.

You may see the screenrecording of delay here https://photos.app.goo.gl/VxJfWRStFdoPvqpn8

I had running Vins Fusion GPU in a Xavier NX too, it is quite surprising you say it runs perfect, it doesn't with me and another two devs, it runs from barely OK with a d435 in low resolution as 400p or even less, to really bad in a OAK-D at 720pin in the NX. It takes the 100% of.CPU even at 400p (with RVIZ on-board), so probably you have not enough resources. Would be nice if you post a video with your Vins performance and the free resources with jetson-stats (jtop command). I cannot run Vins Fusion as you described andy setup os quite good, NVME,opencv 4.4 with cuda and cudnn etc, I think your problem maybe is derivated of VINS. You can see here the massive latency that VINS fusion get with OAK-D at 720p https://youtu.be/Pbe17GtwTzQ

[EhrazImam]

@FPSychotic we have tried several times VINS- FUSION-GPU and it works fine... As we have already check the stats... surely will share you the video...by tomorrow... Give it a try with the opencv-3.4.1 https://github.com/arjunskumar/vins-fusion-gpu-tx2-nano you can follow this...

[FPSychotic]

@FPSychotic we have tried several times VINS- FUSION-GPU and it works fine... As we have already check the stats... surely will share you the video...by tomorrow... Give it a try with the opencv-3.4.1 https://github.com/arjunskumar/vins-fusion-gpu-tx2-nano you can follow this...

I guess we are suffering the same issue than this
https://github.com/arjunskumar/vins-fusion-gpu-tx2-nano/issues/4. Maybe he is affected too. Probably our issue is related with 20.04/noetic and opencv4.4 and the PR we have installed. Sadly I cannot use OpenCV3x

[EhrazImam]

Not sure about the noetic.....in my case it was working fine on melodic..

[bhaskar-glitch]

It's difficult to determine what's going wrong from your video, maybe you can first check the callback frequency of one of the functions depthPoseCallback or depthOdomCallback depending on which input method you choose. The callback frequency should be identical to your input message frequency.

If they are not identical, please leave here the function you use on your platform and upload the bag file containing depth and camera_pose/odometry message to google drive, I will check it.

Hi @bigsuperZZZX This is the bag file https://drive.google.com/file/d/17ISud4i6q3JiSFyarO-13vu5OeGeoH2T/view?usp=drivesdk

[bigsuperZZZX]

Please check the code as I suggested first and provide me with the results.

[bhaskar-glitch]

Please check the code as I suggested first and provide me with the results.

Okay sure but im unable to check the callback frequency of the functions you told?? i didn't understand what you said...Can you please give me some brief info...that what exactly I've to check?

[bigsuperZZZX]

Well, it requires some code modifications to know the frequency, but never mind, I have done these tests and posted the result here. It seems that you are using a non-standard odometry frame with x-y-z to up-left-back. so I modified the extrinsic to

md_.cam2body_ << 0.0, -1.0, 0.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, 0.0, -1.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

in grid_map.cpp. Then everything looks well.

[bhaskar-glitch]

Well, it requires some code modifications to know the frequency, but never mind, I have done these tests and posted the result here. It seems that you are using a non-standard odometry frame with x-y-z to up-left-back. so I modified the extrinsic to

md_.cam2body_ << 0.0, -1.0, 0.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, 0.0, -1.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

in grid_map.cpp. Then everything looks well.

I tried the extrinsic values you have given but the point cloud is quite small and also orientation of cloud is wrong

[EhrazImam]

body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ -5.7586305857286746e-03, -4.0463318787729019e-03, 9.9997523237933461e-01, 2.0329267950355900e-02, -9.9998287214160420e-01, -1.0224590553211677e-03, -5.7628118925283633e-03, 7.9325209639615653e-03, 1.0457519809151661e-03, -9.9999129084997906e-01, -4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]

body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ -1.0021770212322867e-03, 3.6313480322730518e-04, 9.9999943188700535e-01, 1.5285779565991807e-02, -9.9999216342926500e-01, -3.8303422615924010e-03, -1.0007788055728661e-03, -5.2435791444330505e-02, 3.8299766679101843e-03, -9.9999259827824449e-01, 3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]

@bigsuperZZZX we have to make changes according to this parameters ? The grid is slow bz of this non-standard odometry frame??? And thanx for quick reply..

[bigsuperZZZX]

Well, it requires some code modifications to know the frequency, but never mind, I have done these tests and posted the result here. It seems that you are using a non-standard odometry frame with x-y-z to up-left-back. so I modified the extrinsic to

md_.cam2body_ << 0.0, -1.0, 0.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, 0.0, -1.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

in grid_map.cpp. Then everything looks well.

As I have mentioned in the last line, this modification should be made in the grid_map.cpp file not the VINS configuration.

[EhrazImam]

Well, it requires some code modifications to know the frequency, but never mind, I have done these tests and posted the result here. It seems that you are using a non-standard odometry frame with x-y-z to up-left-back. so I modified the extrinsic to

md_.cam2body_ << 0.0, -1.0, 0.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, 0.0, -1.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

in grid_map.cpp. Then everything looks well.

As I have mentioned in the last line, this modification should be made in the grid_map.cpp file not the VINS configuration.

Yes, We did followed your modification in the grid_map.cpp. But the grid was to small to identify whats happening in the grid as above @bhaskar-glitch shared you the screenshot.

And, Is this delay because of non-standard odometry frame?

[bigsuperZZZX]

That's weird. Maybe try it on a desktop computer?

[EhrazImam]

That's weird. Maybe try it on a desktop computer?

ok I will try on Desktop but still i am not getting is, that Delay in depth to grid is because of non-standard odometry frame? or something else..

[bigsuperZZZX]

I'm not sure where this delay comes from, because I can not reproduce this issue on my computer

[bigsuperZZZX]

I'll close this issue if there are no more questions. Feel free to open it again whenever you want.