search

MIT-SPARK / Kimera-VIO-ROS

ROS wrapper for Kimera-VIO
BSD 2-Clause "Simplified" License
365 stars 151 forks source link

Output freezes/lags when using intel D435i with Kimera Vio Ros #113

Open skiddattle opened 3 years ago

skiddattle commented 3 years ago

Description: Hi, I am a undergraduate and have been using trying to run kimera vio ros along with intel d435i. I am able to run the easy v101 rosbag with no issues with this kimera vio ros installation.

I have managed to get it to build and successfully run, but then receive a laggy output when it comes to textured mesh. Besides on that, I have also tried to calibrate the cameras using Kalibr, but have no changes in the performance yet. Can I get your insight as to what might be the problem? I have highlighted some console output I have that might be significant, but I am not sure that might the problem.

The main issue is that the output freezes every now and then, and the console for kimera_vio_ros_realsense_IR.launch switches between 2 states, as shown below in the console output. It will freeze in state1 and then switch to state 2 briefly, then starts to move for a while.

Links: I have included a youtube link here of a demo of the output i am getting: https://youtu.be/uvWD0-YqHjs

Here is a link to the entire screen log of the output for kimera_vio_ros_realsense_IR.launch: https://drive.google.com/file/d/1jWju043nz0kkA6HzRJssPfWc4cyy-sQr/view?usp=sharing

Here is a link to the screen log for rs_camera.launch: https://drive.google.com/file/d/1tdibWVNbaBJAoIymEm2dci-RENHXnqN6/view?usp=sharing

The files that I have changed are basically rs_camera.launch, kimera_vio_ros_realsense_IR.launch and kimera_vio_realsense.rviz, as well as nodelet.launch.xml. The major changes are:

1) rs_camera.launch

2) nodelet.launch.xml

Command:

roscore 
roslaunch realsense2_camera rs_camera.launch unite_imu_method:=linear_interpolation
rosrun dynamic_reconfigure dynparam set /camera/stereo_module emitter_enabled 0
roslaunch kimera_vio_ros kimera_vio_ros_realsense_IR.launch
rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_realsense.rviz

Console output:

From kimera_vio_ros_realsense_IR.launch:

Stable State 1
-----------                                  #  Log Hz  {avg     +- std    }    [min,max]
Data Provider [ms]                              0   
Display [ms]                                   75   7.23498 {0.893333 +- 6.61197}   [0,57]
Mesher [ms]                                    38   3.66008 {0.105263 +- 0.388307}  [0,2]
VioBackEnd [ms]                                38   3.66007 {2.76316 +- 1.95122}    [0,7]
VioFrontEnd Frame Rate [ms]                   227   22.4427 {13.0617 +- 2.54834}    [6,24]
VioFrontEnd Keyframe Rate [ms]                 37   3.70204 {72.1622 +- 15.5644}    [56,112]
VioFrontEnd [ms]                              265   25.2435 {21.9585 +- 22.8785}    [6,139]
Visualizer [ms]                                38   3.66003 {0.00000 +- 0.00000}    [0,0]
backend_input_queue Size [#]                   38   3.64872 {1.00000 +- 0.00000}    [1,1]
data_provider_left_frame_queue Size [#]       311   29.6291 {22.2315 +- 4.23769}    [1,44]
data_provider_right_frame_queue Size [#]      311   29.6289 {22.2315 +- 4.23769}    [1,44]
display_input_queue Size [#]                   75   7.19802 {1.00000 +- 0.00000}    [1,1]
mesher_backend Size [#]                        38   3.65999 {1.00000 +- 0.00000}    [1,1]
mesher_frontend Size [#]                      265   25.2431 {3.85660 +- 1.99178}    [1,7]
stereo_frontend_input_queue Size [#]          265   25.1910 {1.06038 +- 0.238683}   [1,3]
visualizer_backend_queue Size [#]              38   3.66000 {1.00000 +- 0.00000}    [1,1]
visualizer_frontend_queue Size [#]            265   25.2431 {3.85660 +- 1.99178}    [1,7]
visualizer_mesher_queue Size [#]               38   3.66000 {1.00000 +- 0.00000}    [1,1]
I1029 10:20:38.813395  2697 RegularVioBackEnd.cpp:212] Tracker has a LOW_DISPARITY status.
W1029 10:20:38.817015  2698 Mesher.cpp:1380] Missing landmark information to build 3D Mesh.
W1029 10:20:38.868305  2705 DataProviderModule.cpp:103] Waiting for IMU data...
I1029 10:20:39.241838  2677 KimeraVioRos.cpp:114] Statistics
-----------                                  #  Log Hz  {avg     +- std    }    [min,max]
Data Provider [ms]                              0   
Display [ms]                                   77   7.28086 {0.870130 +- 6.52596}   [0,57]
Mesher [ms]                                    39   3.68215 {0.102564 +- 0.383534}  [0,2]
VioBackEnd [ms]                                39   3.68214 {2.79487 +- 1.93553}    [0,7]
VioFrontEnd Frame Rate [ms]                   230   22.3865 {13.0696 +- 2.56755}    [6,24]
VioFrontEnd Keyframe Rate [ms]                 38   3.72399 {72.0263 +- 15.3755}    [56,112]
VioFrontEnd [ms]                              269   25.2413 {22.0409 +- 22.7434}    [6,139]
Visualizer [ms]                                39   3.68210 {0.00000 +- 0.00000}    [0,0]
backend_input_queue Size [#]                   39   3.67085 {1.00000 +- 0.00000}    [1,1]
data_provider_left_frame_queue Size [#]       326   29.6464 {23.4417 +- 5.22577}    [1,55]
data_provider_right_frame_queue Size [#]      326   29.6460 {23.4417 +- 5.22577}    [1,55]
display_input_queue Size [#]                   77   7.24440 {1.00000 +- 0.00000}    [1,1]
mesher_backend Size [#]                        39   3.68207 {1.00000 +- 0.00000}    [1,1]
mesher_frontend Size [#]                      269   25.2409 {3.84387 +- 2.00048}    [1,7]
stereo_frontend_input_queue Size [#]          269   25.1878 {1.05948 +- 0.238683}   [1,3]
visualizer_backend_queue Size [#]              39   3.68207 {1.00000 +- 0.00000}    [1,1]
visualizer_frontend_queue Size [#]            269   25.2409 {3.84387 +- 2.00048}    [1,7]
visualizer_mesher_queue Size [#]               39   3.68208 {1.00000 +- 0.00000}    [1,1]
I1029 10:20:39.741838  2677 KimeraVioRos.cpp:114] Statistics

-----------

Stable State 2:
mW1029 10:20:50.418304  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039348120213[ns].
W1029 10:20:50.418315  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039348120213
W1029 10:20:50.418331  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039381815910[ns].
W1029 10:20:50.418346  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039381815910
W1029 10:20:50.418365  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039415513992[ns].
W1029 10:20:50.418377  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039415513992
W1029 10:20:50.418398  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039449214220[ns].
W1029 10:20:50.418411  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039449214220
W1029 10:20:50.418428  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039482916594[ns].
W1029 10:20:50.418457  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039482916594
W1029 10:20:50.418496  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039516620874[ns].
W1029 10:20:50.418514  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039516620874
W1029 10:20:50.418540  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039550327539[ns].
W1029 10:20:50.418558  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039550327539
W1029 10:20:50.418586  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039584036350[ns].
W1029 10:20:50.418597  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039584036350
W1029 10:20:50.418639  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039617745876[ns].
W1029 10:20:50.418658  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039617745876
W1029 10:20:50.418678  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039651458979[ns].
W1029 10:20:50.418691  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039651458979
W1029 10:20:50.418712  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039685174227[ns].
W1029 10:20:50.418726  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039685174227
W1029 10:20:50.418745  2705 ThreadsafeImuBuffer.cpp:118] No IMU measurements available strictly between time 1603938038370243073[ns] and 1603938039718891144[ns].
W1029 10:20:50.418759  2705 DataProviderModule.cpp:124] No IMU measurements here, and IMU data stream already passed this time regionfrom timestamp: 1603938038370243073 to timestamp: 1603938039718891144

From  rs_camera.launch:
NODES
  /camera/
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)

ROS_MASTER_URI=http://localhost:11311
]2;/home/bryan/catkin_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch http://localhost:11311
process[camera/realsense2_camera_manager-1]: started with pid [3992]
process[camera/realsense2_camera-2]: started with pid [3993]
[ INFO] [1603940118.616062570]: Initializing nodelet with 4 worker threads.
[ INFO] [1603940118.721459545]: RealSense ROS v2.2.14
[ INFO] [1603940118.721516166]: Running with LibRealSense v2.35.2
[ INFO] [1603940118.730260415]:  
[ INFO] [1603940118.891535161]: Device with serial number 843112071857 was found.

[ INFO] [1603940118.891647787]: Device with physical ID /sys/devices/pci0000:00/0000:00:14.0/usb3/3-1/3-1:1.0/video4linux/video0 was found.
[ INFO] [1603940118.891713807]: Device with name Intel RealSense D435I was found.
[ INFO] [1603940118.893061551]: Device with port number 3-1 was found.
[ INFO] [1603940118.895981891]: getParameters...
[ INFO] [1603940118.942705518]: setupDevice...
[ INFO] [1603940118.942753855]: JSON file is not provided
[ INFO] [1603940118.942798787]: ROS Node Namespace: camera
[ INFO] [1603940118.942834717]: Device Name: Intel RealSense D435I
[ INFO] [1603940118.942859320]: Device Serial No: 843112071857
[ INFO] [1603940118.942883138]: Device physical port: /sys/devices/pci0000:00/0000:00:14.0/usb3/3-1/3-1:1.0/video4linux/video0
[ INFO] [1603940118.942928117]: Device FW version: 05.12.07.100
[ INFO] [1603940118.942952298]: Device Product ID: 0x0B3A
[ INFO] [1603940118.942976729]: Enable PointCloud: Off
[ INFO] [1603940118.943005113]: Align Depth: Off
[ INFO] [1603940118.943026434]: Sync Mode: Off
[ INFO] [1603940118.943087732]: Device Sensors: 
[ INFO] [1603940118.945150812]: Stereo Module was found.
 29/10 10:55:18,945 WARNING [140357300827904] (backend-v4l2.cpp:1223) Pixel format 36315752-1a66-a242-9065-d01814a likely requires patch for fourcc code RW16!
[ INFO] [1603940118.949948220]: RGB Camera was found.
[ INFO] [1603940118.950179983]: Motion Module was found.
[ INFO] [1603940118.950265597]: (Infrared, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1603940118.950310646]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping...
[ INFO] [1603940118.950346984]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping...
[ INFO] [1603940118.950372999]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1603940118.950436872]: num_filters: 0
[ INFO] [1603940118.950500522]: Setting Dynamic reconfig parameters.
[ INFO] [1603940119.200808639]: Done Setting Dynamic reconfig parameters.
[ INFO] [1603940119.201574235]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16
[ INFO] [1603940119.201849304]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1603940119.202170753]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1603940119.202749619]: color stream is enabled - width: 640, height: 480, fps: 30, Format: RGB8
[ INFO] [1603940119.202794703]: setupPublishers...
[ INFO] [1603940119.205209501]: Expected frequency for depth = 30.00000
[ INFO] [1603940119.238664210]: Expected frequency for infra1 = 30.00000
[ INFO] [1603940119.261180560]: Expected frequency for infra2 = 30.00000
[ INFO] [1603940119.280002662]: Expected frequency for color = 30.00000
[ INFO] [1603940119.299729206]: Start publisher IMU
[ INFO] [1603940119.302056381]: setupStreams...
[ INFO] [1603940119.353215155]: insert Depth to Stereo Module
[ INFO] [1603940119.353394551]: insert Color to RGB Camera
[ INFO] [1603940119.353568398]: insert Infrared to Stereo Module
[ INFO] [1603940119.353681115]: insert Infrared to Stereo Module
[ INFO] [1603940119.353802336]: insert Gyro to Motion Module
[ INFO] [1603940119.353923010]: insert Accel to Motion Module
 29/10 10:55:19,360 WARNING [140357078886144] (backend-hid.cpp:681) HID set_power 1 failed for /sys/devices/pci0000:00/0000:00:14.0/usb3/3-1/3-1:1.5/0003:8086:0B3A.0005/HID-SENSOR-200073.2.auto/iio:device0/buffer/enable
[ INFO] [1603940119.371776721]: SELECTED BASE:Depth, 0
 29/10 10:55:19,433 WARNING [140357062100736] (sensor.cpp:970) HID timestamp not found, switching to Host timestamps.
[ INFO] [1603940119.436948256]: RealSense Node Is Up!
[ WARN] [1603940120.201546779]: Hardware Notification:Motion Module failure,1.60394e+12,Error,Hardware Error

Additional files: Please attach all the files needed to reproduce the error.

changed files.zip

Please give also the following information:

  • SparkVio branch, tag or commit used:
  • GTSAM version used:
  • OpenGV version used:
  • OpenCV version used: type `opencv_version 3.4.2
  • Operating system and version (e.g. Ubuntu 16.04 or Windows 10): 18.04
  • Did you change the source code? (yes / no): yes
  • Kimera Vio: 40fff93f0b21f645e1386fd77875c8113cbabcc0 Merge pull request #100 from Wallbraker/opencv-4-and-warning-fixes KimeraVioRos:afeb7ec80945da0c8ed6a7edea4f6f5ecd644743 Merge pull request #101 from MIT-SPARK/fix/pragmas opencv3_catkin: 880da1510aa80a247b80adca32ac2941ddfa94ff Merge pull request #27 from ethz-asl/fix/unordered_map_flag_rename
skiddattle commented 3 years ago

Hi, I did a rostopic echo camera/imu and found out the imu messages from the d435i camera freezes from time to time. So to rectify that, I lowered the fps for gyro and accel in rs_camera.launch to 200 and 63 fps, the fixed fps rate found out using rs-enumerate-devices. Based on that, I now no longer get freezing output.

I would now just like to verify if the output I am getting now if its correct? Because the representation I am getting is quite different from playing back a rosbag file.

Here is a link to the youtube video: https://youtu.be/J9qtbiWoBMs

Here is an image of the file: 2020-10-30