[Robot Localization] How to use absolute orientation from IMU with absolut positionning from visual SLAM

Question

Hi,

I'm quite new to ROS and robot_localization so I'm getting confused really fast with TFs. I've been using for some time now a visual SLAM algorithm that works fine and I'd like to slowly improve my localization by fusing other data, hopefully I'm at the right place ! The first step I'd like to take is to fuse the IMU, especially to now have an absolute orientation w.r.t. gravity. Here is all the data I can have:

1. Pose from the visual SLAM. This is W.R.T. the starting position. Also, this is using the classic "camera frame" with Z facing forward.
2. The Linear acceleration and angular twist. This is expressed in the "camera frame".
3. The Orientation of the IMU W.R.T. the ENU frame, so this is an absolute orientation.

It would be great if I could fuse my data like this:

• Fuse the absolute position from the SLAM with the acceleration from the IMU
• Fuse the absolute orientation from the IMU with the relative orientation of the SLAM (and angular twist from the IMU, though this is kind of "duplicated data" since it's already used to compute the orientation thanks to a madgwick filter).

I managed to fuse the orientation using the "differential" parameter, but this orientation is not applied to my position ? Even though the base_link frame is beautifully tilted according to gravity, when I move the camera forward, base_link still goes up following world_frame Z axis, and not base_link Z axis. What could I have missed here ? Thanks in advance !

Following the template of other questions asked here, I'm posting my config file and a sample of each of my messages.

Here is my config file :

frequency: 10
sensor_timeout: 0.2
two_d_mode: false
publish_tf: true
world_frame: "odom"
# map_frame: "map"
odom_frame: "odom"
base_link_frame: "base_link"
print_diagnostics: true

imu0: "imu/data"
imu0_differential: false
imu0_relative: false
imu0_queue_size: 100
imu0_remove_gravitational_acceleration: true
imu0_config: [  false, false, false,
                true,  true,  true,
                false, false, false,
                true,  true,  true,
                true,  true,  true]

pose0: "odometry/visual"
pose0_differential: true
pose0_relative: false
pose0_queue_size: 10
pose0_config: [ false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, false,
                false, false, false]

pose1: "odometry/visual"
pose1_differential: false
pose1_relative: false
pose1_queue_size: 10
pose1_config: [ true,   true,   true,
                false,   false,   false,
                false,   false,   false,
                false,  false,  false,
                false,  false,  false]

Here is a sample of the visual SLAM message:

Pose message:
header: 
  seq: 1
  stamp: 1672996698.261539459
  frame_id: odom
pose: 
  pose: 
    position: 
      x: -4.2075920646311715245e-05
      y: -0.00011147373152198269963
      z: -0.0004081070655956864357
    orientation: 
      x: -1.1501989303764181268e-05
      y: -9.490221587097517905e-06
      z: 5.1159746290189072942e-05
      w: 0.99999999858016019871
  covariance[]
    covariance[0]: 1.0000000000000000623e-09
    covariance[1]: 0
    covariance[2]: 0
    covariance[3]: 0
    covariance[4]: 0
    covariance[5]: 0
    covariance[6]: 0
    covariance[7]: 1.0000000000000000623e-09
    covariance[8]: 0
    covariance[9]: 0
    covariance[10]: 0
    covariance[11]: 0
    covariance[12]: 0
    covariance[13]: 0
    covariance[14]: 1.0000000000000000623e-09
    covariance[15]: 0
    covariance[16]: 0
    covariance[17]: 0
    covariance[18]: 0
    covariance[19]: 0
    covariance[20]: 0
    covariance[21]: 0.10000000000000000555
    covariance[22]: 0
    covariance[23]: 0
    covariance[24]: 0
    covariance[25]: 0
    covariance[26]: 0
    covariance[27]: 0
    covariance[28]: 0.10000000000000000555
    covariance[29]: 0
    covariance[30]: 0
    covariance[31]: 0
    covariance[32]: 0
    covariance[33]: 0
    covariance[34]: 0
    covariance[35]: 0.10000000000000000555

Here is a sample of the IMU message : IMU message:

header: 
  seq: 0
  stamp: 1672996698.457345247
  frame_id: imu
orientation: 
  x: -0.48278
  y: 0.49618
  z: -0.50431
  w: 0.51614
orientation_covariance[]
  orientation_covariance[0]: 1e-06
  orientation_covariance[1]: 0
  orientation_covariance[2]: 0
  orientation_covariance[3]: 0
  orientation_covariance[4]: 1e-06
  orientation_covariance[5]: 0
  orientation_covariance[6]: 0
  orientation_covariance[7]: 0
  orientation_covariance[8]: 1e-06
angular_velocity: 
  x: -0.0052141
  y: -2.6929e-05
  z: 8.508e-06
angular_velocity_covariance[]
  angular_velocity_covariance[0]: 0.001
  angular_velocity_covariance[1]: 0
  angular_velocity_covariance[2]: 0
  angular_velocity_covariance[3]: 0
  angular_velocity_covariance[4]: 0.001
  angular_velocity_covariance[5]: 0
  angular_velocity_covariance[6]: 0
  angular_velocity_covariance[7]: 0
  angular_velocity_covariance[8]: 0.001
linear_acceleration: 
  x: -0.24852
  y: -9.833
  z: 0.40811
linear_acceleration_covariance[]
  linear_acceleration_covariance[0]: 0.001
  linear_acceleration_covariance[1]: 0
  linear_acceleration_covariance[2]: 0
  linear_acceleration_covariance[3]: 0
  linear_acceleration_covariance[4]: 0.001
  linear_acceleration_covariance[5]: 0
  linear_acceleration_covariance[6]: 0
  linear_acceleration_covariance[7]: 0
  linear_acceleration_covariance[8]: 0.001

Also, my TF tree is super simple right now: map => (equal) odom => (set by robot_localisation) base_link => (equal) imu

---

Originally posted by mathislm on ROS Answers with karma: 13 on 2023-01-06

Post score: 1

Answer 1

Cheers for adding your config and sensor data. Very helpful.

Right off the bat, you stated yourself that

Pose from the visual SLAM. This is W.R.T. the starting position. Also, this is using the classic "camera frame" with Z facing forward.

However, you have the frame_id of that pose data set to odom, and you also told the EKF that your world_frame was odom. ROS assumes a right-handed coordinate frame with +X forward, +Y left, and +Z up. While it's common for visual odometry sources to produce data in the camera frame, you need to account for that by (a) changing the frame_id in the visual odometry to be something like camera (you can name it whatever you want) and then (b) publishing a static transform (using, e.g., a static_transform_publisher) from odom to camera that captures the rotation from the coordinate frame I described to your camera frame.

Secondly, you said you want to fuse the absolute orientation from your IMU with the position from your visual SLAM, but that's not going to work. Imagine that you start your robot facing 90 degrees in the ENU frame, so your initial pose in the world frame is (0, 0) with a heading of 90 degrees. Then you push the robot forward one meter. The visual odometry says your (x, y) position is now (1, 0), but your IMU will say that your heading is still 90. So it will look like your robot moved laterally in the EKF output.

If having a heading that is accurate w.r.t. the ENU frame is critical, then you need to change your visual slam input. You should either make it so that the visual slam node publishes velocity data and fuse that, or you should turn on differential mode for the visual slam data (all of it). In other words, fix the frame_id on your visual odometry, then do this:

imu0: "imu/data"
imu0_differential: false
imu0_relative: false
imu0_queue_size: 100
imu0_remove_gravitational_acceleration: true
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

pose0: "odometry/visual"
pose0_differential: true
pose0_relative: false
pose0_queue_size: 10
pose0_config: [true,  true,  true,
               true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false]

---

Originally posted by Tom Moore with karma: 13689 on 2023-05-01

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by mathislm on 2023-05-02:
The second option is very similar to what I did: I'm adding to the slam's input an initial orientation that is the orientation of the IMU read at the start of the visual slam.