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]