Robot_localization: fuse absolute with relative measurements

Question

I want to fuse global pose from tag pose estimation with visual odometry and IMU.

I am using:

• a RealSense D435i with realsense-ros that provides all the transformations between each sensor
• and rtab_map for the visual odometry

Fusing relative measurements (visual odometry and IMU) looks correct with rviz:

ekf_local.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: odom

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true]
odom0_differential: false
odom0_relative: true

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

I am not able to fuse absolute pose with visual odometry and IMU. I want to use something like this:

ekf_global.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: map

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true, true, true]

odom0_differential: false
odom0_relative: true

odom1: /tag_pose

odom1_config: [true,  true,  true,
               true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_relative: false

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

odom0 is the output of rtab_map:

frame_id: "odom"
child_frame_id: "camera_link"

imu0 is:

frame_id: "camera_imu_optical_frame"

(Realsense ROS publishes all the transformations between the camera_link and for instance camera_imu_optical_frame.)

What should be the transformation for odom1?

Is it the transformation between the map (frame_id) and the camera_link (so the raw transformation between the tag and the camera)? Is it the transformation between the map (frame_id) and the odom? Since I have another ekf_node that fuses the continuous data (vo and IMU), I can transform the raw tag pose.

The output of the local ekf is clear. It is the transformation between the odom and the camera_link. The odom frame is the initial position of the sensor when launching the nodes.

What is the output of the global ekf (/ekf_global/odometry/filtered)? From the doc it should be the transformation between the map and odom. But from my understanding:

• map frame is fixed and corresponds in my case to the tag frame
• odom frame is the initial pose of the sensor and the transformation map / odom should be fixed
• camera_link is the free floating camera frame

So does the ekf (/ekf_global/odometry/filtered) estimates the map --> odom transformation and I have to use the /ekf_local/odometry/filtered to get the transformation between the tag/map and the camera_link?

---

Edit:

I did some tests, recapped here.

The homogeneous transformation a_T_b means: X_a = a_T_b . X_b.

The vision library detects the tag and estimates the transformation camlink_T_tag. The goal was to still have the transformation camlink_T_tag when the tag is not detected (occlusion, ...).

Option 1:

• map and tag frames are the same
• ekf_local/odometry/filtered estimates odom_T_camlink
• ekf_global/odometry/filtered estimates map_T_camlink
• when the tag is detected, the transformation map_T_camlink is published
• when the tag is not detected, you have to publish the transformation map_T_camlink using the fixed transformation map_T_odom
• ekf_global/odometry/filtered is used to get the filtered tag pose
• result

Option 2:

• map and odom frames are the same, you have to publish a static identity transform map_T_odom
• ekf_local/odometry/filtered estimates odom_T_camlink
• ekf_global/odometry/filtered estimates map_T_camlink
• so both nodes (kind of) estimate the same transformation
• when the tag is detected, the transformation map_T_camlink = map_T_tag . tag_T_camlink is published with map_T_tag being a static transform computed at the first tag detection
• there is nothing to publish when the tag is not detected since map and odom are the same
• but to have the transformation camlink_T_tag, you need the static transform tag_T_map to be published
• result

Option 3:

• filter only relative measurements
• ekf_local/odometry/filtered estimates odom_T_camlink
• when the tag is detected, you use the raw pose from the vision library
• when the tag is not detected, you have camlink_T_tag = camlink_T_odom . odom_T_tag with odom_T_tag being a static transformation that must be computed when the tag is detected and with the output of ekf_local/odometry/filtered
• result

I had not the time to test the three approaches on the same sequence, so this is just to have an idea.

---

Originally posted by cv_ros_user on ROS Answers with karma: 17 on 2019-08-08

Post score: 0

Answer 1

OK, there's a lot to unpack here.

First, please always include a full sample message from every sensor input.

Second, both the map and odom frame are world-fixed frames. If you were to feed the same inputs to both EKF instances, you'd have the exact same output from both filters.

The odom-frame EKF is generating the odom->camera_link transform in your case. The map frame EKF is technically producing the map->camera_link transform. So it's producing the pose of the camera in the map frame, and the odom EKF is giving you the pose of the camera in the odom frame.

The problem is that tf doesn't permit re-parenting of frames, so the map frame EKF has to take its (internal) map->camera_link transform, and multiply it by the inverse of the odom->camera_link transform, which generates the map->odom transform. But all of that should be transparent: all you care about is that you can obtain the pose of the camera in the map and odom frames, and you can carry out transforms along that chain.

As for your tag poses, the only packages I've used all give the pose of the tag in the camera frame. If you want to fuse tag poses, you need some sort of data store that contains the map-frame poses of all the AR tags and serves them up as, e.g., static transforms. That, or you can have a special node that looks up the tag pose in the map frame, and uses the current detection to compute the map-frame pose of the robot, which then goes to the EKF.

How do you get the map-frame poses of the AR tags? You can try recording them during mapping, or you can manually measure their locations and add them to your data store. But there's no way the EKF can just take a relative AR tag pose and somehow work with it. It has to be grounded to something.

EDIT: also, is there any reason you are fusing every single variable for odom0? And I'd ditch both yaw and linear acceleration from the IMU, if I were you. I know you have it in differential mode, but magnetometers are awful.

---

Originally posted by Tom Moore with karma: 13689 on 2019-09-25

This answer was ACCEPTED on the original site

Post score: 3

---

Original comments

Comment by cv_ros_user on 2019-09-25:
Thanks for the answer. I have edited my questions to add the tests I did.

Yes in my case I assume the tag does not move.

For odom0, I don't recall why I did this (I changed the values until having something that kind of work...). I think it should be at least:

odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               false,  false,  false]

since I use rtab_map in rgbd_odometry mode.

Noted for the IMU. The RealSense uses the Bosch BMI055 that should have an accelerometer and a gyroscope.

Is it better to use the raw odom0 and imu0 for ekf_global or to use directly the output of ekf_local/odometry/filtered? I have seen some contradictory info.

Comment by cv_ros_user on 2019-09-27:
Yes, I think I have understood what you said about the IMU. I use the ROS Madgwick filter btw. Not sure what absolute orientation could be reliably estimated with the accelerometer? Most likely the configuration for the IMU should be the angular velocity and the linear acceleration.