search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.61k stars 763 forks source link

Custom VIO into Rtabmap #1181

Open Genozen opened 6 months ago

Genozen commented 6 months ago

Hello, I've managed to load OpenVins with my D435i and have calibrated properly so that the VIO runs reasonably well in ROS 2 Humble.

I would now like to send the VIO topics to Rtabmap such that it can take advantage of the loop-closure and mapping techniques.

What are some steps and things to consider in order to achieve this?

Current OpenVins output topic:

/camera/d435i/accel/imu_info
/camera/d435i/accel/metadata
/camera/d435i/accel/sample
/camera/d435i/color/camera_info
/camera/d435i/color/image_raw
/camera/d435i/color/metadata
/camera/d435i/depth/camera_info
/camera/d435i/depth/image_rect_raw
/camera/d435i/depth/metadata
/camera/d435i/extrinsics/depth_to_accel
/camera/d435i/extrinsics/depth_to_color
/camera/d435i/extrinsics/depth_to_depth
/camera/d435i/extrinsics/depth_to_gyro
/camera/d435i/extrinsics/depth_to_infra1
/camera/d435i/extrinsics/depth_to_infra2
/camera/d435i/gyro/imu_info
/camera/d435i/gyro/metadata
/camera/d435i/gyro/sample
/camera/d435i/imu
/camera/d435i/infra1/camera_info
/camera/d435i/infra1/image_rect_raw
/camera/d435i/infra1/metadata
/camera/d435i/infra2/camera_info
/camera/d435i/infra2/image_rect_raw
/camera/d435i/infra2/metadata
/clicked_point
/initialpose
/move_base_simple/goal
/ov_msckf/loop_depth
/ov_msckf/loop_depth_colored
/ov_msckf/loop_extrinsic
/ov_msckf/loop_feats
/ov_msckf/loop_intrinsics
/ov_msckf/loop_pose
/ov_msckf/odomimu
/ov_msckf/pathgt
/ov_msckf/pathimu
/ov_msckf/points_aruco
/ov_msckf/points_msckf
/ov_msckf/points_sim
/ov_msckf/points_slam
/ov_msckf/posegt
/ov_msckf/poseimu
/ov_msckf/trackhist
/parameter_events
/rosout
/tf
/tf_static
matlabbe commented 6 months ago

Based on my comment on similar question: https://github.com/introlab/rtabmap_ros/issues/1084#issuecomment-1856752838

What is /ov_msckf/odomimu, is it an Odometry msg?

ros2 run rtabmap_slam rtabmap --ros-args -r odom:=/ov_msckf/odomimu ...
Genozen commented 5 months ago

Hi, sorry for the duplication on the question (thought my message didn't go through).

And yes, the /odomimu is a Odometry msg

ros2 topic info /ov_msckf/odomimu 
Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0

Would this change anything to your initial suggestion?

I will try it in a couple hours.

Genozen commented 5 months ago

@matlabbe

I've remapped some of the topics to match the Openvins one, using the example you gave:

ros2 launch rtabmap_launch rtabmap.launch.py \
visual_odometry:=false \
odom_topic:=/ov_msckf/odomimu \
rgb_topic:=/camera/d435i/color/image_raw \
depth_topic:=/camera/d435i/depth/image_rect_raw \
camera_info_topic:=/camera/d435i/color/camera_info \

but got a warning message and no image is showing in Rtab:

[rtabmap_viz-2] [WARN] [1705445325.096275150] [rtabmap.rtabmap_viz]: rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap_viz-2] rtabmap_viz subscribed to (exact sync):
[rtabmap_viz-2]    /ov_msckf/odomimu \
[rtabmap_viz-2]    /camera/d435i/color/image_raw \
[rtabmap_viz-2]    /camera/d435i/depth/image_rect_raw \
[rtabmap_viz-2]    /camera/d435i/color/camera_info

I would like to mainly prove out the concept that I can utilize OpenVins' VIO and coupled with Rtabmap to perform loop closure.

I think doing it in the infra1 and infra2 frame might be better? I see there's a D435i launch example that uses the two infra camera img...

Genozen commented 5 months ago

@borongyuan Hello, I see you have a PR that was merged regarding OpenVins + Rtabmap. I would love to test this out but am too novice to this. Any chance you could shed some light on how to run your version of the commit? I have a standalone OpenVins and have just properly calibrated my D435i... OV_MSCKF works on a real camera as you did, but am missing the Rtab integration.

borongyuan commented 5 months ago

If you have an OAK camera, you can try it first. I haven't used RealSense in a long time, so I'm not sure how well it performs. You can first try running RTAB-Map standalone without ROS. The camera driver will parse factory calibration, so you don't need to recalibrate.

matlabbe commented 5 months ago

@Genozen for this warning:

[rtabmap_viz-2] [WARN] [1705445325.096275150] [rtabmap.rtabmap_viz]: rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap_viz-2] rtabmap_viz subscribed to (exact sync):
[rtabmap_viz-2]    /ov_msckf/odomimu \
[rtabmap_viz-2]    /camera/d435i/color/image_raw \
[rtabmap_viz-2]    /camera/d435i/depth/image_rect_raw \
[rtabmap_viz-2]    /camera/d435i/color/camera_info

Two options:

  1. If all those topics are published as expected (verify with rostopic hz), you would need to use rtabmap_sync/rgbd_sync node to exactly synchronize (with approx_sync:=false) your camera topics together, then setup rtabmap and rtabmap_viz nodes to use approx_sync:=true and subscribe_rgbd:=true. The nodes will then to try to sync rgbd_image topic (output of rgbd_sync) with /ov_msckf/odomimu.
  2. set odom_frame_id to frame used by the VIO node as base frame (the frame that would be set as frame_id inside /ov_msckf/odomimu). The downside is that it will ignore VIO covariance data.
Genozen commented 5 months ago

Thanks @borongyuan! I do have an Oak-D Pro W unopened yet (might be IMX378, but I'll try to get the OV9782), is there a recommended config or topic remapping I should do before I do the ros2 launch rtabmap_launch rtabmap_launch.py ?

@matlabbe

Thank you for the suggestions. I went ahead and tried both options.

Option 1 procedure:

ros2 launch realsense2_camera rs_launch.py enable_accel:=true enable_infra1:=true enable_infra2:=true enable_fisheye1:=true enable_fisheye2:=true enable_gyro:=true unite_imu_method:=2 depth_module.profile:=640x480x90 camera_name:=d435i ros2 launch ov_msckf subscribe.launch.py config:=rs_d455 # modified for d435i

when running rgbd_sync node, I got:

ros2 run rtabmap_sync rgbd_sync --ros-args -r /rgb/image:=/camera/d435i/color/image_raw -r /depth/image:=/camera/d435i/depth/image_rect_raw -r /rgb/camera_info:=/camera/d435i/color/camera_info -p approx_sync:=false

[INFO] [1706077938.997277406] [rgbd_sync]: rgbd_sync: approx_sync = false
[INFO] [1706077938.997444061] [rgbd_sync]: rgbd_sync: queue_size  = 10
[INFO] [1706077938.997461243] [rgbd_sync]: rgbd_sync: qos         = 0
[INFO] [1706077938.997471721] [rgbd_sync]: rgbd_sync: qos_camera_info = 0
[INFO] [1706077938.997482337] [rgbd_sync]: rgbd_sync: depth_scale = 1.000000
[INFO] [1706077938.997501755] [rgbd_sync]: rgbd_sync: decimation = 1
[INFO] [1706077938.997512302] [rgbd_sync]: rgbd_sync: compressed_rate = 0.000000
[INFO] [1706077939.007935808] [rgbd_sync]: 
rgbd_sync subscribed to (exact sync):
   /camera/d435i/color/image_raw,
   /camera/d435i/depth/image_rect_raw,
   /camera/d435i/color/camera_info
[WARN] [1706077943.009976676] [rgbd_sync]: rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
rgbd_sync subscribed to (exact sync):
   /camera/d435i/color/image_raw,
   /camera/d435i/depth/image_rect_raw,
   /camera/d435i/color/camera_info
[WARN] [1706077948.010371528] [rgbd_sync]: rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.

I have verified that the topics are published and updating

   /camera/d435i/color/image_raw, (@ 30 hz)
   /camera/d435i/depth/image_rect_raw, (@ 90 hz)
   /camera/d435i/color/camera_info (@ 30 hz)

Despite the warning, I proceeded to run: ros2 run rtabmap_slam rtabmap --ros-args -r odom:=/ov_msckf/odomimu -r /depth/image:=/camera/d435i/depth/image_rect_raw -r /rgb/image:=/camera/d435i/color/image_raw -r /rgb/camera_info:=/camera/d435i/camera_info -p approx_sync:=true -p subscribe_rgbd:=true

and checked that /rgbd_image topic isn't published due to the error/warning in rgbd_sync step...

frame is as follow: frames_2024-01-24_00.47.27.pdf

As for option 2:

Got similar warnings as well:

[INFO] [launch]: All log files can be found below /home/ascent/.ros/log/2024-01-24-01-01-44-515263-ascent-1083198
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rtabmap-1]: process started with pid [1083264]
[INFO] [rtabmap_viz-2]: process started with pid [1083266]
[rtabmap-1] [INFO] [1706079705.130180837] [rtabmap.rtabmap]: rtabmap(maps): latch                      = true
[rtabmap-1] [INFO] [1706079705.130470394] [rtabmap.rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[rtabmap-1] [INFO] [1706079705.130508526] [rtabmap.rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[rtabmap-1] [INFO] [1706079705.130526056] [rtabmap.rtabmap]: rtabmap(maps): map_cleanup                = true
[rtabmap-1] [INFO] [1706079705.130539535] [rtabmap.rtabmap]: rtabmap(maps): map_always_update          = false
[rtabmap-1] [INFO] [1706079705.130551269] [rtabmap.rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[rtabmap-1] [INFO] [1706079705.130564119] [rtabmap.rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[rtabmap-1] [INFO] [1706079705.130578227] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[rtabmap-1] [INFO] [1706079705.130590519] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rtabmap-1] [INFO] [1706079705.130643527] [rtabmap.rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[rtabmap-1] [INFO] [1706079705.148258180] [rtabmap.rtabmap]: rtabmap: frame_id      = base_link
[rtabmap-1] [INFO] [1706079705.148316567] [rtabmap.rtabmap]: rtabmap: odom_frame_id = global
[rtabmap-1] [INFO] [1706079705.148330954] [rtabmap.rtabmap]: rtabmap: map_frame_id  = map
[rtabmap-1] [INFO] [1706079705.148342338] [rtabmap.rtabmap]: rtabmap: log_to_rosout_level  = 4
[rtabmap-1] [INFO] [1706079705.148355258] [rtabmap.rtabmap]: rtabmap: initial_pose  = 
[rtabmap-1] [INFO] [1706079705.148367131] [rtabmap.rtabmap]: rtabmap: use_action_for_goal  = false
[rtabmap-1] [INFO] [1706079705.148379982] [rtabmap.rtabmap]: rtabmap: tf_delay      = 0.050000
[rtabmap-1] [INFO] [1706079705.148395835] [rtabmap.rtabmap]: rtabmap: tf_tolerance  = 0.100000
[rtabmap-1] [INFO] [1706079705.148409105] [rtabmap.rtabmap]: rtabmap: odom_sensor_sync   = false
[rtabmap-1] [INFO] [1706079705.148421117] [rtabmap.rtabmap]: rtabmap: pub_loc_pose_only_when_localizing = false
[rtabmap-1] [INFO] [1706079705.148433409] [rtabmap.rtabmap]: rtabmap: gen_scan  = false
[rtabmap-1] [INFO] [1706079705.148449193] [rtabmap.rtabmap]: rtabmap: gen_depth  = false
[rtabmap-1] [INFO] [1706079705.170141251] [rtabmap.rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[rtabmap-1] [INFO] [1706079705.170248037] [rtabmap.rtabmap]: rtabmap: Using database from "/home/ascent/.ros/rtabmap.db" (0 MB).
[rtabmap-1] [INFO] [1706079705.184449953] [rtabmap.rtabmap]: rtabmap: Database version = "0.21.4".
[rtabmap-1] [INFO] [1706079705.184506663] [rtabmap.rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[rtabmap-1] [INFO] [1706079705.221540079] [rtabmap.rtabmap]: Setup callbacks
[rtabmap-1] [INFO] [1706079705.221694076] [rtabmap.rtabmap]: rtabmap: subscribe_depth = true
[rtabmap-1] [INFO] [1706079705.221720336] [rtabmap.rtabmap]: rtabmap: subscribe_rgb = true
[rtabmap-1] [INFO] [1706079705.221735701] [rtabmap.rtabmap]: rtabmap: subscribe_stereo = false
[rtabmap-1] [INFO] [1706079705.221748551] [rtabmap.rtabmap]: rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[rtabmap-1] [INFO] [1706079705.221760424] [rtabmap.rtabmap]: rtabmap: subscribe_sensor_data = false
[rtabmap-1] [INFO] [1706079705.221771459] [rtabmap.rtabmap]: rtabmap: subscribe_odom_info = false
[rtabmap-1] [INFO] [1706079705.221781865] [rtabmap.rtabmap]: rtabmap: subscribe_user_data = false
[rtabmap-1] [INFO] [1706079705.221791922] [rtabmap.rtabmap]: rtabmap: subscribe_scan = false
[rtabmap-1] [INFO] [1706079705.221802468] [rtabmap.rtabmap]: rtabmap: subscribe_scan_cloud = false
[rtabmap-1] [INFO] [1706079705.221811966] [rtabmap.rtabmap]: rtabmap: subscribe_scan_descriptor = false
[rtabmap-1] [INFO] [1706079705.221822442] [rtabmap.rtabmap]: rtabmap: queue_size      = 10
[rtabmap-1] [INFO] [1706079705.221832569] [rtabmap.rtabmap]: rtabmap: qos_image       = 1
[rtabmap-1] [INFO] [1706079705.221841928] [rtabmap.rtabmap]: rtabmap: qos_camera_info = 1
[rtabmap-1] [INFO] [1706079705.221851985] [rtabmap.rtabmap]: rtabmap: qos_scan        = 1
[rtabmap-1] [INFO] [1706079705.221862111] [rtabmap.rtabmap]: rtabmap: qos_odom        = 1
[rtabmap-1] [INFO] [1706079705.221872448] [rtabmap.rtabmap]: rtabmap: qos_user_data   = 1
[rtabmap-1] [INFO] [1706079705.221880549] [rtabmap.rtabmap]: rtabmap: approx_sync     = false
[rtabmap-1] [INFO] [1706079705.221900663] [rtabmap.rtabmap]: Setup depth callback
[rtabmap-1] [INFO] [1706079705.229038462] [rtabmap.rtabmap]: 
[rtabmap-1] rtabmap subscribed to (exact sync):
[rtabmap-1]    /camera/d435i/color/image_raw \
[rtabmap-1]    /camera/d435i/depth/image_rect_raw \
[rtabmap-1]    /camera/d435i/color/camera_info
[rtabmap_viz-2] [INFO] [1706079705.287294820] [rtabmap.rtabmap_viz]: rtabmap_viz: Using configuration from "/home/ascent/.ros/rtabmapGUI.ini"
[rtabmap_viz-2] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-2] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-2] libpng warning: iCCP: known incorrect sRGB profile
[rtabmap_viz-2] [WARN] [1706079706.250494736] [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-2] [INFO] [1706079706.255069194] [rtabmap.rtabmap_viz]: Reading parameters from the ROS server...
[rtabmap_viz-2] [INFO] [1706079706.306582908] [rtabmap.rtabmap_viz]: Parameters read = 364
[rtabmap_viz-2] [INFO] [1706079706.306637243] [rtabmap.rtabmap_viz]: Parameters successfully read.

[rtabmap-1] [WARN] [1706079725.232054349] [rtabmap.rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap-1] rtabmap subscribed to (exact sync):
[rtabmap-1]    /camera/d435i/color/image_raw \
[rtabmap-1]    /camera/d435i/depth/image_rect_raw \
[rtabmap-1]    /camera/d435i/color/camera_info

D435i Example Launch Re-try

I then decided to run the basic rtabmap d435i launch examples, and noticed all the launch examples gives me similar warning messages despite topics are matched/published... (these example launch code (rtabmap_d435i_infra.py & rtabmap_d435i_stereo.py were successful last year without a problem)

Genozen commented 5 months ago

Good news, somehow I got the /rgbd_image to publish now ~11 to 15 Hz then slowly ramps up to 60 Hz... and no more warning messages. (need to reproduce the steps again). I think it's related to my D435i launch parameters, where I need to enable align_depth

image

The rtabmap_viz seems to update very slow though, any suggestions as to why that is? similar to this issue: https://github.com/introlab/rtabmap_ros/issues/1106 -> slightly confused by the comment :

Note that if odometry topic is not subscribed to rtabmap_viz, you will only see the map updates (at 1Hz)

[INFO] [1706084060.369871073] [rtabmap]: rtabmap (604): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0936s, Maps update=0.0001s pub=0.0012s (local map=154, WM=154)
[ WARN] (2024-01-24 02:14:20.594) MsgConversion.cpp:1989::getTransform() (can transform global -> base_link?) Could not find a connection between 'global' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.204357 timeout was 0.2. (wait_for_transform=0.200000)
[ WARN] (2024-01-24 02:14:20.797) MsgConversion.cpp:1989::getTransform() (can transform global -> base_link?) Could not find a connection between 'global' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.202329 timeout was 0.2. (wait_for_transform=0.200000)

Increased the queue_size to 90 suggested from rtabmap_ros #978 https://github.com/introlab/rtabmap_ros/issues/978. Still very slow.

All input topics are at 60 Hz.

Frame results (there's also a disconnected TF in here...): frames_2024-01-24_02.35.07.pdf

matlabbe commented 5 months ago

What do you mean it is slow? are you seeing only 1 Hz update rate? or is it faster sometimes and lag other time?

You should also try to fix the tf tree, as canTransform returned after 0.204357 timeout was 0.2. will always add a 200 ms delay every time the Tf tree is looked at.

Genozen commented 4 months ago

@matlabbe. Just got back to working on it.

The Rtabmap is indeed reporting 1 Hz. (The synced node, rgbd_image is around 3~5 Hz).

I've added a manual tf topic to link the frames.

code executed: ros2 run rtabmap_sync rgbd_sync --ros-args -r /rgb/image:=/camera/d435i/color/image_raw -r /depth/image:=/camera/d435i/depth/image_rect_raw -r /rgb/camera_info:=/camera/d435i/color/camera_info -p approx_sync:=false

ros2 run rtabmap_slam rtabmap --ros-args -r odom:=/ov_msckf/odomimu -r /depth/image:=/camera/d435i/depth/image_rect_raw -r /rgb/image:=/camera/d435i/color/image_raw -r /rgb/camera_info:=/camera/d435i/camera_info -p approx_sync:=true -p subscribe_rgbd:=true -p frame_id:="d435i_link"

There's also a g2o related issue that might not be related to contributing the slowness of rtabmap

[INFO] [1707505685.195324591] [rtabmap]: rtabmap(maps): latch                      = true
[INFO] [1707505685.195665228] [rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[INFO] [1707505685.195699111] [rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[INFO] [1707505685.195716058] [rtabmap]: rtabmap(maps): map_cleanup                = true
[INFO] [1707505685.195731333] [rtabmap]: rtabmap(maps): map_always_update          = false
[INFO] [1707505685.195743214] [rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[INFO] [1707505685.195754468] [rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[INFO] [1707505685.195765399] [rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[INFO] [1707505685.195776294] [rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[INFO] [1707505685.195854403] [rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[INFO] [1707505685.221103171] [rtabmap]: rtabmap: frame_id      = d435i_link
[INFO] [1707505685.221153846] [rtabmap]: rtabmap: map_frame_id  = map
[INFO] [1707505685.221164050] [rtabmap]: rtabmap: log_to_rosout_level  = 4
[INFO] [1707505685.221172713] [rtabmap]: rtabmap: initial_pose  = 
[INFO] [1707505685.221182062] [rtabmap]: rtabmap: use_action_for_goal  = false
[INFO] [1707505685.221190412] [rtabmap]: rtabmap: tf_delay      = 0.050000
[INFO] [1707505685.221202767] [rtabmap]: rtabmap: tf_tolerance  = 0.100000
[INFO] [1707505685.221210896] [rtabmap]: rtabmap: odom_sensor_sync   = false
[INFO] [1707505685.221218388] [rtabmap]: rtabmap: pub_loc_pose_only_when_localizing = false
[INFO] [1707505685.221226271] [rtabmap]: rtabmap: gen_scan  = false
[INFO] [1707505685.221233731] [rtabmap]: rtabmap: gen_depth  = false
[INFO] [1707505685.353250273] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[INFO] [1707505685.353386668] [rtabmap]: rtabmap: Using database from "/home/kishore/kishorenukala/.ros/rtabmap.db" (0 MB).
[INFO] [1707505685.372638490] [rtabmap]: rtabmap: Database version = "0.21.4".
[INFO] [1707505685.372815588] [rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[INFO] [1707505685.399605817] [rtabmap]: Setup callbacks
[WARN] [1707505685.399715832] [rtabmap]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.
[INFO] [1707505685.399824459] [rtabmap]: rtabmap: subscribe_depth = false
[INFO] [1707505685.399849595] [rtabmap]: rtabmap: subscribe_rgb = false
[INFO] [1707505685.399866216] [rtabmap]: rtabmap: subscribe_stereo = false
[INFO] [1707505685.399880158] [rtabmap]: rtabmap: subscribe_rgbd = true (rgbd_cameras=1)
[INFO] [1707505685.399895624] [rtabmap]: rtabmap: subscribe_sensor_data = false
[INFO] [1707505685.399909033] [rtabmap]: rtabmap: subscribe_odom_info = false
[INFO] [1707505685.399922833] [rtabmap]: rtabmap: subscribe_user_data = false
[INFO] [1707505685.399935344] [rtabmap]: rtabmap: subscribe_scan = false
[INFO] [1707505685.399947196] [rtabmap]: rtabmap: subscribe_scan_cloud = false
[INFO] [1707505685.399959215] [rtabmap]: rtabmap: subscribe_scan_descriptor = false
[INFO] [1707505685.399971328] [rtabmap]: rtabmap: queue_size      = 10
[INFO] [1707505685.399983224] [rtabmap]: rtabmap: qos_image       = 0
[INFO] [1707505685.399995144] [rtabmap]: rtabmap: qos_camera_info = 0
[INFO] [1707505685.400006427] [rtabmap]: rtabmap: qos_scan        = 0
[INFO] [1707505685.400017970] [rtabmap]: rtabmap: qos_odom        = 0
[INFO] [1707505685.400029566] [rtabmap]: rtabmap: qos_user_data   = 0
[INFO] [1707505685.400040777] [rtabmap]: rtabmap: approx_sync     = true
[INFO] [1707505685.400065318] [rtabmap]: Setup rgbd callback
[INFO] [1707505685.401936115] [rtabmap]: 
rtabmap subscribed to (approx sync):
   /ov_msckf/odomimu \
   /rgbd_image
[INFO] [1707505685.562125497] [rtabmap]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0094s, RTAB-Map=0.0432s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505686.554623626] [rtabmap]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0008s, RTAB-Map=0.0464s, Maps update=0.0000s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505687.592417314] [rtabmap]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0476s, Maps update=0.0000s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505688.592018329] [rtabmap]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0476s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505689.584141384] [rtabmap]: rtabmap (5): Rate=1.00s, Limit=0.000s, Conversion=0.0013s, RTAB-Map=0.0381s, Maps update=0.0000s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505690.582588176] [rtabmap]: rtabmap (6): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0371s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505691.593864401] [rtabmap]: rtabmap (7): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0466s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505692.689713373] [rtabmap]: rtabmap (8): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0456s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505693.727124208] [rtabmap]: rtabmap (9): Rate=1.00s, Limit=0.000s, Conversion=0.0003s, RTAB-Map=0.0485s, Maps update=0.0001s pub=0.0000s (local map=1, WM=1)
[INFO] [1707505694.740371478] [rtabmap]: rtabmap (10): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0578s, Maps update=0.0001s pub=0.0001s (local map=1, WM=1)
[INFO] [1707505695.922332022] [rtabmap]: rtabmap (11): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0685s, Maps update=0.0001s pub=0.0026s (local map=1, WM=1)
[ WARN] (2024-02-09 13:08:16.904) Rtabmap.cpp:4462::process() Republishing data of requested node(s) 1 (Rtabmap/MaxRepublished=2)
[INFO] [1707505696.908058129] [rtabmap]: rtabmap (12): Rate=1.00s, Limit=0.000s, Conversion=0.0018s, RTAB-Map=0.0541s, Maps update=0.0001s pub=0.0028s (local map=1, WM=1)
[INFO] [1707505697.903082479] [rtabmap]: rtabmap (13): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0481s, Maps update=0.0001s pub=0.0015s (local map=1, WM=1)
[INFO] [1707505698.916453396] [rtabmap]: rtabmap (14): Rate=1.00s, Limit=0.000s, Conversion=0.0008s, RTAB-Map=0.0637s, Maps update=0.0001s pub=0.0025s (local map=1, WM=1)
[INFO] [1707505699.928227793] [rtabmap]: rtabmap (15): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0702s, Maps update=0.0001s pub=0.0018s (local map=1, WM=1)
[INFO] [1707505701.064866389] [rtabmap]: rtabmap (16): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0455s, Maps update=0.0001s pub=0.0013s (local map=1, WM=1)
[INFO] [1707505702.075174968] [rtabmap]: rtabmap (17): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0544s, Maps update=0.0001s pub=0.0013s (local map=1, WM=1)
[INFO] [1707505703.096648796] [rtabmap]: rtabmap (18): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0417s, Maps update=0.0001s pub=0.0012s (local map=1, WM=1)
[INFO] [1707505704.203869391] [rtabmap]: rtabmap (19): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0472s, Maps update=0.0000s pub=0.0010s (local map=1, WM=1)
[INFO] [1707505705.208658253] [rtabmap]: rtabmap (20): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0541s, Maps update=0.0001s pub=0.0014s (local map=1, WM=1)
[INFO] [1707505706.204722933] [rtabmap]: rtabmap (21): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0479s, Maps update=0.0000s pub=0.0012s (local map=2, WM=2)
[INFO] [1707505707.212280411] [rtabmap]: rtabmap (22): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0570s, Maps update=0.0000s pub=0.0008s (local map=3, WM=3)
[INFO] [1707505708.277746796] [rtabmap]: rtabmap (23): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0524s, Maps update=0.0001s pub=0.0009s (local map=3, WM=3)
[INFO] [1707505709.280383150] [rtabmap]: rtabmap (24): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0548s, Maps update=0.0000s pub=0.0008s (local map=4, WM=4)
[INFO] [1707505710.603150416] [rtabmap]: rtabmap (25): Rate=1.00s, Limit=0.000s, Conversion=0.0008s, RTAB-Map=0.0450s, Maps update=0.0000s pub=0.0014s (local map=5, WM=5)
[INFO] [1707505711.924460998] [rtabmap]: rtabmap (26): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0637s, Maps update=0.0001s pub=0.0015s (local map=5, WM=5)
[INFO] [1707505712.929807328] [rtabmap]: rtabmap (27): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0362s, Maps update=0.0000s pub=0.0007s (local map=6, WM=6)
[INFO] [1707505713.934577255] [rtabmap]: rtabmap (28): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0384s, Maps update=0.0001s pub=0.0013s (local map=6, WM=6)
[INFO] [1707505714.991960606] [rtabmap]: rtabmap (29): Rate=1.00s, Limit=0.000s, Conversion=0.0005s, RTAB-Map=0.0637s, Maps update=0.0000s pub=0.0015s (local map=7, WM=7)
[INFO] [1707505716.024447526] [rtabmap]: rtabmap (30): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0888s, Maps update=0.0001s pub=0.0021s (local map=7, WM=7)
[INFO] [1707505717.036923455] [rtabmap]: rtabmap (31): Rate=1.00s, Limit=0.000s, Conversion=0.0003s, RTAB-Map=0.0720s, Maps update=0.0001s pub=0.0012s (local map=7, WM=7)
[INFO] [1707505718.038298586] [rtabmap]: rtabmap (32): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0708s, Maps update=0.0002s pub=0.0018s (local map=7, WM=7)
[INFO] [1707505719.119808928] [rtabmap]: rtabmap (33): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0543s, Maps update=0.0001s pub=0.0016s (local map=7, WM=7)
[INFO] [1707505720.291593986] [rtabmap]: rtabmap (34): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0601s, Maps update=0.0000s pub=0.0010s (local map=8, WM=8)
[INFO] [1707505721.371183728] [rtabmap]: rtabmap (35): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0698s, Maps update=0.0000s pub=0.0010s (local map=9, WM=9)
[INFO] [1707505722.602775014] [rtabmap]: rtabmap (36): Rate=1.00s, Limit=0.000s, Conversion=0.0004s, RTAB-Map=0.0678s, Maps update=0.0000s pub=0.0018s (local map=10, WM=10)
[INFO] [1707505723.625189262] [rtabmap]: rtabmap (37): Rate=1.00s, Limit=0.000s, Conversion=0.0014s, RTAB-Map=0.0857s, Maps update=0.0001s pub=0.0015s (local map=10, WM=10)
[INFO] [1707505724.624007833] [rtabmap]: rtabmap (38): Rate=1.00s, Limit=0.000s, Conversion=0.0011s, RTAB-Map=0.0812s, Maps update=0.0000s pub=0.0019s (local map=11, WM=11)
[INFO] [1707505725.747618035] [rtabmap]: rtabmap (39): Rate=1.00s, Limit=0.000s, Conversion=0.0014s, RTAB-Map=0.1673s, Maps update=0.0001s pub=0.0012s (local map=12, WM=12)
[INFO] [1707505726.911090844] [rtabmap]: rtabmap (40): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.1061s, Maps update=0.0000s pub=0.0015s (local map=13, WM=13)
[FATAL] (2024-02-09 13:08:47.964) OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met!
[FATAL] [1707505727.964122881] [rtabmap]: [FATAL] (2024-02-09 13:08:47.964) OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2024-02-09 13:08:47.964) OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met!
[ros2run]: Aborted
matlabbe commented 4 months ago

Is it easy to reproduce? maybe sharing a rosbag of the input topics to rtabmap node could help to debug here.

Your rtabmap command could be simpler:

ros2 run rtabmap_slam rtabmap --ros-args \
   -r odom:=/ov_msckf/odomimu \
   -p approx_sync:=true \
   -p subscribe_rgbd:=true \
   -p frame_id:="d435i_link"
ros2 bag record /tf /tf_static  /ov_msckf/odomimu /rgbd_image
Kishore-1297 commented 4 months ago

Hi @matlabbe , I am working on the same and have attached the rosbag with input topics for rtabmap node

rgbd_image was generated using rtabmap_sync node and /ov_msckf/odomimu was generated from openvins. The rtabmapslam node ends up with the graph optimization error

OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met! [FATAL] OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met! terminate called after throwing an instance of 'UException' what(): [FATAL] OptimizerG2O.cpp:1835::optimizeBA() Condition (optimizer.verifyInformationMatrices()) not met!

all the time. When the rtabmap slam hits the optimizer error, the rviz visualisation of openvins freezes for 2 sec and odomimu path drifts later in rviz visualization.

The commands used are: ros2 launch ov_msckf subscribe.launch.py config:=rs_d455

ros2 run rtabmap_sync rgbd_sync --ros-args -r /rgb/image:=/camera/d435i/color/image_raw -r /depth/image:=/camera/d435i/depth/image_rect_raw -r /rgb/camera_info:=/camera/d435i/color/camera_info -p approx_sync:=false -p queue_size:=10

ros2 run rtabmap_slam rtabmap --ros-args -r odom:=/ov_msckf/odomimu -p approx_sync:=true -p subscribe_rgbd:=true -p frame_id:="d435i_link"

ros bag with /tf /tf_static /ov_msckf/odomimu /rgbd_image ros_bag

matlabbe commented 4 months ago

The odometry is not computed in the right frame. We can see the resulting point clouds have an optical rotation in them (the floor is vertical instead of being horizontal): Peek 2024-02-24 12-48

I guess global, imu, cam0 and cam1 frames are published by ov_msckf node? Is ov_msckf ignoring the frame set in image topic or d435i_link?

I'll suggest to debug the cloud orientation with rviz first by launching only ov_msckf and visualize the live camera point cloud. Debug ov_msckf till the point cloud is correctly oriented, then retry rtabmap.

Kishore-1297 commented 4 months ago

Hi @matlabbe Thank you for your reply. I have checked the point cloud being generated by d435i and observed that it is being generated in d435i_depth_optical_frame and also a static tf is always being published between frames d435i_link and d435i_depth_optical_frame. When point cloud is visualised without the ov_msckf running in the background the live point cloud looks correct wrt all the axes and d435i_link being the reference frame

When ov_msckf is executed, global, imu, cam0, and cam1 frames are generated. And the tree is incomplete as there is no tf between these frames and d435i... frames. To avoid the warnings in tf I have run a static tf publisher ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 "imu" "d435i_link". The pointcloud is still being generated in d435i_depth_optical_frame as in the previous case but when visualised in rviz the point cloud is optically rotated in global frame due to the tf that is already in place between imu and global. This same optically rotated point cloud is being reflected in rtabmap-rviz eventhough the frame_id for rtab was set to d435i_link.

I could get the live point cloud visualisation correctly by defining a static identity tf imu and d435i_depth_optical_frame instead of d435i_link, but I know that this isn't a correct way of doing it as it would lead to tf errors between global and d435i_color_optical_frame while running RTAB MAP .And /ov_msckf/odomimu is being generated in global frame.

I don't have much of an experience working on tf and would greatly appreciate your inputs regd it.

matlabbe commented 4 months ago

I could get the live point cloud visualisation correctly by defining a static identity tf imu and d435i_depth_optical_frame instead of d435i_link

You can then set imu as the base frame frame_id in rtabmap, that could work.