Direction of Map frame using rgbd with IMU SLAM

[UditSinghParihar]

Hello @matlabbe , Thanks for your excellent work. I have a question regarding the direction of the x-axis for the map frame.

Setup

Currently, I am using Realsense D435i with an imu sensor inbuilt, and using the following commands for localization mode:

roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
     enable_accel:=true

rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu


roslaunch rtabmap_ros rtabmap.launch localization:=true depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false wait_imu_to_init:=true imu_topic:=/rtabmap/imu 

And have a node to subscribe to transform between \map and \camera_link frame, which would be used for a tracking a global route by our planning and control module:

class RtabmapTf:
	def __init__(self):
		self._tf_listener = tf.TransformListener()

	def run(self):
		rate = rospy.Rate(20) # 20hz
		while not rospy.is_shutdown():
			try:
				(trans, rot) = self._tf_listener.lookupTransform("/map", "/camera_link", rospy.Time(0))	
			except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
				pass
			rate.sleep()

RTABMAP, RTABMAP_ROS, GTSAM are build from source whereas realsense apt installation is being done. We are using Ubuntu 20.04, ROS Noetic.

Question

For two independent runs of these commands, I am observing that:

1. For the first run, the map frame's direction of x is forward (or along the camera facing the direction of realsense).
2. For the second run, the map frame's direction of x is leftward (or left of the camera facing the direction of realsense).

I believe that the map frame's direction of x should always be forward as observed in the first case, then why sometimes in other runs, I am getting the map frame's direction of x as leftward? Is there some way to get the map frame's direction of x forward always (or along the camera facing the direction of realsense)?

[UditSinghParihar]

Hi @matlabbe , In this video you can see that the map frame has x-axis pointing leftwards from the camera face (or the direction of motion). And this keeps on changing in different RTABMAP run in mapping mode using realsense with IMU. Though this change of orientation of the map frame doesn't happen when we run using RGBD mode without IMU. Can you explain the reason for this behavior and how to consistently get the same direction of the map frame with the x-axis forward, when fusing RGBD and imu data?

[matlabbe]

VO would initialize with original IMU orientation and TF between imu and camera topics. Just tried on my side and indeed there is a 90 deg yaw. It seems coming from tf between camera and imu frame:

rosrun tf tf_echo camera_link camera_imu_optical_frame
At time 0.000
- Translation: [-0.012, -0.006, 0.005]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]
            in RPY (radian) [-1.571, -0.000, -1.571]
            in RPY (degree) [-90.000, -0.000, -90.000]

[bittmanndo82132]

Hello Guys,

having the exact same problem, using D435i. The camera_link is 90 degree shifted compared to map. Did you find a way to solve this problem, so map/odom and camera_link face in the same direction in the beginning ?

[matlabbe]

The issue is not on rtabmap side, but the imu filter that is initialized in the imu frame, which has 90 deg yaw on it. In rtabmap standalone, we added an option to avoid this by converting the raw imu data into base frame before initializing the imu filter.

In ROS, either make a node to convert the imu sent by realsense2 camera driver into base camera frame and resend it to madgwick filter, or change directly the publisher in realsense2 camera driver here to publish the imu in base camera frame instead of its own imu frame. Doing it manually, you may just switch the right x,y,z values of the acceleration and angular parameters so that they follow base frame convention.