Sensor fusion with D435 (depth camera) and IMU

[BenJeau]

Hi, I have an Intel Realsense D435 and an android phone which is publishing some IMU data (sensor_msgs/IMU) and GPS data (sensor_msgs/NavSatFix). I was able to get the camera working in rtabmap using the following launch file:

<launch>
   <include file="$(find rtabmap_ros)/launch/rtabmap.launch" >
        <arg name="rtabmap_args" value="--delete_db_on_start" />
        <arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw" />
        <arg name="rgb_topic" value="/camera/color/image_raw" />
        <arg name="camera_info_topic" value="/camera/color/camera_info" />
        <arg name="approx_sync" value="false" />
   </include>
</launch>

Although, I am unsure how to do the sensor fusion with the IMU. I've tried adding the following with no success (I've also tried the sensor_fusion.launch with no success, but there were only warnings in the logs, but rtabmap wasn't showing up, whereas with the rtabmap.launch and the modifications below, it was opening rtabmap, but nothing was shown0:

Note: I have no idea what I'm doing and not sure how TF fully work, if that's a part of the problem, please let me know :)

...
        <arg name="imu_topic" value="/android/imu" />
        <arg name="wait_imu_to_init" value="true" />
   </include>
   <node pkg="tf" type="static_transform_publisher" name="android_camera_link"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /android /camera_link 100">

Which results in errors like (where the last line is repeated multiple times):

[ WARN] [1633312096.807109225]: odometry: waiting imu (/android/imu) to initialize orientation (wait_imu_to_init=true)
[ INFO] [1633312096.825247283]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1633312096.825351319]: rtabmap: Deleted database "/home/ben/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1633312096.825376641]: rtabmap: Using database from "/home/ben/.ros/rtabmap.db" (0 MB).
[ WARN] (2021-10-03 21:48:16.827) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ WARN] (2021-10-03 21:48:16.828) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ WARN] (2021-10-03 21:48:16.829) Memory.cpp:738::parseParameters() Mem/UseOdomFeatures is enabled, but Vis/FeatureType and Kp/DetectorStrategy parameters are not the same! Disabling Mem/UseOdomFeatures...
[ WARN] (2021-10-03 21:48:16.830) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ WARN] (2021-10-03 21:48:16.831) Memory.cpp:738::parseParameters() Mem/UseOdomFeatures is enabled, but Vis/FeatureType and Kp/DetectorStrategy parameters are not the same! Disabling Mem/UseOdomFeatures...
[ WARN] [1633312097.042840934]: odometry: Could not get transform from camera_link to android (stamp=1633312096.248000) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Lookup would require extrapolation into the past.  Requested time 1633312096.248000000 but the earliest data is at time 1633312096.724293517, when looking up transform from frame [android] to frame [camera_link]. canTransform returned after 0.201797 timeout was 0.2."
[ERROR] [1633312097.042870111]: Could not transform IMU msg from frame "android" to frame "camera_link", TF not available at time 1633312096.248000
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ WARN] [1633312097.244874721]: odometry: Could not get transform from camera_link to android (stamp=1633312096.395000) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Lookup would require extrapolation into the past.  Requested time 1633312096.395000000 but the earliest data is at time 1633312096.724293517, when looking up transform from frame [android] to frame [camera_link]. canTransform returned after 0.201965 timeout was 0.2."
[ERROR] [1633312097.244898563]: Could not transform IMU msg from frame "android" to frame "camera_link", TF not available at time 1633312096.395000
[ INFO] [1633312097.314329415]: rtabmapviz: Reading parameters from the ROS server...
[ INFO] [1633312097.314716620]: rtabmapviz: Cannot get rtabmap's parameters, waiting max 5 seconds in case the node has just been launched.
[ WARN] [1633312097.445743358]: odometry: Could not get transform from camera_link to android (stamp=1633312096.594000) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Lookup would require extrapolation into the past.  Requested time 1633312096.594000000 but the earliest data is at time 1633312096.724293517, when looking up transform from frame [android] to frame [camera_link]. canTransform returned after 0.200801 timeout was 0.2."
[ERROR] [1633312097.445768166]: Could not transform IMU msg from frame "android" to frame "camera_link", TF not available at time 1633312096.594000
[ERROR] [1633312097.475740196]: Overwriting previous data! Make sure IMU is published faster than data rate. (last image stamp buffered=1633312097.379595 and new one is 1633312097.446989, last imu stamp received=1633312096.887000)

Would someone know how to get it working? Would I use the rtabmap.launch or the sensor fusion one?

Last quick question, in this situation, the GPS is not really useful right? Or can it be integrated to map the environment better?

Hope this is fine, asking a question in Github - feel free to close if you feel like this is not the right place to ask questions.

[matlabbe]

Hi @BenJeau,

For this error:

Could not transform IMU msg from frame "android" to frame "camera_link"

Request time is 1633312096.594000000 and earliest time is 1633312096.724293517 in TF buffer (so you asking a TF too far in the past). As your TF between camera_link and android frames is published by a static_transform_publisher, maybe the stamps of IMU/camera msgs are not synchronized on the same computer clock. If there is a delay between your android device and the camera, that could be the cause of the problem. Not sure exactly how the /android/imu is published and from which computer the realsense data is published, but maybe the clocks between the device are not synchronized.

For the GPS, you will have to add this under rtabmap.launch include (some usage of GPS explained here):

<arg name="gps_topic"  value="/gps/fix" /> 

Then to actually use the GPS in graph optimimization, we should enable priors:

<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/PriorsIgnored false" />

cheers, Mathieu