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

asked 2023-01-06 03:25:21 -0500

mathislm gravatar image


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:
  seq: 1
  stamp: 1672996698.261539459
  frame_id: odom
      x: -4.2075920646311715245e-05
      y: -0.00011147373152198269963
      z: -0.0004081070655956864357
      x: -1.1501989303764181268e-05
      y: -9.490221587097517905e-06
      z: 5.1159746290189072942e-05
      w: 0.99999999858016019871
    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 ...
edit retag flag offensive close merge delete