rtabmap_ros icon indicating copy to clipboard operation
rtabmap_ros copied to clipboard
verified publisher icon introlab

3D map didn't come out on rtabmap GUI

Open Jenanaputra opened this issue 1 year ago • 9 comments

Hi Guys ....

I am using ROS2 Humble. I am trying to get the 3d map using rtabmap package that is provided in ros2, but without using scan data , and statically set the odom to 0,0,0 as the program runs. The scenarios of my testing are :

  1. I am working with depthai camera. I published the RGB camera data (/camera/image_raw) whose size (1280x720), depth camera data (/camera/depth/image_raw) whose size (1280x720), and camera info.
  2. I statically publish the odometry by using ros2 topic pub
  3. Run the rtabmap package : ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=false approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2

After I run those steps above, nothing came up but I got the logging on my terminal :

[INFO] [launch]: All log files can be found below /home/jenanaputra/.ros/log/2024-02-21-14-26-31-965619-jenanaputra-Predator-PH315-55-39897
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rgbd_sync-1]: process started with pid [39898]
[INFO] [rtabmap-2]: process started with pid [39900]
[INFO] [rtabmap_viz-3]: process started with pid [39902]
[rgbd_sync-1] [INFO] [1708496792.308280928] [rtabmap.rgbd_sync]: rgbd_sync: approx_sync = false
[rgbd_sync-1] [INFO] [1708496792.308801342] [rtabmap.rgbd_sync]: rgbd_sync: queue_size  = 10
[rgbd_sync-1] [INFO] [1708496792.309051503] [rtabmap.rgbd_sync]: rgbd_sync: qos         = 2
[rgbd_sync-1] [INFO] [1708496792.309204622] [rtabmap.rgbd_sync]: rgbd_sync: qos_camera_info = 2
[rgbd_sync-1] [INFO] [1708496792.309348746] [rtabmap.rgbd_sync]: rgbd_sync: depth_scale = 1.000000
[rgbd_sync-1] [INFO] [1708496792.309502593] [rtabmap.rgbd_sync]: rgbd_sync: decimation = 1
[rgbd_sync-1] [INFO] [1708496792.309636838] [rtabmap.rgbd_sync]: rgbd_sync: compressed_rate = 0.000000
[rgbd_sync-1] [INFO] [1708496792.318867316] [rtabmap.rgbd_sync]: 
[rgbd_sync-1] rgbd_sync subscribed to (exact sync):
[rgbd_sync-1]    /camera/image_raw,
[rgbd_sync-1]    /camera/depth/image_raw,
[rgbd_sync-1]    /camera/camera_info
[rtabmap-2] [INFO] [1708496792.359519526] [rtabmap.rtabmap]: rtabmap(maps): latch                      = true
[rtabmap-2] [INFO] [1708496792.359712402] [rtabmap.rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[rtabmap-2] [INFO] [1708496792.359736849] [rtabmap.rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[rtabmap-2] [INFO] [1708496792.359749204] [rtabmap.rtabmap]: rtabmap(maps): map_cleanup                = true
[rtabmap-2] [INFO] [1708496792.359758804] [rtabmap.rtabmap]: rtabmap(maps): map_always_update          = false
[rtabmap-2] [INFO] [1708496792.359767739] [rtabmap.rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[rtabmap-2] [INFO] [1708496792.359776599] [rtabmap.rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[rtabmap-2] [INFO] [1708496792.359785309] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[rtabmap-2] [INFO] [1708496792.359794014] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rtabmap-2] [INFO] [1708496792.359843726] [rtabmap.rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[rtabmap-2] [INFO] [1708496792.376234422] [rtabmap.rtabmap]: rtabmap: frame_id      = base_footprint
[rtabmap-2] [INFO] [1708496792.376255314] [rtabmap.rtabmap]: rtabmap: map_frame_id  = map
[rtabmap-2] [INFO] [1708496792.376259805] [rtabmap.rtabmap]: rtabmap: log_to_rosout_level  = 4
[rtabmap-2] [INFO] [1708496792.376263645] [rtabmap.rtabmap]: rtabmap: initial_pose  = 
[rtabmap-2] [INFO] [1708496792.376266989] [rtabmap.rtabmap]: rtabmap: use_action_for_goal  = false
[rtabmap-2] [INFO] [1708496792.376271587] [rtabmap.rtabmap]: rtabmap: tf_delay      = 0.050000
[rtabmap-2] [INFO] [1708496792.376284653] [rtabmap.rtabmap]: rtabmap: tf_tolerance  = 0.100000
[rtabmap-2] [INFO] [1708496792.376288416] [rtabmap.rtabmap]: rtabmap: odom_sensor_sync   = false
[rtabmap-2] [INFO] [1708496792.376291659] [rtabmap.rtabmap]: rtabmap: pub_loc_pose_only_when_localizing = false
[rtabmap-2] [INFO] [1708496792.376296205] [rtabmap.rtabmap]: rtabmap: gen_scan  = false
[rtabmap-2] [INFO] [1708496792.376299515] [rtabmap.rtabmap]: rtabmap: gen_depth  = false
[rtabmap-2] [INFO] [1708496792.393006215] [rtabmap.rtabmap]: Update RTAB-Map parameter "Grid/RangeMin"="0.2" from arguments
[rtabmap-2] [INFO] [1708496792.393033110] [rtabmap.rtabmap]: Update RTAB-Map parameter "RGBD/NeighborLinkRefining"="true" from arguments
[rtabmap-2] [INFO] [1708496792.393037799] [rtabmap.rtabmap]: Update RTAB-Map parameter "Reg/Force3DoF"="true" from arguments
[rtabmap-2] [INFO] [1708496792.393041365] [rtabmap.rtabmap]: Update RTAB-Map parameter "Reg/Strategy"="1" from arguments
[rtabmap-2] [INFO] [1708496792.393299485] [rtabmap.rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[rtabmap-2] [INFO] [1708496792.393404812] [rtabmap.rtabmap]: rtabmap: Deleted database "/home/jenanaputra/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[rtabmap-2] [INFO] [1708496792.393415148] [rtabmap.rtabmap]: rtabmap: Using database from "/home/jenanaputra/.ros/rtabmap.db" (0 MB).
[rtabmap-2] [INFO] [1708496792.460935390] [rtabmap.rtabmap]: rtabmap: Database version = "0.21.3".
[rtabmap-2] [INFO] [1708496792.460978599] [rtabmap.rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[rtabmap-2] [INFO] [1708496792.479079592] [rtabmap.rtabmap]: Setup callbacks
[rtabmap-2] [WARN] [1708496792.479148314] [rtabmap.rtabmap]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.
[rtabmap-2] [INFO] [1708496792.479191677] [rtabmap.rtabmap]: rtabmap: subscribe_depth = false
[rtabmap-2] [INFO] [1708496792.479197189] [rtabmap.rtabmap]: rtabmap: subscribe_rgb = false
[rtabmap-2] [INFO] [1708496792.479200959] [rtabmap.rtabmap]: rtabmap: subscribe_stereo = false
[rtabmap-2] [INFO] [1708496792.479204608] [rtabmap.rtabmap]: rtabmap: subscribe_rgbd = true (rgbd_cameras=1)
[rtabmap-2] [INFO] [1708496792.479208553] [rtabmap.rtabmap]: rtabmap: subscribe_sensor_data = false
[rtabmap-2] [INFO] [1708496792.479211857] [rtabmap.rtabmap]: rtabmap: subscribe_odom_info = false
[rtabmap-2] [INFO] [1708496792.479214983] [rtabmap.rtabmap]: rtabmap: subscribe_user_data = false
[rtabmap-2] [INFO] [1708496792.479218049] [rtabmap.rtabmap]: rtabmap: subscribe_scan = false
[rtabmap-2] [INFO] [1708496792.479221349] [rtabmap.rtabmap]: rtabmap: subscribe_scan_cloud = false
[rtabmap-2] [INFO] [1708496792.479224656] [rtabmap.rtabmap]: rtabmap: subscribe_scan_descriptor = false
[rtabmap-2] [INFO] [1708496792.479227984] [rtabmap.rtabmap]: rtabmap: queue_size      = 10
[rtabmap-2] [INFO] [1708496792.479254717] [rtabmap.rtabmap]: rtabmap: qos_image       = 2
[rtabmap-2] [INFO] [1708496792.479258292] [rtabmap.rtabmap]: rtabmap: qos_camera_info = 2
[rtabmap-2] [INFO] [1708496792.479261451] [rtabmap.rtabmap]: rtabmap: qos_scan        = 2
[rtabmap-2] [INFO] [1708496792.479264542] [rtabmap.rtabmap]: rtabmap: qos_odom        = 2
[rtabmap-2] [INFO] [1708496792.479267727] [rtabmap.rtabmap]: rtabmap: qos_user_data   = 2
[rtabmap-2] [INFO] [1708496792.479270779] [rtabmap.rtabmap]: rtabmap: approx_sync     = true
[rtabmap-2] [INFO] [1708496792.479277949] [rtabmap.rtabmap]: Setup rgbd callback
[rtabmap-2] [INFO] [1708496792.483998112] [rtabmap.rtabmap]: 
[rtabmap-2] rtabmap subscribed to (approx sync):
[rtabmap-2]    /odom \
[rtabmap-2]    /rtabmap/rgbd_image
[rtabmap_viz-3] [INFO] [1708496792.486450453] [rtabmap.rtabmap_viz]: rtabmap_viz: Using configuration from "/home/jenanaputra/.ros/rtabmapGUI.ini"
[rtabmap_viz-3] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-3] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-3] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-3] [WARN] [1708496793.241670314] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rtabmap_viz-3] [INFO] [1708496793.244146506] [rtabmap.rtabmap_viz]: Reading parameters from the ROS server...
[rtabmap_viz-3] [INFO] [1708496793.269397853] [rtabmap.rtabmap_viz]: Parameters read = 363
[rtabmap_viz-3] [INFO] [1708496793.269431584] [rtabmap.rtabmap_viz]: Parameters successfully read.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:306   WARN| vtkOpenGLPolyDataMapper::GetVertexShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetVertexShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:298   WARN| vtkOpenGLPolyDataMapper::SetVertexShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetVertexShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:336   WARN| vtkOpenGLPolyDataMapper::GetGeometryShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetGeometryShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:328   WARN| vtkOpenGLPolyDataMapper::SetGeometryShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetGeometryShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:321   WARN| vtkOpenGLPolyDataMapper::GetFragmentShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetFragmentShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:313   WARN| vtkOpenGLPolyDataMapper::SetFragmentShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetFragmentShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:306   WARN| vtkOpenGLPolyDataMapper::GetVertexShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetVertexShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:298   WARN| vtkOpenGLPolyDataMapper::SetVertexShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetVertexShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:336   WARN| vtkOpenGLPolyDataMapper::GetGeometryShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetGeometryShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:328   WARN| vtkOpenGLPolyDataMapper::SetGeometryShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetGeometryShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:321   WARN| vtkOpenGLPolyDataMapper::GetFragmentShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::GetFragmentShaderCode instead.
[rtabmap_viz-3] 2024-02-21 14:26:33.441 (   1.144s) [        F6F1C3C0]vtkOpenGLPolyDataMapper:313   WARN| vtkOpenGLPolyDataMapper::SetFragmentShaderCode was deprecated for VTK 9.0 and will be removed in a future version.  Use vtkOpenGLShaderProperty::SetFragmentShaderCode instead.
[rtabmap_viz-3] [WARN] [1708496793.451321954] [rtabmap.rtabmap_viz]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.
[rtabmap_viz-3] [INFO] [1708496793.451398052] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_depth = false
[rtabmap_viz-3] [INFO] [1708496793.451402078] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_rgb = false
[rtabmap_viz-3] [INFO] [1708496793.451404433] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_stereo = false
[rtabmap_viz-3] [INFO] [1708496793.451406484] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_rgbd = true (rgbd_cameras=1)
[rtabmap_viz-3] [INFO] [1708496793.451409377] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_sensor_data = false
[rtabmap_viz-3] [INFO] [1708496793.451411510] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_odom_info = false
[rtabmap_viz-3] [INFO] [1708496793.451413491] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_user_data = false
[rtabmap_viz-3] [INFO] [1708496793.451415528] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan = false
[rtabmap_viz-3] [INFO] [1708496793.451417523] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan_cloud = false
[rtabmap_viz-3] [INFO] [1708496793.451419482] [rtabmap.rtabmap_viz]: rtabmap_viz: subscribe_scan_descriptor = false
[rtabmap_viz-3] [INFO] [1708496793.451421432] [rtabmap.rtabmap_viz]: rtabmap_viz: queue_size      = 10
[rtabmap_viz-3] [INFO] [1708496793.451423665] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_image       = 2
[rtabmap_viz-3] [INFO] [1708496793.451425779] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_camera_info = 2
[rtabmap_viz-3] [INFO] [1708496793.451430559] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_scan        = 2
[rtabmap_viz-3] [INFO] [1708496793.451432527] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_odom        = 2
[rtabmap_viz-3] [INFO] [1708496793.451434426] [rtabmap.rtabmap_viz]: rtabmap_viz: qos_user_data   = 2
[rtabmap_viz-3] [INFO] [1708496793.451436310] [rtabmap.rtabmap_viz]: rtabmap_viz: approx_sync     = true
[rtabmap_viz-3] [INFO] [1708496793.451442123] [rtabmap.rtabmap_viz]: Setup rgbd callback
[rtabmap_viz-3] [INFO] [1708496793.454770364] [rtabmap.rtabmap_viz]: 
[rtabmap_viz-3] rtabmap_viz subscribed to (approx sync):
[rtabmap_viz-3]    /odom \
[rtabmap_viz-3]    /rtabmap/rgbd_image
[rtabmap_viz-3] [INFO] [1708496793.456889484] [rtabmap.rtabmap_viz]: rtabmap_viz started.

I also attached you the screenshot of my rqt_graph, therefore you can help me analyze what wrong with my testing. rqt

I got stuck with this problem, Anyone maybe has their thought about this problem ?

Jenanaputra avatar Feb 21 '24 07:02 Jenanaputra

You may debug if topics are correctly published with ros2 topic hz. For example, I think /rtabmap/rgbd_image may not be published because depthai doesn't publish rgb and depth topics with exactly the same stamp. You have to use approx_rgbd_sync:=true.

Make sure the camera topics are actually published and that you publish /odom at a similar rate (or faster) than /rtabmap/rgbd_image.

matlabbe avatar Feb 24 '24 21:02 matlabbe

@matlabbe thanks for your reply, I will try this out

Jenanaputra avatar Mar 01 '24 06:03 Jenanaputra

@matlabbe , I tried to use approx_rgbd_sync:=true ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=false approx_sync:=true approx_rgbd_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2

but i got the same issue. I also noticed there is warning coming out on my terminal [rgbd_sync-1] [WARN] [1709716048.898040509] [rtabmap.rgbd_sync]: The time difference between rgb and depth frames is high (diff=0.100908s, rgb=1709716048.695479s, depth=1709716048.594570s). You may want to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use approx_sync=false if streams have all the exact same timestamp

Actually I publish the depth and rgb with the same time stamp. Do you have any thought about this ?

Jenanaputra avatar Mar 06 '24 09:03 Jenanaputra

If depth and rgb topics have same stamp, set approx_rgbd_sync:=false. Keep approx_sync:=true to sync /odom with the image topic.

Are you launching rtabmap on same computer than the camera? the 100 ms time difference is quite high, it should be at most 15 ms for images published at 30 Hz.

matlabbe avatar Mar 09 '24 21:03 matlabbe

@matlabbe still got the same issue. I even made sure the rgb data and the depth image data are published with the same time stamp by creating the new code to make sure it.

Regarding to the time difference , I launch the rtabmap in my computer, and accessed the oakd camera wireless (I dont know this can be tha cause of high time difference).

Do you have any another suggestion to try ? Is it possible because of the size of the image quite high ?

Jenanaputra avatar Mar 14 '24 07:03 Jenanaputra

@matlabbe is that possible that the issue is caused because I only use odom data, camera info data, rgb camera data, depth camera data and imu data as the input for the rtabmap package ?

Jenanaputra avatar Mar 16 '24 06:03 Jenanaputra

If you are subscribing on image topics on WIFI, check if your bandwidth is limited (that could explain large sync delay, as some images may not be received). I would also suggest to subscribe to compressed topics instead of the raw images with compressed:=true if you use rtabmap.launch.py.

For bandwidth issues, you can use ros2 topic hz on the topics from the remote computer and compare with actual frame rate if you do it on the camera's computer.

matlabbe avatar Mar 17 '24 00:03 matlabbe

@matlabbe could you tell me the command line to launch the rtabmap package with compressed image data ? I have no idea about that.

Jenanaputra avatar Mar 25 '24 09:03 Jenanaputra

ros2 launch rtabmap_launch rtabmap.launch.py compressed:=true ...

By default for RGB/stereo images, compressed suffix is used, for depth image, it is compressedDepth. You can change which image_transport plugin you want to use with rgb_image_transport and depth_image_transport arguments respectively.

matlabbe avatar Mar 31 '24 18:03 matlabbe