rtabmap with external odometry (Realsense T265)

[dschnabel]

OS: Ubuntu 16.04.6 LTS
ROS version: Lunar
RTAB-Map: 0.17.6
Cameras: Realsense D435 and T265

I'm starting both cameras with this launch command:
roslaunch realsense2_camera rs_d400_and_t265.launch align_depth:=true
(see rs_d400_and_t265.launch)

The map frame looks like this.

When I start rtabmap without external odometry and have visual odometry (uses the image/depth data provided by the D435), it seems to be working fine:
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/d400/color/camera_info approx_sync:=false frame_id:=t265_odom_frame

This starts rtabmap and shows the 3D image as expected.

But when I include external odometry from the T265 then I get an empty rtabmap window where nothing is displayed:
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/d400/color/camera_info approx_sync:=false frame_id:=t265_odom_frame visual_odometry:=false odom_topic:=/t265/odom/sample

There's also a warning message on stdout which only happens if I include external odometry:

[ WARN] [1581412938.063114202]: /rtabmap/rtabmapviz: 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. 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/rtabmapviz subscribed to (exact sync):
/t265/odom/sample,
/d400/color/image_raw,
/d400/aligned_depth_to_color/image_raw,
/d400/color/camera_info,
/rtabmap/odom_info
[ WARN] [1581412938.420139502]: /rtabmap/rtabmap: 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. 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/rtabmap subscribed to (exact sync):
/t265/odom/sample,
/d400/color/image_raw,
/d400/aligned_depth_to_color/image_raw,
/d400/color/camera_info,
/rtabmap/odom_info

Odometry data is coming through fine as the command rostopic echo /t265/odom/sample shows:

header:
seq: 2186
stamp:
secs: 1581324175
nsecs: 245380878
frame_id: "t265_odom_frame"
child_frame_id: "t265_pose_frame"
pose:
pose:
position:
x: -0.000204464944545
y: -7.67098099459e-05
z: -0.000421570963226
orientation:
x: -0.00432776007801
y: 0.0274204164743
z: 0.000154268200276
w: 0.999614655972
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
twist:
linear:
x: 0.00355814174321
y: -0.00205399848924
z: 0.000575806943033
angular:
x: 0.00179574472786
y: 0.00634724180174
z: 0.00126657017099
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]

Any ideas what else I can check? Thanks!

[RealSenseCustomerSupport]

---

Hi @dschnabel

Can you help me understand the difference between this thread and the following one: #1075

Here are a few other threads to review that may help:
#711
#749
#873

[dschnabel]

Yes, sorry for the confusion, @RealSenseCustomerSupport . I had opened #1075 because I was under the wrong impression that rviz and rtabmapviz are separate programs. But turns out that this is just the frontend. In rviz I was able to configure a different 'Fixed Frame' and 'Reference Frame' which then showed me some data in the 3D window. But I was still seeing the Did not receive data since 5 seconds error message even with rviz so I guess the error was still there.

Playing with the config options I got a bit further now. I had to remove the approx_sync:=false. This seems to allow to get me some mapping data but I'm running into other issues, such as

Rejected loop closure 1 -> 136: Not enough inliers

or

The working memory is empty and the memory is not incremental (Mem/IncrementalMemory=False), no loop closure can be detected!

Obviously I'm still doing something wrong. I'm still very new to SLAM, ROS and the Realsense Cameras. All I want to do at this point is to create a map in rtabmap using the D345 and T265. Is there a good tutorial that covers this? I looked through the threads you posted but don't see something like that.

Thank you.

[RealSenseCustomerSupport]

---

Hi @dschnabel

I have looked and those errors don't seem to be on the realsense side of things. It seems to be on the rtabmap side of things.

Found a couple other threads that should be looked at:

introlab/rtabmap#418
introlab/rtabmap#409

I know this isn't rtabmap but there is the example of occupancy mapping here: https://github.com/IntelRealSense/realsense-ros/tree/occupancy-mapping/occupancy

[dschnabel]

Hi @RealSenseCustomerSupport

thank you for those threads. The occupancy mapping was a great starting point. Once I got that working, I went back to playing around with rs_rtabmap.launch. I then disabled re-localization of the T265:

    <rosparam>
            /t265/tracking_module/enable_relocalization: false
    </rosparam>

Next I set subscribe_odom_info to false as per this thread in rtabmap.launch at several places: here and here. This change requires restarting the roscore.

When I launch rtabmap via roslaunch realsense2_camera rs_rtabmap.launch I have to wait long enough for both cameras (D435 and T265) to start up. Once both cameras have started, I can finally use rtabmap to build a map. This is working great for me now.

Appreciate your help with this. Closing as solved.